src/newfit.cpp

   1 #include "newfit.h"
   2
   3 // вычисление модуля расстояния между двумя точками, с учётом геометрического преобразования
   4 double F (const DVec &ai, const DVec &biR, const DVec &angl) {
   5     const double cosA {cos(angl[0])}, cosB {cos(angl[1])}, cosC {cos(angl[2])}, sinA {sin(angl[0])}, sinB {sin(angl[1])}, sinC {sin(angl[2])};
   6     return  sqrt(   pow(ai[0] + biR[1] * (cosA * sinC - cosC * sinA * sinB) - biR[2] * (sinA * sinC + cosA * cosC * sinB) - cosB * cosC * biR[0], 2) +
   7                     pow(ai[1] - biR[1] * (cosA * cosC + sinA * sinB * sinC) + biR[2] * (cosC * sinA - cosA * sinB * sinC) - cosB * sinC * biR[0], 2) +
   8                     pow(ai[2] + sinB * biR[0] - cosA * cosB * biR[2] - cosB * sinA * biR[1], 2)  );
   9 }
  10
  11 template< typename T1, typename T2 >
  12 void checkIfNan(T1 &a, const T2 &b) {
  13     if (!std::isnan(b)) {
  14         a += b;
  15     } else {
  16         a += 0.;
  17     }
  18 }
  19
  20 // вычисление функции F и её частных производных
  21 void searchF0xyzabc (long double &F, double &Fx, double &Fy, double &Fz, double &Fa, double &Fb, double &Fc, const RVec &ai, const RVec &biR, const DVec &angl) {
  22     const double cosA {cos(angl[0])}, cosB {cos(angl[1])}, cosC {cos(angl[2])}, sinA {sin(angl[0])}, sinB {sin(angl[1])}, sinC {sin(angl[2])};
  23     double t01 {pow(ai[0] + biR[1] * (cosA * sinC - cosC * sinA * sinB) - biR[2] * (sinA * sinC + cosA * cosC * sinB) - cosB * cosC * biR[0], 2)};
  24     double t02 {pow(ai[1] - biR[1] * (cosA * cosC + sinA * sinB * sinC) + biR[2] * (cosC * sinA - cosA * sinB * sinC) - cosB * sinC * biR[0], 2)};
  25     double t03 {pow(ai[2] + sinB * biR[0] - cosA * cosB * biR[2] - cosB * sinA * biR[1], 2)};
  26     double t04 {sqrt(t01 + t02 + t03)};
  27     double t05 {(ai[0] + biR[1] * (cosA * sinC - cosC * sinA * sinB) - biR[2] * (sinA * sinC + cosA * cosC * sinB) - cosB * cosC * biR[0])};
  28     double t06 {(ai[2] + sinB * biR[0] - cosA * cosB * biR[2] - cosB * sinA * biR[1])};
  29     double t07 {(ai[1] - biR[1] * (cosA * cosC + sinA * sinB * sinC) + biR[2] * (cosC * sinA - cosA * sinB * sinC) - cosB * sinC * biR[0])};
  30
  31     checkIfNan(F, t04);
  32     checkIfNan(Fx, -(2 * cosB * cosC * t05 - 2 * sinB * t06 + 2 * cosB * sinC * t07) / (2 * t04));
  33     checkIfNan(Fy, -(2 * (cosA * cosC + sinA * sinB * sinC) * t07 - 2 * (cosA * sinC - cosC * sinA * sinB) * t05 + 2 * cosB * sinA * t06) / (2 * t04));
  34     checkIfNan(Fz, -(2 * (sinA * sinC + cosA * cosC * sinB) * t05 - 2 * (cosC * sinA - cosA * sinB * sinC) * t07 + 2 * cosA * cosB * t06) / (2 * t04));
  35     checkIfNan(Fa, -(2 * (cosA * cosB * biR[1] - cosB * sinA * biR[2]) * t06 -
  36             2 * (biR[1] * (cosC * sinA - cosA * sinB * sinC) + biR[2] * (cosA * cosC + sinA * sinB * sinC)) * t07 +
  37             2 * (biR[1] * (sinA * sinC + cosA * cosC * sinB) + biR[2] * (cosA * sinC - cosC * sinA * sinB)) * t05) / (2 * t04));
  38     checkIfNan(Fb, -(2 * (cosA * cosB * sinC * biR[2] - sinB * sinC * biR[0] + cosB * sinA * sinC * biR[1]) * t07 +
  39             2 * (cosA * cosB * cosC * biR[2] - cosC * sinB * biR[0] + cosB * cosC * sinA * biR[1]) * t05 -
  40             2 * (cosB * biR[0] + sinA * sinB * biR[1] + cosA * sinB * biR[2]) * t06) / (2 * t04));
  41     checkIfNan(Fc, (2 * (biR[1] * (cosA * cosC + sinA * sinB * sinC) - biR[2] * (cosC * sinA - cosA * sinB * sinC) + cosB * sinC * biR[0]) * t05 -
  42             2 * (biR[2] * (sinA * sinC + cosA * cosC * sinB) - biR[1] * (cosA * sinC - cosC * sinA * sinB) + cosB * cosC * biR[0]) * t07) / (2 * t04));
  43 }
  44
  45 // применения геометрического преобразования: смещение на вектор (x, y, z) и повороты на градусы A, B, C относительно осей (x, y, z)
  46 //
  47 // А порядок поворотов какой?
  48 void ApplyFit (std::vector< RVec > &b, const DVec &R, const DVec &angl) {
  49     DVec t(0., 0., 0.);
  50     const double cosA {cos(angl[0])}, cosB {cos(angl[1])}, cosC {cos(angl[2])}, sinA {sin(angl[0])}, sinB {sin(angl[1])}, sinC {sin(angl[2])};
  51     for (auto &i : b) {
  52         t = i.toDVec();
  53         i[0] = static_cast< float >((t[2] + R[2]) * (sinA * sinC + cosA * cosC * sinB) - (t[1] + R[1]) * (cosA * sinC - cosC * sinA * sinB) + cosB * cosC * (t[0] + R[0]));
  54         i[1] = static_cast< float >((t[1] + R[1]) * (cosA * cosC + sinA * sinB * sinC) - (t[2] + R[2]) * (cosC * sinA - cosA * sinB * sinC) + cosB * sinC * (t[0] + R[0]));
  55         i[2] = static_cast< float >(cosA * cosB * (t[2] + R[2]) - sinB * (t[0] + R[0]) + cosB * sinA * (t[1] + R[1]));
  56     }
  57 }
  58
  59 // подсчёт геометрических центров множеств a и b на основе пар pairs
  60 void CalcMid (const std::vector< RVec > &a, const std::vector< RVec > &b, DVec &midA, DVec &midB, const std::vector< std::pair< size_t, size_t > > &pairs) {
  61     for (const auto &i : pairs) {
  62         midA += a[i.first].toDVec();
  63         midB += b[i.second].toDVec();
  64     }
  65     midA[0] /= pairs.size();
  66     midA[1] /= pairs.size();
  67     midA[2] /= pairs.size();
  68
  69     midB[0] /= pairs.size();
  70     midB[1] /= pairs.size();
  71     midB[2] /= pairs.size();
  72 }
  73
  74 // функция фитирования фрейма b на фрейм a на основе пар pairs с "точностью" FtCnst
  75 void MyFitNew (const std::vector< RVec > &a, std::vector< RVec > &b, const std::vector< std::pair< size_t, size_t > > &pairs, long double FtCnst) {
  76     double fx {0.}, fy {0.}, fz {0.}, fa {0.}, fb {0.}, fc {0.};
  77     long double f1 {0.}, f2 {0.}, l {1};
  78     DVec tempA(0., 0., 0.), tempB(0., 0., 0.), tempC(0., 0., 0.), tempD(0., 0., 0.);
  79     CalcMid(a, b, tempA, tempB, pairs);
  80     tempA -= tempB;
  81     // поиск стартового отклонения множеств
  82     for (const auto &i : pairs) {
  83         f1 += F(a[i.first].toDVec(), b[i.second].toDVec() + tempA, tempC);
  84     }
  85     if (FtCnst == 0) {
  86         FtCnst = 0.000'001; // experimental
  87     }
  88     if (f1 == 0) {
  89         ApplyFit(b, tempA, tempC);
  90         return;
  91     } else {
  92         // поиск оптимального геометрического преобразования методом градиентного спуска
  93         while (f1 < f2  || f1 - f2 > FtCnst) {
  94             f1 = 0.; f2 = 0.; fx = 0.; fy = 0.; fz = 0.; fa = 0.; fb = 0.; fc = 0.; l = 1.;
  95             // поиск производных и отклонения при заданных параметрах
  96             for (const auto &i : pairs) {
  97                 searchF0xyzabc(f1, fx, fy, fz, fa, fb, fc, a[i.first], b[i.second] + tempA.toRVec(), tempC);
  98             }
  99             while (true) {
 100                 f2 = 0;
 101                 // поиск отклонения при новых параметрах
 102                 tempB[0] = tempA[0] - l * fx;
 103                 tempB[1] = tempA[1] - l * fy;
 104                 tempB[2] = tempA[2] - l * fz;
 105                 tempD[0] = tempC[0] - l * fa;
 106                 tempD[1] = tempC[1] - l * fb;
 107                 tempD[2] = tempC[2] - l * fc;
 108                 for (const auto &i : pairs) {
 109                     f2 += F(a[i.first].toDVec(), b[i.second].toDVec() + tempB, tempD);
 110                 }
 111                 if (f2 < f1) {
 112                     tempA = tempB;
 113                     tempC = tempD;
 114                     break;
 115                 } else {
 116                     // по факту существует минимальное значение переменной типа double
 117                     // в случае его достижения дважды останавливаем цикл т.к. упёрлись в предел допустимой точности
 118                     // таким образом гарантируем выход из цикла
 119                     if (l == DBL_MIN || l == 0.) { //DBL_TRUE_MIN
 120                         f2 = f1;
 121                         break;
 122                     }
 123                     l *= 0.1;
 124                 }
 125             }
 126         }
 127         ApplyFit(b, tempA, tempC);
 128     }
 129 }