• Ceres-Solver学习日志:扰动导数使用样例与广义导数及曲空间和切空间


             Ceres中的扰动导数主要通过是继承ceres::LocalParameterization来实现。当然,LocalParameterization本身具有更远大的使命或者说具有更广义的功能,扰动导数只是其特定的应用,也是其典型应用。

             谈到扰动求导自然避不开李代数的知识,但这不属于本文的范围,可参见相关理论或我之前的文章,本文假设你已具备了李代数的知识。如果实在没有,也不是不行,正如前面所说,扰动导数只是LocalParameterization的特定应用,本文虽然题为扰动导数巴拉巴拉,但大部分内容是在阐述链式求导、广义导数及如何使用LocalParameterization。

    1.链式求导

             以二维为例,以x为自变量,y为自变量,Y为两者之间的映射关系,则世间二维关系都可以建模为Y(x,y)=0。

             如果映射关系很简单,则根据多元微分理论,可得y对x的导数为dy/dx=∂Y(x,y)/∂y   /   ∂Y(x,y)/∂x。

             然而,实际中的二维关系或者说几何物理模型通常并不这么简单,而是存在多个中间关系,或者说通过多个中间关系可以更好地表达实际意义。

             于是,实际的二维关系可能是这样的:A(x,a)=0, B(a, b)=0, C(b, c)=0, Y(c, y)=0。式中,小写字母是中间变量,大写字母是映射关系。根据链式链式求导方法,可得dy/dx=dy/dc*dc/db*db/da*da/dx。这里是四个链式结点。

             Ceres提供了一种实现,可以将所有链式结点分成前后两部分实现,前一部分由残差类(对自动求导而言)或CostFunction::Evaluate(对手动求导而言)实现,后一分部分由LocalParameterization::ComuputeJacobian实现,前一部分导数命名为GlobalJacobian,后一部分命名为Jacobian,整个导数命名为LocalJacobian,即LocalJacobian=GlobaJacobian*Jacobian。

             当然,你也可以在前一部分实现整个导数,则Jacobian为单位矩阵。或在后一部分实现全部导数,则GloabalJacobian为单位矩阵。

    2.广义导数

             以上链式环节中,默认都是常规求导,即因变量增量与自变量增量的比值的极限,我们暂且称之为加法导数,以与后面的乘法导数(即扰动导数)呼应。

             自近代物理后,加法导数以不能满足物理学求解的需求,于是出了乘法导数(即扰动导数),具体怎么回事,参见李代数相关理论。这里,主要是想说以上链式结点中,每个结点都可以是不同的求导方式,尽管当前数学主是加法导数和乘法导数,但我们可以根据实际需求自定义某种导数(这是要成为数学家的节奏啦),我这里就称之为广义导数。

             Ceres将链式求导分为两段的目的就是为了能使用两种导数,从而减小计算量。因为不同的结点使用不同的求导方式可以减小计算,如旋转矩阵对旋转向量的乘法导数的计算量远小于加法导数。

             此外,利用不同的求导方式所得的雅可比矩阵来计算函数的下降方向的增量(相关理论参见梯度下降法、牛顿法、高斯牛顿法及LM法),此增量与初值的合并方式也因求导方式而异,默认是加法。Ceres提供了LocalParameterization::Plus来实现非加法的合并方式。所以,无论前一部分是什么求导方式,只要最终求导方式(即因变量对自变量的导数不是加法导数)不是加法导数,则一定要重载LocalParameterization::Plus。

    3.曲切空间

             有没有发现,上面所举例的中间关系,也是二维关系,而实际中通常是更高维的关系或更低维的关系。如自变量是10,因变量是5维,中间变量可能有3维、7维、30维、700维,这都取决于实际物理几何模型。如针孔成像模型中,世界坐标作为自变量是3维,图像坐标作为因变量是2维的,中间变量归一化物理坐标是2维的,中间变量相机坐标是3维的。或者将世界坐标当作已知量,将旋转当作未知量,则旋转向量作为自变量是3维,中间变量旋转矩阵是9维的,中间变量相机坐标是3维的,中间变量归一化物理坐标是2维的。

             可见,自变量和因变量的维度与中间变量的维度没有太大关系,关键还是取决于如何建模型。在不影响几何物理意义的情况下,当然是越少越好,这可以极大的减少计算量,也符合大道至简的规则。

            于是,根据变量的维度,我们相对地可以定义曲空间和切空间。在表示能表示同一物理几何意义的情况下,将最低维度的空间或更低维度的空间称为切空间,将最高维度的空间或更高维度的空间称为曲空间。

            典型地,对旋转向量和旋转四元数,旋转向量位于切空间,旋转四元数位于曲空间;对旋转向量和旋转矩阵,旋转向量位于切空间,旋转矩阵位于曲空间;对旋转四元数和旋转矩阵,旋转四元数位于切空间,旋转矩阵位于曲空间。

    4.使用样例

             提供三个使用样例,封装为两个类:

             OptimizeRt:基于单次观察,优化此次观察的外参,定义了ProjectionModelRt、ProjectionCostRt、LocalParamRWithGeneralJ、LocalParamRWithIdentJ四个类。

             OptimizeKDRt:基于多次观察,优化相机内参和所有观察的外参,定义了ProjectionModelKDRt和ProjectionCostKDRt两个类并使用了OptimizeRt:: LocalParamRWithGeneralJ和OptimizeRt::LocalParamRWithIdentJ两个类。

        OptimzedRtWithNormazelized: 正如其名,基归一化坐标来优化外参,比较这两个类可发现它们的联系和区别,但归一化坐标是用cv::projectPoints生成而没用cv::undistortPoints(用之不能收敛),以排除数据本身仿真且cv::undistortPoints非闭环实现的干扰。

             其中类MotionSim用于生成仿真数据,使用说明参见《CV学习日志:CV开发之三维仿真器》。

             存在可能不收敛的测试结果,属于正常现象,可修改初值精度来增加收敛性。

             以下是详细代码,依赖于C++14、OpenCV4.x、Ceres和Spdlog。

       1 #include <opencv2/opencv.hpp>
       2 #include <opencv2/viz.hpp>
       3 #include <spdlog/spdlog.h>
       4 #include <ceres/ceres.h>
       5 using namespace std;
       6 using namespace cv;
       7 
       8 class MotionSim
       9 {
      10 public:
      11     static void TestMe(int argc, char** argv)
      12     {
      13         MotionSim motionSim(false);
      14         motionSim.camFovX = 45;
      15         motionSim.camFovY = 30;
      16         motionSim.camRand = 10;
      17         motionSim.enableVerbose = false;
      18         motionSim.runMotion(false, false, 7);
      19         motionSim.visMotion();
      20     }
      21 
      22 public:
      23     struct MotionView
      24     {
      25         Mat_<double> r = Mat_<double>(3, 1);
      26         Mat_<double> t = Mat_<double>(3, 1);
      27         Mat_<double> q = Mat_<double>(4, 1);
      28         Mat_<double> rt = Mat_<double>(6, 1);
      29         Mat_<double> radian = Mat_<double>(3, 1);
      30         Mat_<double> degree = Mat_<double>(3, 1);
      31         Mat_<double> R = Mat_<double>(3, 3);
      32         Mat_<double> T = Mat_<double>(3, 4);
      33         Mat_<double> K;
      34         Mat_<double> D;
      35         Mat_<Vec3d> point3D;
      36         Mat_<Vec2d> point2D;
      37         Mat_<int> point3DIds;
      38         string print(string savePath = "")
      39         {
      40             string str;
      41             str += fmt::format("r: {}
    ", cvarr2str(r.t()));
      42             str += fmt::format("t: {}
    ", cvarr2str(t.t()));
      43             str += fmt::format("q: {}
    ", cvarr2str(q.t()));
      44             str += fmt::format("rt: {}
    ", cvarr2str(rt.t()));
      45             str += fmt::format("radian: {}
    ", cvarr2str(radian.t()));
      46             str += fmt::format("degree: {}
    ", cvarr2str(degree.t()));
      47             str += fmt::format("R: {}
    ", cvarr2str(R));
      48             str += fmt::format("T: {}
    ", cvarr2str(T));
      49             str += fmt::format("K: {}
    ", cvarr2str(K));
      50             str += fmt::format("D: {}
    ", cvarr2str(D.t()));
      51             if (savePath.empty() == false) { FILE* out = fopen(savePath.c_str(), "w"); fprintf(out, str.c_str()); fclose(out); }
      52             return str;
      53         }
      54     };
      55     static string cvarr2str(InputArray v)
      56     {
      57         Ptr<Formatted> fmtd = cv::format(v, Formatter::FMT_DEFAULT);
      58         string dst; fmtd->reset();
      59         for (const char* str = fmtd->next(); str; str = fmtd->next()) dst += string(str);
      60         return dst;
      61     }
      62     static void euler2matrix(double e[3], double R[9], bool forward = true, int argc = 0, char** argv = 0)
      63     {
      64         if (argc > 0)
      65         {
      66             int N = 999;
      67             for (int k = 0; k < N; ++k)//OpenCV not better than DIY
      68             {
      69                 //1.GenerateData
      70                 Matx31d radian0 = radian0.randu(-3.14159265358979323846, 3.14159265358979323846);
      71                 Matx33d R; euler2matrix(radian0.val, R.val, true);
      72                 const double deg2rad = 3.14159265358979323846 * 0.0055555555555555556;
      73                 const double rad2deg = 180 * 0.3183098861837906715;
      74 
      75                 //2.CalcByOpenCV
      76                 Matx31d radian1 = cv::RQDecomp3x3(R, Matx33d(), Matx33d()) * deg2rad;
      77 
      78                 //3.CalcByDIY
      79                 Matx31d radian2; euler2matrix(R.val, radian2.val, false);
      80 
      81                 //4.AnalyzeError
      82                 double infRadian0Radian1 = norm(radian0, radian1, NORM_INF);
      83                 double infRadian1Radian2 = norm(radian1, radian2, NORM_INF);
      84 
      85                 //5.PrintError
      86                 cout << endl << "LoopCount: " << k << endl;
      87                 if (infRadian0Radian1 > 0 || infRadian1Radian2 > 0)
      88                 {
      89                     cout << endl << "5.1PrintError" << endl;
      90                     cout << endl << "infRadian0Radian1: " << infRadian0Radian1 << endl;
      91                     cout << endl << "infRadian1Radian2: " << infRadian1Radian2 << endl;
      92                     if (0)
      93                     {
      94                         cout << endl << "5.2PrintDiff" << endl;
      95                         cout << endl << "radian0-degree0:" << endl << radian0.t() << endl << radian0.t() * rad2deg << endl;
      96                         cout << endl << "radian1-degree1:" << endl << radian1.t() << endl << radian1.t() * rad2deg << endl;
      97                         cout << endl << "radian2-degree2:" << endl << radian2.t() << endl << radian2.t() * rad2deg << endl;
      98                         cout << endl << "5.3PrintOthers" << endl;
      99                         cout << endl << "R:" << endl << R << endl;
     100                     }
     101                     cout << endl << "Press any key to continue" << endl; std::getchar();
     102                 }
     103             }
     104             return;
     105         }
     106         if (forward)//check with 3D Rotation Converter
     107         {
     108             double sinR = std::sin(e[0]);
     109             double sinP = std::sin(e[1]);
     110             double sinY = std::sin(e[2]);
     111             double cosR = std::cos(e[0]);
     112             double cosP = std::cos(e[1]);
     113             double cosY = std::cos(e[2]);
     114 
     115             //RPY indicates: first Yaw aroundZ, second Pitch aroundY, third Roll aroundX
     116             R[0] = cosY * cosP; R[1] = cosY * sinP * sinR - sinY * cosR; R[2] = cosY * sinP * cosR + sinY * sinR;
     117             R[3] = sinY * cosP; R[4] = sinY * sinP * sinR + cosY * cosR; R[5] = sinY * sinP * cosR - cosY * sinR;
     118             R[6] = -sinP;       R[7] = cosP * sinR;                      R[8] = cosP * cosR;
     119         }
     120         else
     121         {
     122             double vs1 = std::abs(R[6] - 1.);
     123             double vs_1 = std::abs(R[6] + 1.);
     124             if (vs1 > 1E-9 && vs_1 > 1E-9)
     125             {
     126                 e[2] = std::atan2(R[3], R[0]); //Yaw aroundZ
     127                 e[1] = std::asin(-R[6]);//Pitch aroundY
     128                 e[0] = std::atan2(R[7], R[8]); //Roll aroundX
     129             }
     130             else if (vs_1 <= 1E-9)
     131             {
     132                 e[2] = 0; //Yaw aroundZ
     133                 e[1] = 3.14159265358979323846 * 0.5;//Pitch aroundY
     134                 e[0] = e[2] + atan2(R[1], R[2]); //Roll aroundX
     135             }
     136             else
     137             {
     138                 e[2] = 0; //Yaw aroundZ
     139                 e[1] = -3.14159265358979323846 * 0.5;//Pitch aroundY
     140                 e[0] = -e[2] + atan2(-R[1], -R[2]); //Roll aroundX
     141             }
     142         }
     143     };
     144     static void quat2matrix(double q[4], double R[9], bool forward = true)
     145     {
     146         if (forward)//refer to qglviwer
     147         {
     148             double L1 = std::sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
     149             if (std::abs(L1 - 1) > 1E-9) { std::printf("Not uint quaternion: NormQ=%.9f
    ", L1); abort(); }
     150 
     151             double xx = 2.0 * q[1] * q[1];
     152             double yy = 2.0 * q[2] * q[2];
     153             double zz = 2.0 * q[3] * q[3];
     154 
     155             double xy = 2.0 * q[1] * q[2];
     156             double xz = 2.0 * q[1] * q[3];
     157             double wx = 2.0 * q[1] * q[0];
     158 
     159             double yz = 2.0 * q[2] * q[3];
     160             double wy = 2.0 * q[2] * q[0];
     161 
     162             double wz = 2.0 * q[3] * q[0];
     163 
     164             R[0] = 1.0 - yy - zz;
     165             R[4] = 1.0 - xx - zz;
     166             R[8] = 1.0 - xx - yy;
     167 
     168             R[1] = xy - wz;
     169             R[3] = xy + wz;
     170 
     171             R[2] = xz + wy;
     172             R[6] = xz - wy;
     173 
     174             R[5] = yz - wx;
     175             R[7] = yz + wx;
     176         }
     177         else
     178         {
     179             double onePlusTrace = 1.0 + R[0] + R[4] + R[8];// Compute one plus the trace of the matrix
     180             if (onePlusTrace > 1E-9)
     181             {
     182                 double s = sqrt(onePlusTrace) * 2.0;
     183                 double is = 1 / s;
     184                 q[0] = 0.25 * s;
     185                 q[1] = (R[7] - R[5]) * is;
     186                 q[2] = (R[2] - R[6]) * is;
     187                 q[3] = (R[3] - R[1]) * is;
     188             }
     189             else
     190             {
     191                 std::printf("1+trace(R)=%.9f is too small and (R11,R22,R33)=(%.9f,%.9f,%.9f)
    ", onePlusTrace, R[0], R[4], R[8]);
     192                 if ((R[0] > R[4]) && (R[0] > R[8]))//max(R00, R11, R22)=R00
     193                 {
     194                     double s = sqrt(1.0 + R[0] - R[4] - R[8]) * 2.0;
     195                     double is = 1 / s;
     196                     q[0] = (R[5] - R[7]) * is;
     197                     q[1] = 0.25 * s;
     198                     q[2] = (R[1] + R[3]) * is;
     199                     q[3] = (R[2] + R[6]) * is;
     200                 }
     201                 else if (R[4] > R[8])//max(R00, R11, R22)=R11
     202                 {
     203                     double s = sqrt(1.0 - R[0] + R[4] - R[8]) * 2.0;
     204                     double is = 1 / s;
     205                     q[0] = (R[2] - R[6]) * is;
     206                     q[1] = (R[1] + R[3]) * is;
     207                     q[2] = 0.25 * s;
     208                     q[3] = (R[5] + R[7]) * is;
     209                 }
     210                 else//max(R00, R11, R22)=R22
     211                 {
     212                     double s = sqrt(1.0 - R[0] - R[4] + R[8]) * 2.0;
     213                     double is = 1 / s;
     214                     q[0] = (R[1] - R[3]) * is;
     215                     q[1] = (R[2] + R[6]) * is;
     216                     q[2] = (R[5] + R[7]) * is;
     217                     q[3] = 0.25 * s;
     218                 }
     219             }
     220             double L1 = std::sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
     221             if (L1 < 1e-9) { std::printf("Wrong rotation matrix: NormQ=%.9f
    ", L1); abort(); }
     222             else { L1 = 1 / L1; q[0] *= L1; q[1] *= L1; q[2] *= L1; q[3] *= L1; }
     223         }
     224     }
     225     static void vec2quat(double r[3], double q[4], bool forward = true)
     226     {
     227         if (forward)//refer to qglviwer
     228         {
     229             double theta = std::sqrt(r[0] * r[0] + r[1] * r[1] + r[2] * r[2]);
     230             if (std::abs(theta) < 1E-9)
     231             {
     232                 q[0] = 1; q[1] = q[2] = q[3] = 0;
     233                 std::printf("Rotation approximates zero: Theta=%.9f
    ", theta);
     234             };
     235 
     236             q[0] = std::cos(theta * 0.5);
     237             double ss = std::sin(theta * 0.5) / theta;
     238             q[1] = r[0] * ss;
     239             q[2] = r[1] * ss;
     240             q[3] = r[2] * ss;
     241         }
     242         else
     243         {
     244             double L1 = std::sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
     245             if (std::abs(L1 - 1) > 1E-9) { std::printf("Not uint quaternion: NormQ=%.9f
    ", L1); abort(); }
     246 
     247             double theta = 2 * acos(q[0]);
     248             if (theta > 3.14159265358979323846) theta = 2 * 3.14159265358979323846 - theta;
     249             double thetaEx = theta / std::sin(theta * 0.5);
     250             r[0] = q[1] * thetaEx;
     251             r[1] = q[2] * thetaEx;
     252             r[2] = q[3] * thetaEx;
     253         }
     254     }
     255     static void vec2matrix(double r[3], double R[9], bool forward = true, int argc = 0, char** argv = 0)
     256     {
     257         if (argc > 0)
     258         {
     259             int N = 999;
     260             for (int k = 0; k < N; ++k) //refer to the subsequent article for more details
     261             {
     262                 //1.GenerateData
     263                 Matx31d r0 = r0.randu(-999, 999);
     264                 Matx33d R0; cv::Rodrigues(r0, R0);
     265 
     266                 //2.CalcByOpenCV
     267                 Matx33d R1;
     268                 Matx31d r1;
     269                 cv::Rodrigues(r0, R1);
     270                 cv::Rodrigues(R0, r1);
     271 
     272                 //3.CalcByDIY
     273                 Matx33d R2;
     274                 Matx31d r2;
     275                 vec2matrix(r0.val, R2.val, true);
     276                 vec2matrix(r2.val, R0.val, false);
     277 
     278                 //4.AnalyzeError
     279                 double infR1R2 = norm(R1, R2, NORM_INF);
     280                 double infr1r2 = norm(r1, r2, NORM_INF);
     281 
     282                 //5.PrintError
     283                 cout << endl << "LoopCount: " << k << endl;
     284                 if (infR1R2 > 1E-12 || infr1r2 > 1E-12)
     285                 {
     286                     cout << endl << "5.1PrintError" << endl;
     287                     cout << endl << "infR1R2: " << infR1R2 << endl;
     288                     cout << endl << "infr1r2: " << infr1r2 << endl;
     289                     if (0)
     290                     {
     291                         cout << endl << "5.2PrintDiff" << endl;
     292                         cout << endl << "R1: " << endl << R1 << endl;
     293                         cout << endl << "R2: " << endl << R2 << endl;
     294                         cout << endl;
     295                         cout << endl << "r1: " << endl << r1.t() << endl;
     296                         cout << endl << "r2: " << endl << r2.t() << endl;
     297                         cout << endl << "5.3PrintOthers" << endl;
     298                     }
     299                     cout << endl << "Press any key to continue" << endl; std::getchar();
     300                 }
     301             }
     302             return;
     303         }
     304 
     305         if (forward)
     306         {
     307             double theta = std::sqrt(r[0] * r[0] + r[1] * r[1] + r[2] * r[2]);
     308             if (theta < 1E-9)
     309             {
     310                 R[0] = R[4] = R[8] = 1.0;
     311                 R[1] = R[2] = R[3] = R[5] = R[6] = R[7] = 0.0;
     312                 std::printf("Rotation approximates zero: Theta=%.9f
    ", theta);
     313                 return;
     314             }
     315             double cs = cos(theta);
     316             double sn = sin(theta);
     317             double itheta = 1. / theta;
     318             double cs1 = 1 - cs;
     319             double nx = r[0] * itheta;
     320             double ny = r[1] * itheta;
     321             double nz = r[2] * itheta;
     322 
     323             double nxnx = nx * nx, nyny = ny * ny, nznz = nz * nz;
     324             double nxny = nx * ny, nxnz = nx * nz, nynz = ny * nz;
     325             double nxsn = nx * sn, nysn = ny * sn, nzsn = nz * sn;
     326 
     327             R[0] = nxnx * cs1 + cs;
     328             R[3] = nxny * cs1 + nzsn;
     329             R[6] = nxnz * cs1 - nysn;
     330 
     331             R[1] = nxny * cs1 - nzsn;
     332             R[4] = nyny * cs1 + cs;
     333             R[7] = nynz * cs1 + nxsn;
     334 
     335             R[2] = nxnz * cs1 + nysn;
     336             R[5] = nynz * cs1 - nxsn;
     337             R[8] = nznz * cs1 + cs;
     338 
     339             if (0)
     340             {
     341                 Mat_<double> dRdu({ 9, 4 }, {
     342                     2 * nx * cs1, 0, 0, (nxnx - 1) * sn,
     343                     ny * cs1, nx * cs1, -sn, nxny * sn - nz * cs,
     344                     nz * cs1, sn, nx * cs1, nxnz * sn + ny * cs,
     345                     ny * cs1, nx * cs1, sn, nxny * sn + nz * cs,
     346                     0, 2 * ny * cs1, 0, (nyny - 1) * sn,
     347                     -sn, nz * cs1, ny * cs1, nynz * sn - nx * cs,
     348                     nz * cs1, -sn, nx * cs1, nxnz * sn - ny * cs,
     349                     sn, nz * cs1, ny * cs1, nynz * sn + nx * cs,
     350                     0, 0, 2 * nz * cs1, (nznz - 1) * sn });
     351 
     352                 Mat_<double> dudv({ 4, 4 }, {
     353                     itheta, 0, 0, -nx * itheta,
     354                     0, itheta, 0, -ny * itheta,
     355                     0, 0, itheta, -nz * itheta,
     356                     0, 0, 0, 1 });
     357 
     358                 Mat_<double> dvdr({ 4, 3 }, {
     359                     1, 0, 0,
     360                     0, 1, 0,
     361                     0, 0, 1,
     362                     nx, ny, nz });
     363 
     364                 Mat_<double> Jacobian = dRdu * dudv * dvdr;//rows=9 cols=3
     365             }
     366         }
     367         else
     368         {
     369             double sx = R[7] - R[5];
     370             double sy = R[2] - R[6];
     371             double sz = R[3] - R[1];
     372             double sn = sqrt(sx * sx + sy * sy + sz * sz) * 0.5;
     373             double cs = (R[0] + R[4] + R[8] - 1) * 0.5;
     374             double theta = acos(cs);
     375             double ss = 2 * sn;
     376             double iss = 1. / ss;
     377             double tss = theta * iss;
     378             r[0] = tss * sx;
     379             r[1] = tss * sy;
     380             r[2] = tss * sz;
     381 
     382             if (0)
     383             {
     384                 Mat_<double> drdu({ 3, 4 }, {
     385                     tss, 0, 0, (sn - theta * cs) * iss * iss * sx * 2,
     386                     0, tss, 0, (sn - theta * cs) * iss * iss * sy * 2,
     387                     0, 0, tss, (sn - theta * cs) * iss * iss * sz * 2 });
     388 
     389                 Mat_<double> dudR({ 4, 9 }, {
     390                     0, 0, 0, 0, 0, -1, 0, 1, 0,
     391                     0, 0, 1, 0, 0, 0, -1, 0, 0,
     392                     0, -1, 0, 1, 0, 0, 0, 0, 0,
     393                     -iss, 0, 0, 0, -iss, 0, 0, 0, -iss });
     394 
     395                 Mat_<double> Jacobian = drdu * dudR;//rows=3 cols=9
     396             }
     397         }
     398     }
     399 
     400 private:
     401     const int nHorPoint3D = 100;
     402     const int nVerPoint3D = 100;
     403     const double varPoint3DXY = 10.;
     404     const double minPoint3DZ = 1.;
     405     const double maxPoint3DZ = 99.;
     406     const double minCamZ = 101.;
     407     const double maxCamZ = 150.;
     408     const double varCamDegree = 10.;
     409     Mat_<Vec3d> allPoint3D = Mat_<Vec3d>(nVerPoint3D * nHorPoint3D, 1);
     410     Mat_<double> allPoint3DZ = Mat_<double>(nVerPoint3D * nHorPoint3D, 1);
     411     Mat_<double> K;
     412     Mat_<double> D;
     413     const double deg2rad = 3.14159265358979323846 * 0.0055555555555555556;
     414     const double rad2deg = 180 * 0.3183098861837906715;
     415 
     416 public:
     417     int camRows = 480;
     418     int camCols = 640;
     419     int camFovY = 90;
     420     int camFovX = 90;
     421     int camRand = 10;//append random[0,camRand] to camera intrinsics
     422     int nCamDist = 5;//refer to opencv for value domain
     423     int nMinMotion = 32; // no less than X motion views
     424     int nMaxMotion = INT_MAX; // no more than X motion views
     425     int nPoint2DThenExit = 32;//exit when less than X pixies
     426     int rotMode = 1 + 2 + 4;//0=noRot 1=xAxis 2=yAxis 4=zAxis
     427     bool noTrans = false;//translate or not while motion
     428     bool world2D = false;//planar world or not
     429     bool rndSeek = true;//use random seek or not
     430     bool enableVerbose = false;//check motions one by one or not
     431     vector<MotionView> motionViews;//World Information: RightX, FrontY, DownZ
     432     MotionSim(bool run = true, bool world2D0 = false, bool noTrans0 = false, int rotMode0 = 7) { if (run) runMotion(world2D0, noTrans0, rotMode0); }
     433 
     434 public:
     435     void runMotion(bool world2D0 = false, bool noTrans0 = false, int rotMode0 = 7)
     436     {
     437         world2D = world2D0;
     438         noTrans = noTrans0;
     439         rotMode = rotMode0;
     440         motionViews.clear();
     441         if (rndSeek) cv::setRNGSeed(clock());
     442         while (motionViews.size() < nMinMotion)
     443         {
     444             //1.GetAllPoint3D
     445             if (world2D) allPoint3DZ = 0.;
     446             else cv::randu(allPoint3DZ, -maxPoint3DZ, -minPoint3DZ);//DownZ
     447             for (int i = 0, k = 0; i < nVerPoint3D; ++i)
     448                 for (int j = 0; j < nHorPoint3D; ++j, ++k)
     449                     allPoint3D(k) = Vec3d((j + cv::randu<double>()) * varPoint3DXY, (i + cv::randu<double>()) * varPoint3DXY, allPoint3DZ(i, j));
     450 
     451             //2.GetCamParams
     452             double camFx = camCols / 2. / std::tan(camFovX / 2. * deg2rad) + cv::randu<double>() * camRand;
     453             double camFy = camRows / 2. / std::tan(camFovY / 2. * deg2rad) + cv::randu<double>() * camRand;
     454             double camCx = camCols / 2. + cv::randu<double>() * camRand;
     455             double camCy = camRows / 2. + cv::randu<double>() * camRand;
     456             K.create(3, 3); K << camFx, 0, camCx, 0, camFy, camCy, 0, 0, 1;
     457             D.create(nCamDist, 1); cv::randu(D, -1.0, 1.0);
     458 
     459             //3.GetAllMotionView
     460             motionViews.clear();
     461             for (int64 k = 0; ; ++k)
     462             {
     463                 //3.1 JoinCamParams
     464                 MotionView view;
     465                 view.K = K.clone();
     466                 view.D = D.clone();
     467 
     468                 //3.2 GetCamTrans
     469                 if (k == 0) view.t(0) = view.t(1) = 0;
     470                 else
     471                 {
     472                     view.t(0) = motionViews[k - 1].t(0) + cv::randu<double>() * varPoint3DXY;
     473                     view.t(1) = motionViews[k - 1].t(1) + cv::randu<double>() * varPoint3DXY;
     474                 }
     475                 view.t(2) = minCamZ + cv::randu<double>() * (maxCamZ - minCamZ);
     476                 view.t(2) = -view.t(2);//DownZ
     477                 if (noTrans && k != 0) { view.t(0) = motionViews[0].t(0); view.t(1) = motionViews[0].t(1); view.t(2) = motionViews[0].t(2); }
     478 
     479                 //3.3 GetCamRot: degree-->radian-->matrix-->vector&quaternion
     480                 view.degree = 0.;
     481                 if (rotMode & 1) view.degree(0) = cv::randu<double>() * varCamDegree;
     482                 if (rotMode & 2) view.degree(1) = cv::randu<double>() * varCamDegree;
     483                 if (rotMode & 4) view.degree(2) = cv::randu<double>() * varCamDegree;
     484                 view.radian = view.degree * deg2rad;
     485                 euler2matrix(view.radian.ptr<double>(), view.R.ptr<double>());
     486                 cv::Rodrigues(view.R, view.r);
     487                 quat2matrix(view.q.ptr<double>(), view.R.ptr<double>(), false);
     488                 cv::hconcat(view.R, view.t, view.T);
     489                 cv::vconcat(view.r, view.t, view.rt);
     490 
     491                 //3.4 GetPoint3DAndPoint2D
     492                 Mat_<Vec2d> allPoint2D;
     493                 cv::projectPoints(allPoint3D, -view.r, -view.R.t() * view.t, view.K, view.D, allPoint2D);
     494                 for (int k = 0; k < allPoint2D.total(); ++k)
     495                     if (allPoint2D(k)[0] > 0 && allPoint2D(k)[0] < camCols && allPoint2D(k)[1] > 0 && allPoint2D(k)[1] < camRows)
     496                     {
     497                         view.point2D.push_back(allPoint2D(k));
     498                         view.point3D.push_back(allPoint3D(k));
     499                         view.point3DIds.push_back(k);
     500                     }
     501 
     502                 //3.5 PrintDetails
     503                 motionViews.push_back(view);
     504                 if (enableVerbose)
     505                 {
     506                     cout << endl << view.print();
     507                     cout << fmt::format("view={}   features={}
    ", k, view.point2D.rows);
     508                     double minV = 0, maxV = 0;//Distortion makes some minV next to maxV
     509                     int minId = 0, maxId = 0;
     510                     cv::minMaxIdx(allPoint2D.reshape(1, int(allPoint2D.total()) * allPoint2D.channels()), &minV, &maxV, &minId, &maxId);
     511                     cout << fmt::format("minInfo:({}, {})", minId, minV) << allPoint3D(minId / 2) << allPoint2D(minId / 2) << endl;
     512                     cout << fmt::format("maxInfo:({}, {})", maxId, maxV) << allPoint3D(maxId / 2) << allPoint2D(maxId / 2) << endl;
     513                     cout << "Press any key to continue" << endl; std::getchar();
     514                 }
     515                 if (view.point2D.rows < nPoint2DThenExit || motionViews.size() > nMaxMotion) break;
     516             }
     517         }
     518     }
     519     void visMotion()
     520     {
     521         //1.CreateWidgets
     522         Size2d validSize(nHorPoint3D * varPoint3DXY, nVerPoint3D * varPoint3DXY);
     523         Mat_<cv::Affine3d> camPoses(int(motionViews.size()), 1); for (int k = 0; k < camPoses.rows; ++k) camPoses(k) = cv::Affine3d(motionViews[k].T);
     524         viz::WText worldInfo(fmt::format("nMotionView: {}
    K: {}
    D: {}", motionViews.size(), cvarr2str(K), cvarr2str(D)), Point(10, 240), 10);
     525         viz::WCoordinateSystem worldCSys(1000);
     526         viz::WPlane worldGround(Point3d(validSize.width / 2, validSize.height / 2, 0), Vec3d(0, 0, 1), Vec3d(0, 1, 0), validSize);
     527         viz::WCloud worldPoints(allPoint3D, Mat_<Vec3b>(allPoint3D.size(), Vec3b(0, 255, 0)));
     528         viz::WTrajectory camTraj1(camPoses, viz::WTrajectory::FRAMES, 8);
     529         viz::WTrajectorySpheres camTraj2(camPoses, 100, 2);
     530         viz::WTrajectoryFrustums camTraj3(camPoses, Matx33d(K), 4., viz::Color::yellow());
     531         worldCSys.setRenderingProperty(viz::OPACITY, 0.1);
     532         worldGround.setRenderingProperty(viz::OPACITY, 0.1);
     533         camTraj2.setRenderingProperty(viz::OPACITY, 0.6);
     534 
     535         //2.ShowWidgets
     536         static viz::Viz3d viz3d(__FUNCTION__);
     537         viz3d.showWidget("worldInfo", worldInfo);
     538         viz3d.showWidget("worldCSys", worldCSys);
     539         viz3d.showWidget("worldGround", worldGround);
     540         viz3d.showWidget("worldPoints", worldPoints);
     541         viz3d.showWidget("camTraj1", camTraj1);
     542         viz3d.showWidget("camTraj2", camTraj2);
     543         viz3d.showWidget("camTraj3", camTraj3);
     544 
     545         //3.UpdateWidghts
     546         static const vector<MotionView>& views = motionViews;
     547         viz3d.registerKeyboardCallback([](const viz::KeyboardEvent& keyboarEvent, void* pVizBorad)->void
     548             {
     549                 if (keyboarEvent.action != viz::KeyboardEvent::KEY_DOWN) return;
     550                 static int pos = 0;
     551                 if (keyboarEvent.code == ' ')
     552                 {
     553                     size_t num = views.size();
     554                     size_t ind = pos % num;
     555                     double xmin3D = DBL_MAX, ymin3D = DBL_MAX, xmin2D = DBL_MAX, ymin2D = DBL_MAX;
     556                     double xmax3D = -DBL_MAX, ymax3D = -DBL_MAX, xmax2D = -DBL_MAX, ymax2D = -DBL_MAX;
     557                     for (size_t k = 0; k < views[ind].point3D.rows; ++k)
     558                     {
     559                         Vec3d pt3 = views[ind].point3D(int(k));
     560                         Vec2d pt2 = views[ind].point2D(int(k));
     561                         if (pt3[0] < xmin3D) xmin3D = pt3[0];
     562                         if (pt3[0] > xmax3D) xmax3D = pt3[0];
     563                         if (pt3[1] < ymin3D) ymin3D = pt3[1];
     564                         if (pt3[1] > ymax3D) ymax3D = pt3[1];
     565                         if (pt2[0] < xmin2D) xmin2D = pt2[0];
     566                         if (pt2[0] > xmax2D) xmax2D = pt2[0];
     567                         if (pt2[1] < ymin2D) ymin2D = pt2[1];
     568                         if (pt2[1] > ymax2D) ymax2D = pt2[1];
     569                     }
     570                     if (pos != 0)
     571                     {
     572                         for (int k = 0; k < views[ind == 0 ? num - 1 : ind - 1].point3D.rows; ++k) viz3d.removeWidget("active" + std::to_string(k));
     573                         viz3d.removeWidget("viewInfo");
     574                         viz3d.removeWidget("camSolid");
     575                     }
     576                     for (int k = 0; k < views[ind].point3D.rows; ++k) viz3d.showWidget("active" + std::to_string(k), viz::WSphere(views[ind].point3D(k), 5, 10));
     577                     viz3d.showWidget("viewInfo", viz::WText(fmt::format("CurrentMotion: {}
    ValidPoints: {}
    Min3DXY_Min2DXY: {}, {}, {}, {}
    Max3DXY_Max2DXY: {}, {}, {}, {}
    Rot_Trans_Euler: {}
    ",
     578                         ind, views[ind].point3D.rows, xmin3D, ymin3D, xmin2D, ymin2D, xmax3D, ymax3D, xmax2D, ymax2D,
     579                         cvarr2str(views[ind].r.t()) + cvarr2str(views[ind].t.t()) + cvarr2str(views[ind].degree.t())), Point(10, 10), 10));
     580                     viz3d.showWidget("camSolid", viz::WCameraPosition(Matx33d(views[ind].K), 10, viz::Color::yellow()), cv::Affine3d(views[ind].T));
     581                     ++pos;
     582                 }
     583             }, 0);
     584         viz3d.spin();
     585     }
     586 };
     587 
     588 class OptimizeRt
     589 {
     590 public:
     591     using MotionView = MotionSim::MotionView;
     592     static void TestMe(int argc = 0, char** argv = 0)
     593     {
     594         int N = 99;
     595         for (int k = 0; k < N; ++k)
     596         {
     597             //0.GenerateData
     598             bool world2D = k % 2;
     599             int rotMode = k % 7 + 1;
     600             MotionSim motionSim(false);
     601             motionSim.camFovX = 90;
     602             motionSim.camFovY = 90;
     603             motionSim.camRand = 10;
     604             motionSim.nMinMotion = 16;//2
     605             motionSim.nMaxMotion = 32;//4
     606             motionSim.rndSeek = false;
     607             motionSim.nCamDist = 5;
     608             motionSim.runMotion(world2D, false, rotMode);
     609             //motionSim.visMotion();
     610             int rndInd = int(motionSim.motionViews.size() * cv::randu<double>());
     611             Mat_<double> r0 = -motionSim.motionViews[rndInd].r;
     612             Mat_<double> t0 = -motionSim.motionViews[rndInd].R.t() * motionSim.motionViews[rndInd].t;
     613             const MotionView& motionView = motionSim.motionViews[rndInd];
     614             double errRatio = 0.9;
     615 
     616             //1.CalcByCeresWithAutoDeriv
     617             Mat_<double> R1; cv::Rodrigues(r0 * errRatio, R1);
     618             Mat_<double> t1 = t0 * errRatio;
     619             ceres::Problem problem1;
     620             problem1.AddResidualBlock(new ceres::AutoDiffCostFunction<ProjectionModelRt, ceres::DYNAMIC, 9, 3>(
     621                 new ProjectionModelRt(motionView, motionSim.nCamDist), motionView.point3D.rows * 2), NULL, R1.ptr<double>(), t1.ptr<double>());
     622             problem1.SetParameterization(R1.ptr<double>(), new LocalParamRWithGeneralJ);
     623             ceres::Solver::Options options1;
     624             ceres::Solver::Summary summary1;
     625             ceres::Solve(options1, &problem1, &summary1);
     626             Mat_<double> r1; cv::Rodrigues(R1, r1);
     627             int nIter1 = (int)summary1.iterations.size();
     628 
     629             //2.CalcByCeresWithGeneralJ
     630             Mat_<double> R2; cv::Rodrigues(r0 * errRatio, R2);
     631             Mat_<double> t2 = t0 * errRatio;
     632             ceres::Problem problem2;
     633             problem2.AddResidualBlock(new ProjectionCostRt(motionView, motionSim.nCamDist, 9), NULL, R2.ptr<double>(), t2.ptr<double>());
     634             problem2.SetParameterization(R2.ptr<double>(), new LocalParamRWithGeneralJ);
     635             ceres::Solver::Options options2;
     636             ceres::Solver::Summary summary2;
     637             ceres::Solve(options2, &problem2, &summary2);
     638             Mat_<double> r2; cv::Rodrigues(R2, r2);
     639             int nIter2 = (int)summary2.iterations.size();
     640 
     641             //3.CalcByCeresWithGeneralJ
     642             Mat_<double> r3 = r0 * errRatio;
     643             Mat_<double> t3 = t0 * errRatio;
     644             ceres::Problem problem3;
     645             problem3.AddResidualBlock(new ProjectionCostRt(motionView, motionSim.nCamDist, 3), NULL, r3.ptr<double>(), t3.ptr<double>());
     646             problem3.SetParameterization(r3.ptr<double>(), new LocalParamRWithIdentJ);
     647             ceres::Solver::Options options3;
     648             ceres::Solver::Summary summary3;
     649             ceres::Solve(options3, &problem3, &summary3);
     650             int nIter3 = (int)summary3.iterations.size();
     651 
     652             //4.AnalyzeError
     653             double infr0r0 = norm(r0, r0 * errRatio, NORM_INF);
     654             double infr0r1 = norm(r0, r1, NORM_INF);
     655             double infr0r2 = norm(r0, r2, NORM_INF);
     656             double infr0r3 = norm(r0, r3, NORM_INF);
     657             double inft0t0 = norm(t0, t0 * errRatio, NORM_INF);
     658             double inft0t1 = norm(t0, t1, NORM_INF);
     659             double inft0t2 = norm(t0, t2, NORM_INF);
     660             double inft0t3 = norm(t0, t3, NORM_INF);
     661 
     662             //5.PrintError
     663             cout << fmt::format("LoopCount: {}      AutoDeriv.iters: {}      GeneralJ.iters: {}      IdentJ.iters: {}
    ", k, nIter1, nIter2, nIter3);
     664             if (infr0r1 > 1e-8 || infr0r2 > 1e-8 || infr0r3 > 1e-8 || inft0t1 > 1e-8 || inft0t2 > 1e-8 || inft0t3 > 1e-8)
     665             {
     666                 cout << fmt::format("infr0r1: {:<15.9}		{:<15.9}
    ", infr0r1, infr0r0);
     667                 cout << fmt::format("infr0r2: {:<15.9}		{:<15.9}
    ", infr0r2, infr0r0);
     668                 cout << fmt::format("infr0r3: {:<15.9}		{:<15.9}
    ", infr0r3, infr0r0);
     669                 cout << fmt::format("inft0t1: {:<15.9}		{:<15.9}
    ", inft0t1, inft0t0);
     670                 cout << fmt::format("inft0t2: {:<15.9}		{:<15.9}
    ", inft0t2, inft0t0);
     671                 cout << fmt::format("inft0t3: {:<15.9}		{:<15.9}
    ", inft0t3, inft0t0);
     672                 cout << "Press any key to continue" << endl; std::getchar();
     673             }
     674         }
     675     }
     676 
     677 public:
     678     struct ProjectionModelRt
     679     {
     680         const int nDist;
     681         double K[4];
     682         const double* D;
     683         Mat_<Vec2d> point2D;
     684         Mat_<Vec3d> point3D;
     685         ProjectionModelRt(const MotionView& motionView0, const int nDist0) : nDist(nDist0)
     686         {
     687             K[0] = motionView0.K(0, 0);
     688             K[1] = motionView0.K(1, 1);
     689             K[2] = motionView0.K(0, 2);
     690             K[3] = motionView0.K(1, 2);
     691             D = motionView0.D.ptr<double>();
     692             point2D = motionView0.point2D;
     693             point3D = motionView0.point3D;
     694         }
     695         template <typename tp> bool operator()(const tp* const rot, const tp* const t, tp* errPoint2D) const
     696         {
     697             //1.Projection params
     698             double fx = K[0];
     699             double fy = K[1];
     700             double cx = K[2];
     701             double cy = K[3];
     702 
     703             //2.Distortion params
     704             double k1 = D[0];
     705             double k2 = D[1];
     706             double p1 = D[2];
     707             double p2 = D[3];
     708             double k3, k4, k5, k6;
     709             double s1, s2, s3, s4;
     710             if (nDist > 4) k3 = D[4];
     711             if (nDist > 5) { k4 = D[5]; k5 = D[6]; k6 = D[7]; }
     712             if (nDist > 8) { s1 = D[8]; s2 = D[9]; s3 = D[10]; s4 = D[11]; }
     713 
     714             //3.Translation params
     715             tp tx = t[0];
     716             tp ty = t[1];
     717             tp tz = t[2];
     718 
     719             //4.Rotation params
     720             tp R11, R12, R13, R21, R22, R23, R31, R32, R33;
     721             {
     722                 R11 = rot[0]; R12 = rot[1]; R13 = rot[2];
     723                 R21 = rot[3]; R22 = rot[4]; R23 = rot[5];
     724                 R31 = rot[6]; R32 = rot[7]; R33 = rot[8];
     725             }
     726 
     727             //5.ReProjection
     728             const Vec2d* data2D = point2D.ptr<Vec2d>();
     729             const Vec3d* data3D = point3D.ptr<Vec3d>();
     730             Vec<tp, 2>* err2D = (Vec<tp, 2>*)errPoint2D;
     731             for (int k = 0; k < point3D.rows; ++k)
     732             {
     733                 //5.1 WorldCoordinate
     734                 double X = data3D[k][0];
     735                 double Y = data3D[k][1];
     736                 double Z = data3D[k][2];
     737 
     738                 //5.2 CameraCoordinate
     739                 tp x = R11 * X + R12 * Y + R13 * Z + tx;
     740                 tp y = R21 * X + R22 * Y + R23 * Z + ty;
     741                 tp z = R31 * X + R32 * Y + R33 * Z + tz;
     742 
     743                 //5.3 StandardPhysicsCoordinate
     744                 tp iz = 1. / z; //if denominator==0
     745                 tp xc = x * iz;
     746                 tp yc = y * iz;
     747 
     748                 //5.4 DistortionPhysicsCoordinate
     749                 tp xc2 = xc * xc;
     750                 tp yc2 = yc * yc;
     751                 tp d2 = xc2 + yc2;
     752                 tp xcyc = 2. * xc * yc;
     753                 tp d4 = d2 * d2;
     754                 tp d6 = d2 * d4;
     755                 tp d2xc2 = d2 + 2. * xc2;
     756                 tp d2yc2 = d2 + 2. * yc2;
     757                 tp nu, de, xd, yd;
     758                 if (nDist < 5)
     759                 {
     760                     nu = 1. + k1 * d2 + k2 * d4;
     761                     xd = xc * nu + p1 * xcyc + p2 * d2xc2;
     762                     yd = yc * nu + p2 * xcyc + p1 * d2yc2;
     763                 }
     764                 else if (nDist < 8)
     765                 {
     766                     nu = 1. + k1 * d2 + k2 * d4 + k3 * d6;
     767                     xd = xc * nu + p1 * xcyc + p2 * d2xc2;
     768                     yd = yc * nu + p2 * xcyc + p1 * d2yc2;
     769                 }
     770                 else if (nDist < 12)
     771                 {
     772                     nu = 1. + k1 * d2 + k2 * d4 + k3 * d6;
     773                     de = 1. / (1. + k4 * d2 + k5 * d4 + k6 * d6);//if denominator==0
     774                     xd = xc * nu * de + p1 * xcyc + p2 * d2xc2;
     775                     yd = yc * nu * de + p2 * xcyc + p1 * d2yc2;
     776                 }
     777                 else if (nDist < 14)
     778                 {
     779                     nu = 1. + k1 * d2 + k2 * d4 + k3 * d6;
     780                     de = 1. / (1. + k4 * d2 + k5 * d4 + k6 * d6);//if denominator==0
     781                     xd = xc * nu * de + p1 * xcyc + p2 * d2xc2 + s1 * d2 + s2 * d4;
     782                     yd = yc * nu * de + p2 * xcyc + p1 * d2yc2 + s3 * d2 + s4 * d4;
     783                 }
     784                 err2D[k][0] = xd * fx + cx - data2D[k][0];
     785                 err2D[k][1] = yd * fy + cy - data2D[k][1];
     786             }
     787             return true;
     788         }
     789     };
     790     struct LocalParamRWithGeneralJ : public ceres::LocalParameterization
     791     {
     792         bool Plus(const double* preParam, const double* deltaParam, double* newParam) const
     793         {
     794             Matx33d ambientPre(preParam);
     795             Matx33d ambientDelta; cv::Rodrigues(Matx31d(deltaParam), ambientDelta);
     796             Matx33d ambientNew = ambientDelta * ambientPre;
     797             memcpy(newParam, ambientNew.val, sizeof(double) * 9);
     798             return true;
     799         }
     800 
     801         bool ComputeJacobian(const double* param, double* jacobian) const
     802         {
     803             Mat_<double> dRdr(9, 3, jacobian);
     804             dRdr << 0, param[6], -param[3], 0, param[7], -param[4], 0, param[8], -param[5],
     805                 -param[6], 0, param[0], -param[7], 0, param[1], -param[8], 0, param[2],
     806                 param[3], -param[0], 0, param[4], -param[1], 0, param[5], -param[2], 0;
     807             return true;
     808 
     809             Matx33d R(param);
     810             Mat_<double> dRdrT({ 3, 9 }, {
     811                 0, 0, 0, -R(2, 0), -R(2, 1), -R(2, 2), R(1, 0), R(1, 1), R(1, 2),
     812                 R(2, 0), R(2, 1), R(2, 2), 0, 0, 0, -R(0, 0), -R(0, 1), -R(0, 2),
     813                 -R(1, 0), -R(1, 1), -R(1, 2), R(0, 0), R(0, 1), R(0, 2), 0, 0, 0 });
     814             cout << endl << cv::norm(dRdr, dRdrT.t(), NORM_INF) << endl;
     815         }
     816 
     817         int GlobalSize() const { return 9; }
     818         int LocalSize() const { return 3; }
     819     };
     820     struct LocalParamRWithIdentJ : public ceres::LocalParameterization
     821     {
     822         bool Plus(const double* preParam, const double* deltaParam, double* newParam) const
     823         {
     824             Matx33d ambientPre; cv::Rodrigues(Matx31d(preParam), ambientPre);
     825             Matx33d ambientDelta; cv::Rodrigues(Matx31d(deltaParam), ambientDelta);
     826             Matx31d tangentNew; cv::Rodrigues(ambientDelta * ambientPre, tangentNew);
     827             memcpy(newParam, tangentNew.val, sizeof(double) * 3);
     828             return true;
     829         }
     830 
     831         bool ComputeJacobian(const double* param, double* jacobian) const
     832         {
     833             Mat_<double>(3, 3, jacobian) = Mat_<double>::eye(3, 3);
     834             return true;
     835         }
     836 
     837         int GlobalSize() const { return 3; }
     838         int LocalSize() const { return 3; }
     839     };
     840     struct ProjectionCostRt : public ceres::CostFunction
     841     {
     842         const int nDist;
     843         double K[4];
     844         const double* D;
     845         Mat_<Vec2d> point2D;
     846         Mat_<Vec3d> point3D;
     847         const int nRot;
     848         const MotionView& motionViewForDBG;//only use once
     849         ProjectionCostRt(const MotionView& motionView0, const int nDist0, const int nRot0) : nDist(nDist0), nRot(nRot0), motionViewForDBG(motionView0)
     850         {
     851             K[0] = motionView0.K(0, 0);
     852             K[1] = motionView0.K(1, 1);
     853             K[2] = motionView0.K(0, 2);
     854             K[3] = motionView0.K(1, 2);
     855             D = motionView0.D.ptr<double>();
     856             point2D = motionView0.point2D;
     857             point3D = motionView0.point3D;
     858             this->set_num_residuals(point2D.rows * 2);
     859             this->mutable_parameter_block_sizes()->push_back(nRot);
     860             this->mutable_parameter_block_sizes()->push_back(3);
     861         }
     862         bool Evaluate(double const* const* params, double* residuals, double** jacobians) const
     863         {
     864             //0.FormatInput
     865             const double* rot = params[0];
     866             const double* t = params[1];
     867             Vec2d* err2D = (Vec2d*)(residuals);
     868             Mat_<double> dpdR; if (jacobians && jacobians[0]) dpdR = Mat_<double>(point2D.rows * 2, nRot, jacobians[0]);
     869             Mat_<double> dpdt; if (jacobians && jacobians[1]) dpdt = Mat_<double>(point2D.rows * 2, 3, jacobians[1]);
     870 
     871             //1.Projection params
     872             double fx = K[0];
     873             double fy = K[1];
     874             double cx = K[2];
     875             double cy = K[3];
     876 
     877             //2.Distortion params
     878             double k1 = D[0];
     879             double k2 = D[1];
     880             double p1 = D[2];
     881             double p2 = D[3];
     882             double k3 = 0, k4 = 0, k5 = 0, k6 = 0;
     883             double s1 = 0, s2 = 0, s3 = 0, s4 = 0;
     884             if (nDist > 4) k3 = D[4];
     885             if (nDist > 5) { k4 = D[5]; k5 = D[6]; k6 = D[7]; }
     886             if (nDist > 8) { s1 = D[8]; s2 = D[9]; s3 = D[10]; s4 = D[11]; }
     887 
     888             //3.Translation params
     889             double tx = t[0];
     890             double ty = t[1];
     891             double tz = t[2];
     892 
     893             //4.Rotation params
     894             double R11, R12, R13, R21, R22, R23, R31, R32, R33;
     895             if (nRot == 9)
     896             {
     897                 R11 = rot[0]; R12 = rot[1]; R13 = rot[2];
     898                 R21 = rot[3]; R22 = rot[4]; R23 = rot[5];
     899                 R31 = rot[6]; R32 = rot[7]; R33 = rot[8];
     900             }
     901             if (nRot == 3)
     902             {
     903                 Matx33d R; cv::Rodrigues(Matx31d(rot), R);
     904                 rot = R.val;
     905                 R11 = rot[0]; R12 = rot[1]; R13 = rot[2];
     906                 R21 = rot[3]; R22 = rot[4]; R23 = rot[5];
     907                 R31 = rot[6]; R32 = rot[7]; R33 = rot[8];
     908                 rot = params[0];
     909             }
     910 
     911             //5.ReProjection
     912             const Vec2d* data2D = point2D.ptr<Vec2d>();
     913             const Vec3d* data3D = point3D.ptr<Vec3d>();
     914             for (int k = 0; k < point3D.rows; ++k)
     915             {
     916                 //5.1 WorldCoordinate
     917                 double X = data3D[k][0];
     918                 double Y = data3D[k][1];
     919                 double Z = data3D[k][2];
     920 
     921                 //5.2 CameraCoordinate
     922                 double x = R11 * X + R12 * Y + R13 * Z + tx;
     923                 double y = R21 * X + R22 * Y + R23 * Z + ty;
     924                 double z = R31 * X + R32 * Y + R33 * Z + tz;
     925 
     926                 //5.3 StandardPhysicsCoordinate
     927                 double iz = 1. / z; //if denominator==0
     928                 double xc = x * iz;
     929                 double yc = y * iz;
     930 
     931                 //5.4 DistortionPhysicsCoordinate
     932                 double xc2 = xc * xc;
     933                 double yc2 = yc * yc;
     934                 double d2 = xc2 + yc2;
     935                 double xcyc = 2. * xc * yc;
     936                 double d4 = d2 * d2;
     937                 double d6 = d2 * d4;
     938                 double d2xc2 = d2 + 2. * xc2;
     939                 double d2yc2 = d2 + 2. * yc2;
     940                 double nu = 0, de = 1., xd = 0, yd = 0;
     941                 if (nDist < 5)
     942                 {
     943                     nu = 1. + k1 * d2 + k2 * d4;
     944                     xd = xc * nu + p1 * xcyc + p2 * d2xc2;
     945                     yd = yc * nu + p2 * xcyc + p1 * d2yc2;
     946                 }
     947                 else if (nDist < 8)
     948                 {
     949                     nu = 1. + k1 * d2 + k2 * d4 + k3 * d6;
     950                     xd = xc * nu + p1 * xcyc + p2 * d2xc2;
     951                     yd = yc * nu + p2 * xcyc + p1 * d2yc2;
     952                 }
     953                 else if (nDist < 12)
     954                 {
     955                     nu = 1. + k1 * d2 + k2 * d4 + k3 * d6;
     956                     de = 1. / (1. + k4 * d2 + k5 * d4 + k6 * d6);//if denominator==0
     957                     xd = xc * nu * de + p1 * xcyc + p2 * d2xc2;
     958                     yd = yc * nu * de + p2 * xcyc + p1 * d2yc2;
     959                 }
     960                 else if (nDist < 14)
     961                 {
     962                     nu = 1. + k1 * d2 + k2 * d4 + k3 * d6;
     963                     de = 1. / (1. + k4 * d2 + k5 * d4 + k6 * d6);//if denominator==0
     964                     xd = xc * nu * de + p1 * xcyc + p2 * d2xc2 + s1 * d2 + s2 * d4;
     965                     yd = yc * nu * de + p2 * xcyc + p1 * d2yc2 + s3 * d2 + s4 * d4;
     966                 }
     967                 err2D[k][0] = xd * fx + cx - data2D[k][0];
     968                 err2D[k][1] = yd * fy + cy - data2D[k][1];
     969                 if (!dpdR.empty() || !dpdt.empty())
     970                 {
     971                     Mat_<double> dpDdpCD({ 2, 2 }, { fx, 0, 0, fy });
     972 
     973                     Mat_<double> dpCDdh({ 2, 5 }, {
     974                         xc * de, -xc * nu * de * de, p2 + s1 + 2 * s2 * d2, nu * de + 2 * p1 * yc + 4 * p2 * xc, 2 * p1 * xc,
     975                         yc * de, -yc * nu * de * de, p1 + s3 + 2 * s4 * d2, 2 * p2 * yc, nu * de + 2 * p2 * xc + 4 * p1 * yc });
     976 
     977                     Mat_<double> dhdpC({ 5, 2 }, {
     978                         xc * (2 * k1 + 4 * k2 * d2 + 6 * k3 * d4), yc * (2 * k1 + 4 * k2 * d2 + 6 * k3 * d4),
     979                         xc * (2 * k4 + 4 * k5 * d2 + 6 * k6 * d4), yc * (2 * k4 + 4 * k5 * d2 + 6 * k6 * d4),
     980                         2 * xc, 2 * yc,
     981                         1, 0,
     982                         0, 1 });
     983 
     984                     Mat_<double> dpCdPC({ 2, 3 }, {
     985                         iz, 0, -xc * iz,
     986                         0, iz, -yc * iz });
     987 
     988                     if (!dpdt.empty()) dpdt.rowRange(2 * k, 2 * k + 2) = dpDdpCD * dpCDdh * dhdpC * dpCdPC;
     989                     if (!dpdR.empty() && nRot == 9)
     990                     {
     991                         Mat_<double> dPCdR({ 3, 9 }, {
     992                             X, Y, Z, 0, 0, 0, 0, 0, 0,
     993                             0, 0, 0, X, Y, Z, 0, 0, 0,
     994                             0, 0, 0, 0, 0, 0, X, Y, Z });
     995                         dpdR.rowRange(2 * k, 2 * k + 2) = (dpDdpCD * dpCDdh * dhdpC * dpCdPC * dPCdR);
     996                     }
     997                     if (!dpdR.empty() && nRot == 3)
     998                     {
     999                         Mat_<double> _skewPC({ 3, 3 }, {
    1000                             0, (z - tz), -(y - ty),
    1001                             -(z - tz), 0, (x - tx),
    1002                               (y - ty), -(x - tx), 0 });
    1003                         dpdR.rowRange(2 * k, 2 * k + 2) = dpDdpCD * dpCDdh * dhdpC * dpCdPC * _skewPC;
    1004                     }
    1005                 }
    1006             }
    1007 
    1008             return true;
    1009             if (nRot == 9)
    1010             {
    1011                 ceres::AutoDiffCostFunction<ProjectionModelRt, ceres::DYNAMIC, 9, 3> autoDeriv(new ProjectionModelRt(motionViewForDBG, nDist), motionViewForDBG.point3D.rows * 2);
    1012                 Mat_<double> dpdRDBG(point3D.rows * 2, 9);
    1013                 Mat_<double> dpdtDBG(point3D.rows * 2, 3);
    1014                 Mat_<Vec2d> err2DDBG(point3D.rows, 1);
    1015                 const double* parametersDBG[2] = { rot, t };
    1016                 double* jacobiansDBG[2] = { dpdRDBG.ptr<double>(), dpdtDBG.ptr<double>() };
    1017                 double* residualsDBG = err2DDBG.ptr<double>();
    1018                 autoDeriv.Evaluate(parametersDBG, residualsDBG, jacobiansDBG);
    1019 
    1020                 if (!dpdR.empty()) cout << endl << norm(dpdR, dpdRDBG, NORM_INF) << endl;
    1021                 if (!dpdt.empty()) cout << endl << norm(dpdt, dpdtDBG, NORM_INF) << endl;
    1022                 cout << endl << norm(Mat_<Vec2d>(point2D.rows, 1, err2D), err2DDBG, NORM_INF) << endl;
    1023                 std::getchar();
    1024             }
    1025             return true;
    1026         }
    1027     };
    1028 };
    1029 
    1030 class OptimizeKDRt
    1031 {
    1032 public:
    1033     using MotionView = MotionSim::MotionView;
    1034     static void TestMe(int argc = 0, char** argv = 0)
    1035     {
    1036         int N = 99;
    1037         for (int k = 0; k < N; ++k)
    1038         {
    1039             //0.GenerateData
    1040             bool world2D = k % 2;
    1041             int rotMode = k % 7 + 1;
    1042             MotionSim motionSim(false);
    1043             motionSim.camFovX = 90;
    1044             motionSim.camFovY = 90;
    1045             motionSim.camRand = 10;
    1046             motionSim.nMinMotion = 16;//2
    1047             motionSim.nMaxMotion = 32;//4
    1048             motionSim.rndSeek = false;
    1049             static const int nDist = 5;
    1050             motionSim.nCamDist = nDist;
    1051             motionSim.runMotion(world2D, false, rotMode);
    1052             //motionSim.visMotion();
    1053             Mat_<double> rs0; for (size_t k = 0; k < motionSim.motionViews.size(); ++k) rs0.push_back(-motionSim.motionViews[k].r);
    1054             Mat_<double> ts0; for (size_t k = 0; k < motionSim.motionViews.size(); ++k) ts0.push_back(-motionSim.motionViews[k].R.t() * motionSim.motionViews[k].t);
    1055             Mat_<double> K0({ 4, 1 }, { motionSim.motionViews[0].K(0, 0), motionSim.motionViews[0].K(1, 1), motionSim.motionViews[0].K(0, 2), motionSim.motionViews[0].K(1, 2) });
    1056             Mat_<double> D0 = motionSim.motionViews[0].D.clone();
    1057             double errRatio = 0.9;
    1058             double errRatioTrans = 0.99;
    1059 
    1060             //1.CalcByCeresWithAutoDeriv
    1061             Mat_<double> Rs1; for (int k = 0; k < rs0.rows; k += 3) { Mat_<double> _R; cv::Rodrigues(rs0.rowRange(k, k + 3) * errRatio, _R); Rs1.push_back(_R); }
    1062             Mat_<double> ts1 = ts0 * errRatioTrans;
    1063             Mat_<double> K1 = K0 * errRatio;
    1064             Mat_<double> D1 = D0 * errRatio;
    1065             ceres::Problem problem1;
    1066             for (int k = 0; k < motionSim.motionViews.size(); ++k)
    1067                 problem1.AddResidualBlock(new ceres::AutoDiffCostFunction<ProjectionModelKDRt, ceres::DYNAMIC, 9, 3, 4, nDist>(
    1068                     new ProjectionModelKDRt(motionSim.motionViews[k], motionSim.nCamDist), motionSim.motionViews[k].point3D.rows * 2), NULL,
    1069                     Rs1.ptr<double>(k * 3), ts1.ptr<double>(k * 3),
    1070                     K1.ptr<double>(), D1.ptr<double>());
    1071             for (int k = 0; k < motionSim.motionViews.size(); ++k) problem1.SetParameterization(Rs1.ptr<double>(k * 3), new OptimizeRt::LocalParamRWithGeneralJ);
    1072             ceres::Solver::Options options;
    1073             ceres::Solver::Summary summary;
    1074             ceres::Solve(options, &problem1, &summary);
    1075             Mat_<double> rs1; for (int k = 0; k < Rs1.rows; k += 3) { Mat_<double> _r; cv::Rodrigues(Rs1.rowRange(k, k + 3), _r); rs1.push_back(_r); }
    1076             int nIter1 = (int)summary.iterations.size();
    1077 
    1078             //2.CalcByCeresWithGeneralJ
    1079             Mat_<double> Rs2; for (int k = 0; k < rs0.rows; k += 3) { Mat_<double> _R; cv::Rodrigues(rs0.rowRange(k, k + 3) * errRatio, _R); Rs2.push_back(_R); }
    1080             Mat_<double> ts2 = ts0 * errRatioTrans;
    1081             Mat_<double> K2 = K0 * errRatio;
    1082             Mat_<double> D2 = D0 * errRatio;
    1083             ceres::Problem problem2;
    1084             for (int k = 0; k < motionSim.motionViews.size(); ++k)
    1085                 problem2.AddResidualBlock(new ProjectionCostKDRt(motionSim.motionViews[k], motionSim.nCamDist, 9), NULL,
    1086                     Rs2.ptr<double>(k * 3), ts2.ptr<double>(k * 3), K2.ptr<double>(), D2.ptr<double>());
    1087             for (int k = 0; k < motionSim.motionViews.size(); ++k) problem2.SetParameterization(Rs2.ptr<double>(k * 3), new OptimizeRt::LocalParamRWithGeneralJ);
    1088             ceres::Solver::Options options2;
    1089             ceres::Solver::Summary summary2;
    1090             ceres::Solve(options2, &problem2, &summary2);
    1091             Mat_<double> rs2; for (int k = 0; k < Rs2.rows; k += 3) { Mat_<double> _r; cv::Rodrigues(Rs2.rowRange(k, k + 3), _r); rs2.push_back(_r); }
    1092             int nIter2 = (int)summary2.iterations.size();
    1093 
    1094             //3.CalcByCeresWithIdentJ
    1095             Mat_<double> rs3; for (int k = 0; k < rs0.rows; k += 3) rs3.push_back(rs0.rowRange(k, k + 3) * errRatio);
    1096             Mat_<double> ts3 = ts0 * errRatioTrans;
    1097             Mat_<double> K3 = K0 * errRatio;
    1098             Mat_<double> D3 = D0 * errRatio;
    1099             ceres::Problem problem3;
    1100             for (int k = 0; k < motionSim.motionViews.size(); ++k)
    1101                 problem3.AddResidualBlock(new ProjectionCostKDRt(motionSim.motionViews[k], motionSim.nCamDist, 3), NULL,
    1102                     rs3.ptr<double>(k * 3), ts3.ptr<double>(k * 3), K3.ptr<double>(), D3.ptr<double>());
    1103             for (int k = 0; k < motionSim.motionViews.size(); ++k) problem3.SetParameterization(rs3.ptr<double>(k * 3), new OptimizeRt::LocalParamRWithIdentJ);
    1104             ceres::Solver::Options options3;
    1105             ceres::Solver::Summary summary3;
    1106             ceres::Solve(options3, &problem3, &summary3);
    1107             int nIter3 = (int)summary3.iterations.size();
    1108 
    1109             //4.AnalyzeError
    1110             double infrs0rs0 = norm(rs0, rs0 * errRatio, NORM_INF);
    1111             double infrs0rs1 = norm(rs0, rs1, NORM_INF);
    1112             double infrs0rs2 = norm(rs0, rs2, NORM_INF);
    1113             double infrs0rs3 = norm(rs0, rs3, NORM_INF);
    1114             double infts0ts0 = norm(ts0, ts0 * errRatioTrans, NORM_INF);
    1115             double infts0ts1 = norm(ts0, ts1, NORM_INF);
    1116             double infts0ts2 = norm(ts0, ts2, NORM_INF);
    1117             double infts0ts3 = norm(ts0, ts3, NORM_INF);
    1118             double infK0K0 = norm(K0, K0 * errRatio, NORM_INF);
    1119             double infK0K1 = norm(K0, K1, NORM_INF);
    1120             double infK0K2 = norm(K0, K2, NORM_INF);
    1121             double infK0K3 = norm(K0, K3, NORM_INF);
    1122             double infD0D0 = norm(D0, D0 * errRatio, NORM_INF);
    1123             double infD0D1 = norm(D0, D1, NORM_INF);
    1124             double infD0D2 = norm(D0, D2, NORM_INF);
    1125             double infD0D3 = norm(D0, D3, NORM_INF);
    1126 
    1127             //5.PrintError
    1128             cout << fmt::format("LoopCount: {}      AutoDeriv.iters: {}      GeneralJ.iters: {}      IdentJ.iters: {}
    ", k, nIter1, nIter2, nIter3);
    1129             if (infrs0rs1 > 1e-8 || infrs0rs2 > 1e-8 || infrs0rs3 > 1e-8 || infts0ts1 > 1e-8 || infts0ts2 > 1e-8 || infts0ts3 > 1e-8 ||
    1130                 infK0K1 > 1e-8 || infK0K2 > 1e-8 || infK0K3 > 1e-8 || infD0D1 > 1e-8 || infD0D2 > 1e-8 || infD0D3 > 1e-8)
    1131             {
    1132                 cout << fmt::format("infrs0rs1: {:<15.9}		{:<15.9}
    ", infrs0rs1, infrs0rs0);
    1133                 cout << fmt::format("infrs0rs2: {:<15.9}		{:<15.9}
    ", infrs0rs2, infrs0rs0);
    1134                 cout << fmt::format("infrs0rs3: {:<15.9}		{:<15.9}
    ", infrs0rs3, infrs0rs0);
    1135                 cout << fmt::format("infts0ts1: {:<15.9}		{:<15.9}
    ", infts0ts1, infts0ts0);
    1136                 cout << fmt::format("infts0ts2: {:<15.9}		{:<15.9}
    ", infts0ts2, infts0ts0);
    1137                 cout << fmt::format("infts0ts3: {:<15.9}		{:<15.9}
    ", infts0ts3, infts0ts0);
    1138                 cout << fmt::format("infK0K1  : {:<15.9}		{:<15.9}
    ", infK0K1, infK0K0);
    1139                 cout << fmt::format("infK0K2  : {:<15.9}		{:<15.9}
    ", infK0K2, infK0K0);
    1140                 cout << fmt::format("infK0K3  : {:<15.9}		{:<15.9}
    ", infK0K3, infK0K0);
    1141                 cout << fmt::format("infD0D1  : {:<15.9}		{:<15.9}
    ", infD0D1, infD0D0);
    1142                 cout << fmt::format("infD0D2  : {:<15.9}		{:<15.9}
    ", infD0D2, infD0D0);
    1143                 cout << fmt::format("infD0D3  : {:<15.9}		{:<15.9}
    ", infD0D3, infD0D0);
    1144                 cout << "Press any key to continue" << endl; std::getchar();
    1145             }
    1146         }
    1147     }
    1148 
    1149 public:
    1150     struct ProjectionModelKDRt
    1151     {
    1152         const int nDist;
    1153         Mat_<Vec2d> point2D;
    1154         Mat_<Vec3d> point3D;
    1155         ProjectionModelKDRt(const MotionView& motionView0, const int nDist0) : nDist(nDist0)
    1156         {
    1157             point2D = motionView0.point2D;
    1158             point3D = motionView0.point3D;
    1159         }
    1160         template <typename tp> bool operator()(const tp* const rot, const tp* const t, const tp* const K, const tp* const D, tp* errPoint2D) const
    1161         {
    1162             //1.Projection params
    1163             tp fx = K[0];
    1164             tp fy = K[1];
    1165             tp cx = K[2];
    1166             tp cy = K[3];
    1167 
    1168             //2.Distortion params
    1169             tp k1 = D[0];
    1170             tp k2 = D[1];
    1171             tp p1 = D[2];
    1172             tp p2 = D[3];
    1173             tp k3, k4, k5, k6;
    1174             tp s1, s2, s3, s4;
    1175             if (nDist > 4) k3 = D[4];
    1176             if (nDist > 5) { k4 = D[5]; k5 = D[6]; k6 = D[7]; }
    1177             if (nDist > 8) { s1 = D[8]; s2 = D[9]; s3 = D[10]; s4 = D[11]; }
    1178 
    1179             //3.Translation params
    1180             tp tx = t[0];
    1181             tp ty = t[1];
    1182             tp tz = t[2];
    1183 
    1184             //4.
    1185             tp R11, R12, R13, R21, R22, R23, R31, R32, R33;
    1186             {
    1187                 R11 = rot[0]; R12 = rot[1]; R13 = rot[2];
    1188                 R21 = rot[3]; R22 = rot[4]; R23 = rot[5];
    1189                 R31 = rot[6]; R32 = rot[7]; R33 = rot[8];
    1190             }
    1191 
    1192             //5.ReProjection
    1193             const Vec2d* data2D = point2D.ptr<Vec2d>();
    1194             const Vec3d* data3D = point3D.ptr<Vec3d>();
    1195             Vec<tp, 2>* err2D = (Vec<tp, 2>*)errPoint2D;
    1196             for (int k = 0; k < point3D.rows; ++k)
    1197             {
    1198                 //5.1 WorldCoordinate
    1199                 double X = data3D[k][0];
    1200                 double Y = data3D[k][1];
    1201                 double Z = data3D[k][2];
    1202 
    1203                 //5.2 CameraCoordinate
    1204                 tp x = R11 * X + R12 * Y + R13 * Z + tx;
    1205                 tp y = R21 * X + R22 * Y + R23 * Z + ty;
    1206                 tp z = R31 * X + R32 * Y + R33 * Z + tz;
    1207 
    1208                 //5.3 StandardPhysicsCoordinate
    1209                 tp iz = 1. / z; //if denominator==0
    1210                 tp xc = x * iz;
    1211                 tp yc = y * iz;
    1212 
    1213                 //5.4 DistortionPhysicsCoordinate
    1214                 tp xc2 = xc * xc;
    1215                 tp yc2 = yc * yc;
    1216                 tp d2 = xc2 + yc2;
    1217                 tp xcyc = 2. * xc * yc;
    1218                 tp d4 = d2 * d2;
    1219                 tp d6 = d2 * d4;
    1220                 tp d2xc2 = d2 + 2. * xc2;
    1221                 tp d2yc2 = d2 + 2. * yc2;
    1222                 tp nu, de, xd, yd;
    1223                 if (nDist < 5)
    1224                 {
    1225                     nu = 1. + k1 * d2 + k2 * d4;
    1226                     xd = xc * nu + p1 * xcyc + p2 * d2xc2;
    1227                     yd = yc * nu + p2 * xcyc + p1 * d2yc2;
    1228                 }
    1229                 else if (nDist < 8)
    1230                 {
    1231                     nu = 1. + k1 * d2 + k2 * d4 + k3 * d6;
    1232                     xd = xc * nu + p1 * xcyc + p2 * d2xc2;
    1233                     yd = yc * nu + p2 * xcyc + p1 * d2yc2;
    1234                 }
    1235                 else if (nDist < 12)
    1236                 {
    1237                     nu = 1. + k1 * d2 + k2 * d4 + k3 * d6;
    1238                     de = 1. / (1. + k4 * d2 + k5 * d4 + k6 * d6);//if denominator==0
    1239                     xd = xc * nu * de + p1 * xcyc + p2 * d2xc2;
    1240                     yd = yc * nu * de + p2 * xcyc + p1 * d2yc2;
    1241                 }
    1242                 else if (nDist < 14)
    1243                 {
    1244                     nu = 1. + k1 * d2 + k2 * d4 + k3 * d6;
    1245                     de = 1. / (1. + k4 * d2 + k5 * d4 + k6 * d6);//if denominator==0
    1246                     xd = xc * nu * de + p1 * xcyc + p2 * d2xc2 + s1 * d2 + s2 * d4;
    1247                     yd = yc * nu * de + p2 * xcyc + p1 * d2yc2 + s3 * d2 + s4 * d4;
    1248                 }
    1249                 err2D[k][0] = xd * fx + cx - data2D[k][0];
    1250                 err2D[k][1] = yd * fy + cy - data2D[k][1];
    1251             }
    1252             return true;
    1253         }
    1254     };
    1255     struct ProjectionCostKDRt : public ceres::CostFunction
    1256     {
    1257         const int nDist;
    1258         Mat_<Vec2d> point2D;
    1259         Mat_<Vec3d> point3D;
    1260         const int nRot;
    1261         const MotionView& motionViewForDBG;//Only used once
    1262         ProjectionCostKDRt(const MotionView& motionView0, const int nDist0, const int nRot0) : nDist(nDist0), nRot(nRot0), motionViewForDBG(motionView0)
    1263         {
    1264             point2D = motionView0.point2D;
    1265             point3D = motionView0.point3D;
    1266             this->set_num_residuals(point2D.rows * 2);
    1267             this->mutable_parameter_block_sizes()->push_back(nRot);
    1268             this->mutable_parameter_block_sizes()->push_back(3);
    1269             this->mutable_parameter_block_sizes()->push_back(4);
    1270             this->mutable_parameter_block_sizes()->push_back(nDist);
    1271         }
    1272         bool Evaluate(double const* const* params, double* residuals, double** jacobians) const
    1273         {
    1274             //0.FormatInput
    1275             const double* rot = params[0];
    1276             const double* t = params[1];
    1277             const double* K = params[2];
    1278             const double* D = params[3];
    1279             Vec2d* err2D = (Vec2d*)(residuals);
    1280             Mat_<double> dpdR; if (jacobians && jacobians[0]) dpdR = Mat_<double>(point2D.rows * 2, nRot, jacobians[0]);
    1281             Mat_<double> dpdt; if (jacobians && jacobians[1]) dpdt = Mat_<double>(point2D.rows * 2, 3, jacobians[1]);
    1282             Mat_<double> dpdK; if (jacobians && jacobians[2]) dpdK = Mat_<double>(point2D.rows * 2, 4, jacobians[2]);
    1283             Mat_<double> dpdD; if (jacobians && jacobians[3]) dpdD = Mat_<double>(point2D.rows * 2, nDist, jacobians[3]);
    1284 
    1285             //1.Projection params
    1286             double fx = K[0];
    1287             double fy = K[1];
    1288             double cx = K[2];
    1289             double cy = K[3];
    1290 
    1291             //2.Distortion params
    1292             double k1 = D[0];
    1293             double k2 = D[1];
    1294             double p1 = D[2];
    1295             double p2 = D[3];
    1296             double k3 = 0, k4 = 0, k5 = 0, k6 = 0;
    1297             double s1 = 0, s2 = 0, s3 = 0, s4 = 0;
    1298             if (nDist > 4) k3 = D[4];
    1299             if (nDist > 5) { k4 = D[5]; k5 = D[6]; k6 = D[7]; }
    1300             if (nDist > 8) { s1 = D[8]; s2 = D[9]; s3 = D[10]; s4 = D[11]; }
    1301 
    1302             //3.Translation params
    1303             double tx = t[0];
    1304             double ty = t[1];
    1305             double tz = t[2];
    1306 
    1307             //4.Rotation params
    1308             double R11, R12, R13, R21, R22, R23, R31, R32, R33;
    1309             if (nRot == 9)
    1310             {
    1311                 R11 = rot[0]; R12 = rot[1]; R13 = rot[2];
    1312                 R21 = rot[3]; R22 = rot[4]; R23 = rot[5];
    1313                 R31 = rot[6]; R32 = rot[7]; R33 = rot[8];
    1314             }
    1315             if (nRot == 3)
    1316             {
    1317                 Matx33d R; cv::Rodrigues(Matx31d(rot), R);
    1318                 rot = R.val;
    1319                 R11 = rot[0]; R12 = rot[1]; R13 = rot[2];
    1320                 R21 = rot[3]; R22 = rot[4]; R23 = rot[5];
    1321                 R31 = rot[6]; R32 = rot[7]; R33 = rot[8];
    1322                 rot = params[0];
    1323             }
    1324 
    1325             //5.ReProjection
    1326             const Vec2d* data2D = point2D.ptr<Vec2d>();
    1327             const Vec3d* data3D = point3D.ptr<Vec3d>();
    1328             for (int k = 0; k < point3D.rows; ++k)
    1329             {
    1330                 //5.1 WorldCoordinate
    1331                 double X = data3D[k][0];
    1332                 double Y = data3D[k][1];
    1333                 double Z = data3D[k][2];
    1334 
    1335                 //5.2 CameraCoordinate
    1336                 double x = R11 * X + R12 * Y + R13 * Z + tx;
    1337                 double y = R21 * X + R22 * Y + R23 * Z + ty;
    1338                 double z = R31 * X + R32 * Y + R33 * Z + tz;
    1339 
    1340                 //5.3 StandardPhysicsCoordinate
    1341                 double iz = 1. / z; //if denominator==0
    1342                 double xc = x * iz;
    1343                 double yc = y * iz;
    1344 
    1345                 //5.4 DistortionPhysicsCoordinate
    1346                 double xc2 = xc * xc;
    1347                 double yc2 = yc * yc;
    1348                 double d2 = xc2 + yc2;
    1349                 double xcyc = 2. * xc * yc;
    1350                 double d4 = d2 * d2;
    1351                 double d6 = d2 * d4;
    1352                 double d2xc2 = d2 + 2. * xc2;
    1353                 double d2yc2 = d2 + 2. * yc2;
    1354                 double nu = 0, de = 1., xd = 0, yd = 0;
    1355                 if (nDist < 5)
    1356                 {
    1357                     nu = 1. + k1 * d2 + k2 * d4;
    1358                     xd = xc * nu + p1 * xcyc + p2 * d2xc2;
    1359                     yd = yc * nu + p2 * xcyc + p1 * d2yc2;
    1360                 }
    1361                 else if (nDist < 8)
    1362                 {
    1363                     nu = 1. + k1 * d2 + k2 * d4 + k3 * d6;
    1364                     xd = xc * nu + p1 * xcyc + p2 * d2xc2;
    1365                     yd = yc * nu + p2 * xcyc + p1 * d2yc2;
    1366                 }
    1367                 else if (nDist < 12)
    1368                 {
    1369                     nu = 1. + k1 * d2 + k2 * d4 + k3 * d6;
    1370                     de = 1. / (1. + k4 * d2 + k5 * d4 + k6 * d6);//if denominator==0
    1371                     xd = xc * nu * de + p1 * xcyc + p2 * d2xc2;
    1372                     yd = yc * nu * de + p2 * xcyc + p1 * d2yc2;
    1373                 }
    1374                 else if (nDist < 14)
    1375                 {
    1376                     nu = 1. + k1 * d2 + k2 * d4 + k3 * d6;
    1377                     de = 1. / (1. + k4 * d2 + k5 * d4 + k6 * d6);//if denominator==0
    1378                     xd = xc * nu * de + p1 * xcyc + p2 * d2xc2 + s1 * d2 + s2 * d4;
    1379                     yd = yc * nu * de + p2 * xcyc + p1 * d2yc2 + s3 * d2 + s4 * d4;
    1380                 }
    1381                 err2D[k][0] = xd * fx + cx - data2D[k][0];
    1382                 err2D[k][1] = yd * fy + cy - data2D[k][1];
    1383                 if (!dpdR.empty() || !dpdt.empty())
    1384                 {
    1385                     Mat_<double> dpDdpCD({ 2, 2 }, { fx, 0, 0, fy });
    1386 
    1387                     Mat_<double> dpCDdh({ 2, 5 }, {
    1388                         xc * de, -xc * nu * de * de, p2 + s1 + 2 * s2 * d2, nu * de + 2 * p1 * yc + 4 * p2 * xc, 2 * p1 * xc,
    1389                         yc * de, -yc * nu * de * de, p1 + s3 + 2 * s4 * d2, 2 * p2 * yc, nu * de + 2 * p2 * xc + 4 * p1 * yc });
    1390 
    1391                     Mat_<double> dhdpC({ 5, 2 }, {
    1392                         xc * (2 * k1 + 4 * k2 * d2 + 6 * k3 * d4), yc * (2 * k1 + 4 * k2 * d2 + 6 * k3 * d4),
    1393                         xc * (2 * k4 + 4 * k5 * d2 + 6 * k6 * d4), yc * (2 * k4 + 4 * k5 * d2 + 6 * k6 * d4),
    1394                         2 * xc, 2 * yc,
    1395                         1, 0,
    1396                         0, 1 });
    1397 
    1398                     Mat_<double> dpCdPC({ 2, 3 }, {
    1399                         iz, 0, -xc * iz,
    1400                         0, iz, -yc * iz });
    1401 
    1402                     if (!dpdt.empty()) dpdt.rowRange(2 * k, 2 * k + 2) = dpDdpCD * dpCDdh * dhdpC * dpCdPC;
    1403                     if (!dpdR.empty() && nRot == 9)
    1404                     {
    1405                         Mat_<double> dPCdR({ 3, 9 }, {
    1406                             X, Y, Z, 0, 0, 0, 0, 0, 0,
    1407                             0, 0, 0, X, Y, Z, 0, 0, 0,
    1408                             0, 0, 0, 0, 0, 0, X, Y, Z });
    1409                         dpdR.rowRange(2 * k, 2 * k + 2) = dpDdpCD * dpCDdh * dhdpC * dpCdPC * dPCdR;
    1410                     }
    1411                     if (!dpdR.empty() && nRot == 3)
    1412                     {
    1413                         Mat_<double> _skewPC({ 3, 3 }, {
    1414                             0, (z - tz), -(y - ty),
    1415                             -(z - tz), 0, (x - tx),
    1416                               (y - ty), -(x - tx), 0 });
    1417                         dpdR.rowRange(2 * k, 2 * k + 2) = dpDdpCD * dpCDdh * dhdpC * dpCdPC * _skewPC;
    1418                     }
    1419                 }
    1420                 if (!dpdD.empty())
    1421                 {
    1422                     Mat_<double> dpDdpCD(2, 2);
    1423                     dpDdpCD << fx, 0, 0, fy;
    1424 
    1425                     Mat_<double> dpCDdD(2, nDist);
    1426                     if (nDist < 5)
    1427                     {
    1428                         dpCDdD <<
    1429                             xc * d2 * de, xc* d4* de, xcyc, d2xc2,
    1430                             yc* d2* de, yc* d4* de, d2yc2, xcyc;
    1431                     }
    1432                     else if (nDist < 8)
    1433                     {
    1434                         dpCDdD <<
    1435                             xc * d2 * de, xc* d4* de, xcyc, d2xc2, xc* d6* de,
    1436                             yc* d2* de, yc* d4* de, d2yc2, xcyc, yc* d6* de;
    1437                     }
    1438                     else if (nDist < 12)
    1439                     {
    1440                         dpCDdD <<
    1441                             xc * d2 * de, xc* d4* de, xcyc, d2xc2, xc* d6* de, -xc * d2 * nu * de * de, -xc * d4 * nu * de * de, -xc * d6 * nu * de * de,
    1442                             yc* d2* de, yc* d4* de, d2yc2, xcyc, yc* d6* de, -yc * d2 * nu * de * de, -yc * d4 * nu * de * de, -yc * d6 * nu * de * de;
    1443                     }
    1444                     else if (nDist < 14)
    1445                     {
    1446                         dpCDdD <<
    1447                             xc * d2 * de, xc* d4* de, xcyc, d2xc2, xc* d6* de, -xc * d2 * nu * de * de, -xc * d4 * nu * de * de, -xc * d6 * nu * de * de, d2, d4, 0, 0,
    1448                             yc* d2* de, yc* d4* de, d2yc2, xcyc, yc* d6* de, -yc * d2 * nu * de * de, -yc * d4 * nu * de * de, -yc * d6 * nu * de * de, 0, 0, d2, d4;
    1449                     }
    1450                     dpdD.rowRange(2 * k, 2 * k + 2) = dpDdpCD * dpCDdD;
    1451                 }
    1452                 if (!dpdK.empty()) Mat_<double>({ 2, 4 }, { xd, 0, 1, 0, 0, yd, 0, 1 }).copyTo(dpdK.rowRange(2 * k, 2 * k + 2));
    1453             }
    1454 
    1455             return true;
    1456             if (nRot == 9)
    1457             {
    1458                 static const int nDist_ = 5; // set this value as nDist
    1459                 ceres::AutoDiffCostFunction<ProjectionModelKDRt, ceres::DYNAMIC, 9, 3, 4, nDist_> autoDeriv(new ProjectionModelKDRt(motionViewForDBG, nDist), motionViewForDBG.point3D.rows * 2);
    1460                 Mat_<double> dpdRDBG(point3D.rows * 2, 9);
    1461                 Mat_<double> dpdtDBG(point3D.rows * 2, 3);
    1462                 Mat_<double> dpdKDBG(point3D.rows * 2, 4);
    1463                 Mat_<double> dpdDDBG(point3D.rows * 2, nDist);
    1464                 Mat_<Vec2d> err2DDBG(point3D.rows, 1);
    1465                 const double* parametersDBG[4] = { rot, t, K, D };
    1466                 double* jacobiansDBG[4] = { dpdRDBG.ptr<double>(), dpdtDBG.ptr<double>(), dpdKDBG.ptr<double>(), dpdDDBG.ptr<double>() };
    1467                 double* residualsDBG = err2DDBG.ptr<double>();
    1468                 autoDeriv.Evaluate(parametersDBG, residualsDBG, jacobiansDBG);
    1469 
    1470                 if (!dpdR.empty()) cout << endl << norm(dpdR, dpdRDBG, NORM_INF) << endl;
    1471                 if (!dpdt.empty()) cout << endl << norm(dpdt, dpdtDBG, NORM_INF) << endl;
    1472                 if (!dpdK.empty()) cout << endl << norm(dpdK, dpdKDBG, NORM_INF) << endl;
    1473                 if (!dpdD.empty()) cout << endl << norm(dpdD, dpdDDBG, NORM_INF) << endl;
    1474                 cout << endl << norm(Mat_<Vec2d>(point2D.rows, 1, err2D), err2DDBG, NORM_INF) << endl;
    1475                 std::getchar();
    1476             }
    1477             return true;
    1478         }
    1479     };
    1480 };
    1481 
    1482 class OptimizeRtWithNormlizedPoint2D
    1483 {
    1484 public:
    1485     using MotionView = MotionSim::MotionView;
    1486     static void TestMe(int argc = 0, char** argv = 0)
    1487     {
    1488         int N = 99;
    1489         for (int k = 0; k < N; ++k)
    1490         {
    1491             //0.GenerateData
    1492             bool world2D = k % 2;
    1493             int rotMode = k % 7 + 1;
    1494             MotionSim motionSim(false);
    1495             motionSim.camFovX = 90;
    1496             motionSim.camFovY = 90;
    1497             motionSim.camRand = 10;
    1498             motionSim.nMinMotion = 16;//2
    1499             motionSim.nMaxMotion = 32;//4
    1500             motionSim.rndSeek = false;
    1501             motionSim.nCamDist = 5;
    1502             motionSim.runMotion(world2D, false, rotMode);
    1503             //motionSim.visMotion();
    1504             int rndInd = int(motionSim.motionViews.size() * cv::randu<double>());
    1505             Mat_<double> r0 = -motionSim.motionViews[rndInd].r;
    1506             Mat_<double> t0 = -motionSim.motionViews[rndInd].R.t() * motionSim.motionViews[rndInd].t;
    1507             const MotionView& motionView = motionSim.motionViews[rndInd];
    1508             double errRatio = 0.9;
    1509 
    1510             //1.CalcByCeresWithAutoDeriv
    1511             Mat_<double> R1; cv::Rodrigues(r0 * errRatio, R1);
    1512             Mat_<double> t1 = t0 * errRatio;
    1513             ceres::Problem problem1;
    1514             problem1.AddResidualBlock(new ceres::AutoDiffCostFunction<RotationModelRt, ceres::DYNAMIC, 9, 3>(
    1515                 new RotationModelRt(motionView), motionView.point3D.rows * 2), NULL, R1.ptr<double>(), t1.ptr<double>());
    1516             problem1.SetParameterization(R1.ptr<double>(), new OptimizeRt::LocalParamRWithGeneralJ);
    1517             ceres::Solver::Options options1;
    1518             ceres::Solver::Summary summary1;
    1519             ceres::Solve(options1, &problem1, &summary1);
    1520             Mat_<double> r1; cv::Rodrigues(R1, r1);
    1521             int nIter1 = (int)summary1.iterations.size();
    1522 
    1523             //2.CalcByCeresWithGeneralJ
    1524             Mat_<double> R2; cv::Rodrigues(r0 * errRatio, R2);
    1525             Mat_<double> t2 = t0 * errRatio;
    1526             ceres::Problem problem2;
    1527             problem2.AddResidualBlock(new RotationCostRt(motionView, motionSim.nCamDist, 9), NULL, R2.ptr<double>(), t2.ptr<double>());
    1528             problem2.SetParameterization(R2.ptr<double>(), new OptimizeRt::LocalParamRWithGeneralJ);
    1529             ceres::Solver::Options options2;
    1530             ceres::Solver::Summary summary2;
    1531             ceres::Solve(options2, &problem2, &summary2);
    1532             Mat_<double> r2; cv::Rodrigues(R2, r2);
    1533             int nIter2 = (int)summary2.iterations.size();
    1534 
    1535             //3.CalcByCeresWithGeneralJ
    1536             Mat_<double> r3 = r0 * errRatio;
    1537             Mat_<double> t3 = t0 * errRatio;
    1538             ceres::Problem problem3;
    1539             problem3.AddResidualBlock(new RotationCostRt(motionView, motionSim.nCamDist, 3), NULL, r3.ptr<double>(), t3.ptr<double>());
    1540             problem3.SetParameterization(r3.ptr<double>(), new OptimizeRt::LocalParamRWithIdentJ);
    1541             ceres::Solver::Options options3;
    1542             ceres::Solver::Summary summary3;
    1543             ceres::Solve(options3, &problem3, &summary3);
    1544             int nIter3 = (int)summary3.iterations.size();
    1545 
    1546             //4.AnalyzeError
    1547             double infr0r0 = norm(r0, r0 * errRatio, NORM_INF);
    1548             double infr0r1 = norm(r0, r1, NORM_INF);
    1549             double infr0r2 = norm(r0, r2, NORM_INF);
    1550             double infr0r3 = norm(r0, r3, NORM_INF);
    1551             double inft0t0 = norm(t0, t0 * errRatio, NORM_INF);
    1552             double inft0t1 = norm(t0, t1, NORM_INF);
    1553             double inft0t2 = norm(t0, t2, NORM_INF);
    1554             double inft0t3 = norm(t0, t3, NORM_INF);
    1555 
    1556             //5.PrintError
    1557             cout << fmt::format("LoopCount: {}      AutoDeriv.iters: {}      GeneralJ.iters: {}      IdentJ.iters: {}
    ", k, nIter1, nIter2, nIter3);
    1558             if (infr0r1 > 1e-8 || infr0r2 > 1e-8 || infr0r3 > 1e-8 || inft0t1 > 1e-8 || inft0t2 > 1e-8 || inft0t3 > 1e-8)
    1559             {
    1560                 cout << fmt::format("infr0r1: {:<15.9}		{:<15.9}
    ", infr0r1, infr0r0);
    1561                 cout << fmt::format("infr0r2: {:<15.9}		{:<15.9}
    ", infr0r2, infr0r0);
    1562                 cout << fmt::format("infr0r3: {:<15.9}		{:<15.9}
    ", infr0r3, infr0r0);
    1563                 cout << fmt::format("inft0t1: {:<15.9}		{:<15.9}
    ", inft0t1, inft0t0);
    1564                 cout << fmt::format("inft0t2: {:<15.9}		{:<15.9}
    ", inft0t2, inft0t0);
    1565                 cout << fmt::format("inft0t3: {:<15.9}		{:<15.9}
    ", inft0t3, inft0t0);
    1566                 cout << "Press any key to continue" << endl; std::getchar();
    1567             }
    1568         }
    1569     }
    1570 
    1571 public:
    1572     struct RotationModelRt
    1573     {
    1574         Mat_<Vec2d> point2D;
    1575         Mat_<Vec3d> point3D;
    1576         RotationModelRt(const MotionView& motionView0)
    1577         {
    1578             point3D = motionView0.point3D;
    1579             cv::projectPoints(point3D, -motionView0.r, -motionView0.R.t() * motionView0.t, Mat_<double>::eye(3, 3), Mat_<double>::zeros(motionView0.D.rows, 1), point2D);
    1580             //cv::undistortPoints(motionView0.point2D, point2D, motionView0.K, motionView0.D);
    1581         }
    1582         template <typename tp> bool operator()(const tp* const rot, const tp* const t, tp* errPoint2D) const
    1583         {
    1584             //1.Translation params
    1585             tp tx = t[0];
    1586             tp ty = t[1];
    1587             tp tz = t[2];
    1588 
    1589             //2.Rotation params
    1590             tp R11, R12, R13, R21, R22, R23, R31, R32, R33;
    1591             {
    1592                 R11 = rot[0]; R12 = rot[1]; R13 = rot[2];
    1593                 R21 = rot[3]; R22 = rot[4]; R23 = rot[5];
    1594                 R31 = rot[6]; R32 = rot[7]; R33 = rot[8];
    1595             }
    1596 
    1597             //3.ReProjection
    1598             const Vec2d* data2D = point2D.ptr<Vec2d>();
    1599             const Vec3d* data3D = point3D.ptr<Vec3d>();
    1600             Vec<tp, 2>* err2D = (Vec<tp, 2>*)errPoint2D;
    1601             for (int k = 0; k < point3D.rows; ++k)
    1602             {
    1603                 //3.1 WorldCoordinate
    1604                 double X = data3D[k][0];
    1605                 double Y = data3D[k][1];
    1606                 double Z = data3D[k][2];
    1607 
    1608                 //3.2 CameraCoordinate
    1609                 tp x = R11 * X + R12 * Y + R13 * Z + tx;
    1610                 tp y = R21 * X + R22 * Y + R23 * Z + ty;
    1611                 tp z = R31 * X + R32 * Y + R33 * Z + tz;
    1612 
    1613                 //3.3 StandardPhysicsCoordinate
    1614                 tp iz = 1. / z; //if denominator==0
    1615                 tp xc = x * iz;
    1616                 tp yc = y * iz;
    1617                 err2D[k][0] = xc - data2D[k][0];
    1618                 err2D[k][1] = yc - data2D[k][1];
    1619             }
    1620             return true;
    1621         }
    1622     };
    1623     struct RotationCostRt : public ceres::CostFunction
    1624     {
    1625         const int nDist;
    1626         double K[4];
    1627         const double* D;
    1628         Mat_<Vec2d> point2D;
    1629         Mat_<Vec3d> point3D;
    1630         const int nRot;
    1631         const MotionView& motionViewForDBG;//only use once
    1632         RotationCostRt(const MotionView& motionView0, const int nDist0, const int nRot0) : nDist(nDist0), nRot(nRot0), motionViewForDBG(motionView0)
    1633         {
    1634             K[0] = motionView0.K(0, 0);
    1635             K[1] = motionView0.K(1, 1);
    1636             K[2] = motionView0.K(0, 2);
    1637             K[3] = motionView0.K(1, 2);
    1638             D = motionView0.D.ptr<double>();
    1639             point2D = motionView0.point2D;
    1640             point3D = motionView0.point3D;
    1641             this->set_num_residuals(point2D.rows * 2);
    1642             this->mutable_parameter_block_sizes()->push_back(nRot);
    1643             this->mutable_parameter_block_sizes()->push_back(3);
    1644             cv::projectPoints(point3D, -motionView0.r, -motionView0.R.t() * motionView0.t, Mat_<double>::eye(3, 3), Mat_<double>::zeros(motionView0.D.rows, 1), point2D);
    1645             //cv::undistortPoints(motionView0.point2D, point2D, motionView0.K, motionView0.D);
    1646         }
    1647         bool Evaluate(double const* const* params, double* residuals, double** jacobians) const
    1648         {
    1649             //0.FormatInput
    1650             const double* rot = params[0];
    1651             const double* t = params[1];
    1652             Vec2d* err2D = (Vec2d*)(residuals);
    1653             Mat_<double> dpdR; if (jacobians && jacobians[0]) dpdR = Mat_<double>(point2D.rows * 2, nRot, jacobians[0]);
    1654             Mat_<double> dpdt; if (jacobians && jacobians[1]) dpdt = Mat_<double>(point2D.rows * 2, 3, jacobians[1]);
    1655 
    1656             //1.Translation params
    1657             double tx = t[0];
    1658             double ty = t[1];
    1659             double tz = t[2];
    1660 
    1661             //2.Rotation params
    1662             double R11, R12, R13, R21, R22, R23, R31, R32, R33;
    1663             if (nRot == 9)
    1664             {
    1665                 R11 = rot[0]; R12 = rot[1]; R13 = rot[2];
    1666                 R21 = rot[3]; R22 = rot[4]; R23 = rot[5];
    1667                 R31 = rot[6]; R32 = rot[7]; R33 = rot[8];
    1668             }
    1669             if (nRot == 3)
    1670             {
    1671                 Matx33d R; cv::Rodrigues(Matx31d(rot), R);
    1672                 rot = R.val;
    1673                 R11 = rot[0]; R12 = rot[1]; R13 = rot[2];
    1674                 R21 = rot[3]; R22 = rot[4]; R23 = rot[5];
    1675                 R31 = rot[6]; R32 = rot[7]; R33 = rot[8];
    1676                 rot = params[0];
    1677             }
    1678 
    1679             //3.ReProjection
    1680             const Vec2d* data2D = point2D.ptr<Vec2d>();
    1681             const Vec3d* data3D = point3D.ptr<Vec3d>();
    1682             for (int k = 0; k < point3D.rows; ++k)
    1683             {
    1684                 //5.1 WorldCoordinate
    1685                 double X = data3D[k][0];
    1686                 double Y = data3D[k][1];
    1687                 double Z = data3D[k][2];
    1688 
    1689                 //5.2 CameraCoordinate
    1690                 double x = R11 * X + R12 * Y + R13 * Z + tx;
    1691                 double y = R21 * X + R22 * Y + R23 * Z + ty;
    1692                 double z = R31 * X + R32 * Y + R33 * Z + tz;
    1693 
    1694                 //5.3 StandardPhysicsCoordinate
    1695                 double iz = 1. / z; //if denominator==0
    1696                 double xc = x * iz;
    1697                 double yc = y * iz;
    1698                 err2D[k][0] = xc - data2D[k][0];
    1699                 err2D[k][1] = yc - data2D[k][1];
    1700                 if (!dpdR.empty() || !dpdt.empty())
    1701                 {
    1702                     Mat_<double> dpCdPC({ 2, 3 }, {
    1703                         iz, 0, -xc * iz,
    1704                         0, iz, -yc * iz });
    1705                     if (!dpdt.empty()) dpCdPC.copyTo(dpdt.rowRange(2 * k, 2 * k + 2));
    1706                     if (!dpdR.empty() && nRot == 9)
    1707                     {
    1708                         Mat_<double> dPCdR({ 3, 9 }, {
    1709                             X, Y, Z, 0, 0, 0, 0, 0, 0,
    1710                             0, 0, 0, X, Y, Z, 0, 0, 0,
    1711                             0, 0, 0, 0, 0, 0, X, Y, Z });
    1712                         dpdR.rowRange(2 * k, 2 * k + 2) = dpCdPC * dPCdR;
    1713                     }
    1714                     if (!dpdR.empty() && nRot == 3)
    1715                     {
    1716                         Mat_<double> _skewPC({ 3, 3 }, {
    1717                             0, (z - tz), -(y - ty),
    1718                             -(z - tz), 0, (x - tx),
    1719                               (y - ty), -(x - tx), 0 });
    1720                         dpdR.rowRange(2 * k, 2 * k + 2) = dpCdPC * _skewPC;
    1721                     }
    1722                 }
    1723             }
    1724 
    1725             return true;
    1726             if (nRot == 9)
    1727             {
    1728                 ceres::AutoDiffCostFunction<RotationModelRt, ceres::DYNAMIC, 9, 3> autoDeriv(new RotationModelRt(motionViewForDBG), motionViewForDBG.point3D.rows * 2);
    1729                 Mat_<double> dpdRDBG(point3D.rows * 2, 9);
    1730                 Mat_<double> dpdtDBG(point3D.rows * 2, 3);
    1731                 Mat_<Vec2d> err2DDBG(point3D.rows, 1);
    1732                 const double* parametersDBG[2] = { rot, t };
    1733                 double* jacobiansDBG[2] = { dpdRDBG.ptr<double>(), dpdtDBG.ptr<double>() };
    1734                 double* residualsDBG = err2DDBG.ptr<double>();
    1735                 autoDeriv.Evaluate(parametersDBG, residualsDBG, jacobiansDBG);
    1736 
    1737                 if (!dpdR.empty()) cout << endl << norm(dpdR, dpdRDBG, NORM_INF) << endl;
    1738                 if (!dpdt.empty()) cout << endl << norm(dpdt, dpdtDBG, NORM_INF) << endl;
    1739                 cout << endl << norm(Mat_<Vec2d>(point2D.rows, 1, err2D), err2DDBG, NORM_INF) << endl;
    1740                 std::getchar();
    1741             }
    1742             return true;
    1743         }
    1744     };
    1745 };
    1746 
    1747 int main(int argc, char** argv) { OptimizeRtWithNormlizedPoint2D::TestMe(argc, argv); return 0; }
    1748 int main1(int argc, char** argv) { OptimizeRt::TestMe(argc, argv); return 0; }
    1749 int main2(int argc, char** argv) { OptimizeKDRt::TestMe(argc, argv); return 0; }
    1750 int main3(int argc, char** argv) { OptimizeRtWithNormlizedPoint2D::TestMe(argc, argv); return 0; }
    View Code
  • 相关阅读:
    WTL之CAppModule
    WTL之窗口子类化
    专业的日志系统该包含什么?
    ATL之什么是套间
    Java线程新特征之同步
    Java之用句柄操作对象
    Android之Application Fundamentals
    Android之Dev Guide
    一些思考
    WTL之窗口超类化(父类化)
  • 原文地址:https://www.cnblogs.com/dzyBK/p/13961868.html
Copyright © 2020-2023  润新知