CV学习日志：CV开发之视觉成像器

1.视觉成像模型发展历程
透视模型：于2000年，Zhang贡献(编码为RTCM=RadialTangentialCameraModel)。
开创理论：于2000年，Geyer和Daniilidis提出UCM成像模型，可用于建模平面、抛物、椭圆及双曲反射相机。
完善理论：于2001年，Barreto和Araujo优化UCM成像模型，从而简化了标定算法的开发。
理论向工程过渡：于2006年，Scaramuzza和Martinelli及Siegwart基于泰勒展开式提出用多项式来近似所有成像模型。
等距模型：于2006年，Kannala和Brandt贡献(编码为KBCM)。
开创工程：于2007年，Mei和Rives融合径向切向畸变到UCM成像模型(编码为RTUCM)，解决UCM建模鱼眼镜头差的问题。
优化工程：于2013年，Heng和Li及Pollefeys。
完善工程：于2016年，Khomutenko和Garcia及Martinet提出EUCM成像模型，解决了UCM径向畸变和建模鱼眼镜头差的问题及RTUCM非闭环解问题。
完善工程：于2018年，Usenko和Demmel及Cremers提出DSCM成像模型，相对于EUCM、RTUCM、KBCM等算法，在精度和速度上取得了折中。
***就工程实现和框架把握而言，2007年和2018年的这两篇论文最佳：
2007：Single view point omnidirectional camera calibration from planar grids
2018：The Double Sphere Camera Model
***成像理论基础阅读资料：
鱼眼相机成像、校准和拼接(笔记)
Models for the various classical lens projections
Computer Generated Angular Fisheye Projections
About the various projections of the photographic objective lenses
2.OpenCV中的标定算法
OpenCV实现了RTCM、KBCM、RTUCM。其中RTCM、KBCM认可度最高，实现在Main模块，其次是RTUCM，实现在Extra模块。
通过查看源码，RTUCM实现较粗糙，如calibrateCamera和stereoCalibrate对CALIB_USE_GUESS都无效，查看源码发现initializeStereoCalibration忽视了此值，直接重新进行单目标定。然而，即使initializeStereoCalibration不忽视，你会发现单目标定calibrate也忽视了此值。
3.本文要点说明
本文基于OpenCV、Ceres、Spdlog来阐述以下数学模型：
(1)成像模型：RTCM、KBCM、EUCM/RTEUCM、DSCM/RTDSCM，含所有模型的正向投影、逆向投影及偏导矩阵。
(2)模型转换：对EUCM/RTEUCM和DSCM/RTDSCM系列的UCM模型，含Pin形式、Mei形式及参数转换公式。
(3)几何分析：基于FOV从几何角度分析模型的成像过程，阐述成像模型中每个参数的几何物理意义。
(4)多视模型：双目模型、三目模型。
在projectPoints中针对旋转使用了四种求导方式：
(1)基于指数映射对旋转向量的加法导数，Ceres优化时不用LocalParameterization。
(2)基于BCH公式对旋转向量的加法导数，Ceres优化时不用LocalParameterization。
(3)基于李代数论对旋转向量的乘法导数，Ceres优化时重载LocalParameterization::Plus实现乘法增量的叠加。
(4)对四元数的加法导数，Ceres优化时重载LocalParameterization::ComputeJacobian实现四元数对旋转向量的导数(若ComputeJacobian是乘法导数还需要重载Plus实现乘法增量的叠加)。
(5)对旋转矩阵的加法导数，Ceres优化时重载LocalParameterization::ComputeJacobian实现旋转矩阵对旋转向量的导数(若ComputeJacobian是乘法导数还需要重载Plus实现乘法增量的叠加)。
以上五种求导方式，计算量依次先增后减，中间者计算量最小。因此，直接对旋转向量的乘法导数是视觉定位中最常使用的。
4.实验测试代码
依赖于OpenCV、Ceres和Spdlog，封装在类ModelCamera：
(1)VisionCM+Mei2Pin、CalibSolid+CamObservation、TestRTCM、TestKBCM、TestRTEUCM(仅四畸变的UCM)、、、、
(2)reprojectPoints、TestInvRTCM、TestInvKBCM、TestInvRTEUCM(仅四畸变的UCM)、、、、
(3)calcJacobians、calcJacobiansOfRTCM、、、、TestJacobianRTCM
(4)rectBino、rectTrino、TestRectBino、testRectTrino
   1 #include <ceres/ModelRotation.h>
   2 #include <opencv2/ccalib/omnidir.hpp>
   3 
   4 class ModelCamera
   5 {
   6 public:
   7     struct VisionCM
   8     {
   9         enum { RTCM, KBCM, RTEUCM, RTDSCM };
  10         int nRot;//3 4 9 and for rvec quat rmat respectively
  11         int nDist;//0 2 4 5 8 12 and echo AutoDiffCostFunction template parameter number except for 0. 0 uses overloaded operator.
  12         int nModel;//0 1 2 and echo AutoDiffCostFunction template parameter number except for 0. 0 uses overloaded operator.
  13         int camModel;//connect model with distortion
  14         bool simPin;//valid only for RTUCM RTEUCM RTDSCM
  15         Mat_<Vec3d> point3D;
  16         void inline checkInput()
  17         {
  18             if (camModel == RTCM && nModel != 0) assert(0);
  19             if (camModel == KBCM && nModel != 0) assert(0);
  20             if (camModel == RTEUCM && nModel != 2) assert(0);
  21             if (camModel == RTDSCM && nModel != 2) assert(0);
  22         }
  23         VisionCM(float* data3D, int nPoint, int nRot0 = 9, int nDist0 = 4, int nModel0 = 1, int camModel0 = RTCM, bool simPin0 = false) : nRot(nRot0), nDist(nDist0), nModel(nModel0), camModel(camModel0), simPin(simPin0) { checkInput(); Mat_<Vec3f>(nPoint, 1, (Vec3f*)data3D).copyTo(point3D); }
  24         VisionCM(double* data3D, int nPoint, int nRot0 = 9, int nDist0 = 4, int nModel0 = 1, int camModel0 = RTCM, bool simPin0 = false) : nRot(nRot0), nDist(nDist0), nModel(nModel0), camModel(camModel0), simPin(simPin0) { checkInput(); Mat_<Vec3d>(nPoint, 1, (Vec3d*)data3D).copyTo(point3D); }
  25         template <typename tp> bool operator()(const tp* const rot, const tp* const t, const tp* const K, tp* data2D0) const { return (*this)(rot, t, K, (tp*)0, (tp*)0, data2D0); }//fix D=0 and a=0 and b=1 if using full format
  26         template <typename tp> bool operator()(const tp* const rot, const tp* const t, const tp* const K, const tp* const DorAB, tp* data2D0) const { return (nModel == 0 ? (*this)(rot, t, K, DorAB, (tp*)0, data2D0) : (*this)(rot, t, K, (tp*)0, DorAB, data2D0)); }//fix D=0 or a=0 or b=1 if using full format
  27         template <typename tp> bool operator()(const tp* const rot, const tp* const t, const tp* const K, const tp* const D, const tp* const ab, tp* data2D0) const
  28         {
  29             //1.Projection params
  30             tp fx = K[0];
  31             tp fy = K[1];
  32             tp cx = K[2];
  33             tp cy = K[3];
  34             tp a; if (nModel > 0) a = ab[0];
  35             tp b; if (nModel > 1) b = ab[1];
  36 
  37             //2.Distortion params
  38             tp k1, k2, p1, p2, k3, k4, k5, k6, s1, s2, s3, s4;
  39             if (nDist > 1) { k1 = D[0]; k2 = D[1]; }
  40             if (nDist > 2) { p1 = D[2]; p2 = D[3]; } //k1=k1 k2=k2 p1=k3 p2=k4 for KB
  41             if (nDist > 4) k3 = D[4];
  42             if (nDist > 5) { k4 = D[5]; k5 = D[6]; k6 = D[7]; }
  43             if (nDist > 8) { s1 = D[8]; s2 = D[9]; s3 = D[10]; s4 = D[11]; }
  44 
  45             //3.Rotation params
  46             tp R[9]; if (nRot == 9) memcpy(R, rot, sizeof(R));
  47             else if (nRot == 4) ceres::QuaternionToRotation(rot, R);
  48             else if (nRot == 3) ceres::AngleAxisToRotationMatrix(rot, ceres::RowMajorAdapter3x3(R));
  49             else spdlog::critical("Invalid rotation configuration");
  50 
  51             //4.Projection actions
  52             Vec<tp, 2>* data2D = (Vec<tp, 2>*)data2D0;
  53             const Vec3d* data3D = point3D.ptr<Vec3d>();
  54             for (int k = 0; k < point3D.rows; ++k)
  55             {
  56                 //4.1 WorldPoint
  57                 double X = data3D[k][0];
  58                 double Y = data3D[k][1];
  59                 double Z = data3D[k][2];
  60 
  61                 //4.2 CameraPoint
  62                 tp x = R[0] * X + R[1] * Y + R[2] * Z + t[0];
  63                 tp y = R[3] * X + R[4] * Y + R[5] * Z + t[1];
  64                 tp z = R[6] * X + R[7] * Y + R[8] * Z + t[2];
  65 
  66                 //4.3 StandardPhysicsPoint
  67                 tp iz; if (camModel == RTCM || camModel == KBCM) iz = 1. / z;//Refraction model
  68                 else if (camModel == RTEUCM)
  69                 {
  70                     tp d = sqrt(b * (x * x + y * y) + z * z);
  71                     iz = simPin ? 1. / (a * d + (1. - a) * z) : 1. / (a * d + z);
  72                 }
  73                 else if (camModel == RTDSCM)
  74                 {
  75                     tp d1 = sqrt(x * x + y * y + z * z);
  76                     tp d2 = sqrt(x * x + y * y + (b * d1 + z) * (b * d1 + z));
  77                     iz = simPin ? 1. / (a * d2 + (1. - a) * (b * d1 + z)) : 1. / (a * d2 + (b * d1 + z));
  78                 }
  79                 else spdlog::critical("Invalid model configuration");
  80                 tp xn = x * iz;
  81                 tp yn = y * iz;
  82 
  83                 //4.4 DistortedPhysicsPoint
  84                 tp xd, yd;
  85                 if (camModel == KBCM)//KBDistortion
  86                 {
  87                     if (nDist == 0)
  88                     {
  89                         tp r = sqrt(xn * xn + yn * yn);
  90                         tp ir = 1. / r;
  91                         tp theta = atan(r);
  92                         xd = xn * ir * theta;
  93                         yd = yn * ir * theta;
  94                     }
  95                     else
  96                     {
  97                         tp r = sqrt(xn * xn + yn * yn);
  98                         tp ir = 1. / r;
  99                         tp theta = atan(r);//same as opencv which only focuses on 0~180 fov
 100                         tp theta2 = theta * theta;
 101                         tp theta4 = theta2 * theta2;
 102                         tp theta6 = theta2 * theta4;
 103                         tp theta8 = theta2 * theta6;
 104                         tp thetad = theta * (1. + k1 * theta2 + k2 * theta4 + p1 * theta6 + p2 * theta8); //p1=k3 p2=k4 here
 105                         xd = xn * ir * thetad;
 106                         yd = yn * ir * thetad;
 107                     }
 108                 }
 109                 else //RTDistortion
 110                 {
 111                     if (nDist == 0) { xd = xn; yd = yn; }
 112                     else
 113                     {
 114                         tp xn2, yn2, r2, r4, r6;
 115                         tp xnyn, r2xn2, r2yn2;
 116                         tp an, bn;
 117                         if (nDist > 1)
 118                         {
 119                             xn2 = xn * xn;
 120                             yn2 = yn * yn;
 121                             r2 = xn2 + yn2;
 122                             r4 = r2 * r2;
 123                             r6 = r2 * r4;
 124                         }
 125                         if (nDist > 2)
 126                         {
 127                             xnyn = 2. * xn * yn;
 128                             r2xn2 = r2 + 2. * xn2;
 129                             r2yn2 = r2 + 2. * yn2;
 130                         }
 131                         if (nDist == 2)
 132                         {
 133                             an = 1. + k1 * r2 + k2 * r4;
 134                             xd = xn * an;
 135                             yd = yn * an;
 136                         }
 137                         else if (nDist == 4)
 138                         {
 139                             an = 1. + k1 * r2 + k2 * r4;
 140                             xd = xn * an + p1 * xnyn + p2 * r2xn2;
 141                             yd = yn * an + p2 * xnyn + p1 * r2yn2;
 142                         }
 143                         else if (nDist == 5)
 144                         {
 145                             an = 1. + k1 * r2 + k2 * r4 + k3 * r6;
 146                             xd = xn * an + p1 * xnyn + p2 * r2xn2;
 147                             yd = yn * an + p2 * xnyn + p1 * r2yn2;
 148                         }
 149                         else if (nDist == 8)
 150                         {
 151                             an = 1. + k1 * r2 + k2 * r4 + k3 * r6;
 152                             bn = 1. / (1. + k4 * r2 + k5 * r4 + k6 * r6);//if denominator==0
 153                             xd = xn * an * bn + p1 * xnyn + p2 * r2xn2;
 154                             yd = yn * an * bn + p2 * xnyn + p1 * r2yn2;
 155                         }
 156                         else if (nDist == 12)
 157                         {
 158                             an = 1. + k1 * r2 + k2 * r4 + k3 * r6;
 159                             bn = 1. / (1. + k4 * r2 + k5 * r4 + k6 * r6);//if denominator==0
 160                             xd = xn * an * bn + p1 * xnyn + p2 * r2xn2 + s1 * r2 + s2 * r4;
 161                             yd = yn * an * bn + p2 * xnyn + p1 * r2yn2 + s3 * r2 + s4 * r4;
 162                         }
 163                         else spdlog::critical("Invalid distortion configuration");
 164                     }
 165                 }
 166 
 167                 //4.5 DistortedPixelPoint
 168                 data2D[k][0] = xd * fx + cx;
 169                 data2D[k][1] = yd * fy + cy;
 170             }
 171             return true;
 172         }
 173         static double Mei2Pin(double a, double* fx = 0, double* fy = 0, double* s = 0, double* D = 0, int nDist = 4, bool forward = true)
 174         {
 175             a = forward ? a / (1 + a) : a / (1 - a);
 176             double ratio1 = forward ? 1 - a : 1 + a;
 177             double ratio2 = ratio1 * ratio1;
 178             double ratio3 = ratio1 * ratio2;
 179             double ratio4 = ratio1 * ratio3;
 180             double ratio6 = ratio3 * ratio3;
 181             if (fx) fx[0] *= ratio1;
 182             if (fy) fy[0] *= ratio1;
 183             if (s) s[0] = ratio1 * s[0];
 184             if (nDist > 0) D[0] *= ratio2;//k1
 185             if (nDist > 1) D[1] *= ratio4;//k2
 186             if (nDist > 2) D[2] *= ratio1;//p1
 187             if (nDist > 3) D[3] *= ratio1;//p2
 188             if (nDist > 4) D[4] *= ratio6;//k3
 189             if (nDist > 5) D[5] *= ratio2;//k4
 190             if (nDist > 6) D[6] *= ratio4;//k5
 191             if (nDist > 7) D[7] *= ratio6;//k6
 192             if (nDist > 8) D[8] *= ratio1;//s1
 193             if (nDist > 9) D[9] *= ratio3;//s2
 194             if (nDist > 10) D[10] *= ratio1;//s3
 195             if (nDist > 11) D[11] *= ratio3;//s4
 196             return a;
 197         }
 198     };
 199 
 200     struct CamSolid
 201     {
 202         short nDist;//same as VisionCM
 203         short nModel;//same as VisionCM
 204         short camModel;//same as VisionCM
 205         short simPin;//same as VisionCM
 206         double K[4] = { 0 };//not use skew param
 207         double D[12] = { 0 };//new max memory
 208         double ab[2] = { 0 };//new max memory
 209         inline double* data() { return (double*)(this); }//convenient for IO
 210         double limitFOV()
 211         {
 212             double rad2deg = 180 / 3.1415926535897932384626433832795;
 213             double A = simPin ? ab[0] / (1 - ab[0]) : ab[0];
 214             if (camModel == VisionCM::RTCM) return 180;
 215             else if (camModel == VisionCM::KBCM) return 360;
 216             else if (camModel == VisionCM::RTEUCM) return 2 * atan2(sqrt(A * A - 1), -sqrt(ab[1])) * rad2deg;//can be simplified as 2 * acos(-1/A) for b=1
 217             else if (camModel == VisionCM::RTDSCM) return 2 * acos(-1 / A) * rad2deg;
 218             return 0;
 219         }
 220         Vec6d idealFOV(int rows, int cols)
 221         {
 222             double hfov1 = 0, hfov2 = 0, vfov1 = 0, vfov2 = 0;
 223             if (camModel == VisionCM::RTCM)//geometric inference
 224             {
 225                 hfov1 = atan(K[2] / K[0]);
 226                 hfov2 = atan((cols - K[2]) / K[0]);
 227                 vfov1 = atan(K[3] / K[1]);
 228                 vfov2 = atan((rows - K[3]) / K[1]);
 229             }
 230             else if (camModel == VisionCM::KBCM)//geometric inference
 231             {
 232                 hfov1 = K[2] / K[0];
 233                 hfov2 = (cols - K[2]) / K[0];
 234                 vfov1 = K[3] / K[1];
 235                 vfov2 = (rows - K[3]) / K[1];
 236             }
 237             else if (camModel == VisionCM::RTEUCM)//geometric inference
 238             {
 239                 auto CalcHalfFOV = [](double F, double c, double A, double b)->double
 240                 {
 241                     const double pi = 3.1415926535897932384626433832795;
 242                     double B = A * c / F;
 243                     double Y = A * (b * B * B - sqrt(A * A - b * A * A * B * B + b * B * B));
 244                     double X = B * (A * A + sqrt(A * A - b * A * A * B * B + b * B * B));
 245                     if (0)
 246                     {
 247                         Y = b * A * A * c * c - F * sqrt(A * A * F * F - b * A * A * A * A * c * c + b * A * A * c * c);
 248                         X = A * A * F * c + c * sqrt(A * A * F * F - b * A * A * A * A * c * c + b * A * A * c * c);
 249                     }
 250                     return atan2(1, -Y / X);
 251                 };
 252                 auto CalcHalfFOV2 = [](double F, double c, double A, double b)->double//intersection angle formula
 253                 {
 254                     double ro = c / F;
 255                     Vec3d m(0, 0, 1 / (1 + A));
 256                     Vec3d n(0, ro, (1 - A * A * b * ro * ro) / (1 + A * sqrt(1 + (1 - A * A) * b * ro * ro)));
 257                     double theta = acos(m.dot(n) / sqrt(m.dot(m)) / sqrt(n.dot(n)));
 258                     return theta;
 259                 };
 260                 if (0)
 261                 {
 262                     vector<double> As(99), Fs(99), cs(99), bs(99); cv::randu(As, 0.1, 1.9);
 263                     cv::randu(Fs, 800, 1200); cv::randu(cs, 400, 600); cv::randu(bs, 0.1, 1.9);
 264                     for (int k = 0; k < As.size(); ++k)
 265                     {
 266                         const double rad2deg = 180 / 3.1415926535897932384626433832795;
 267                         double ang1 = CalcHalfFOV(Fs[k], cs[k], As[k], bs[k]) * rad2deg;
 268                         double ang2 = CalcHalfFOV2(Fs[k], cs[k], As[k], bs[k]) * rad2deg;
 269                         cout << endl << fmt::format("A1-A2: {} - {} = {:.6f}", ang1, ang2, ang1 - ang2) << endl;
 270                     }
 271                 }
 272                 hfov1 = CalcHalfFOV(simPin ? K[0] / (1 - ab[0]) : K[0], K[2], simPin ? ab[0] / (1 - ab[0]) : ab[0], ab[1]);
 273                 hfov2 = CalcHalfFOV(simPin ? K[0] / (1 - ab[0]) : K[0], cols - K[2], simPin ? ab[0] / (1 - ab[0]) : ab[0], ab[1]);
 274                 vfov1 = CalcHalfFOV(simPin ? K[1] / (1 - ab[0]) : K[1], K[3], simPin ? ab[0] / (1 - ab[0]) : ab[0], ab[1]);
 275                 vfov2 = CalcHalfFOV(simPin ? K[1] / (1 - ab[0]) : K[1], rows - K[3], simPin ? ab[0] / (1 - ab[0]) : ab[0], ab[1]);
 276             }
 277             else if (camModel == VisionCM::RTDSCM)//intersection angle formula
 278             {
 279                 auto CalcHalfFOV = [](double F, double c, double A, double b)->double
 280                 {
 281                     double ro = c / F;
 282                     double zo = (1 - A * A * ro * ro) / (1 + A * sqrt(1 + (1 - A * A) * ro * ro));
 283                     double wo = (b * zo + sqrt(zo * zo + (1 - b * b) * ro * ro)) / (zo * zo + ro * ro);
 284                     Vec3d m(0, 0, 1);
 285                     Vec3d n(0, wo * ro, wo * zo - b);
 286                     double theta = acos(m.dot(n) / sqrt(m.dot(m)) / sqrt(n.dot(n)));
 287                     return theta;
 288                 };
 289                 hfov1 = CalcHalfFOV(simPin ? K[0] / (1 - ab[0]) : K[0], K[2], simPin ? ab[0] / (1 - ab[0]) : ab[0], ab[1]);
 290                 hfov2 = CalcHalfFOV(simPin ? K[0] / (1 - ab[0]) : K[0], cols - K[2], simPin ? ab[0] / (1 - ab[0]) : ab[0], ab[1]);
 291                 vfov1 = CalcHalfFOV(simPin ? K[1] / (1 - ab[0]) : K[1], K[3], simPin ? ab[0] / (1 - ab[0]) : ab[0], ab[1]);
 292                 vfov2 = CalcHalfFOV(simPin ? K[1] / (1 - ab[0]) : K[1], rows - K[3], simPin ? ab[0] / (1 - ab[0]) : ab[0], ab[1]);
 293             }
 294             return Vec6d(hfov1, hfov2, hfov1 + hfov2, vfov1, vfov2, vfov1 + vfov2) * (180 / 3.1415926535897932384626433832795);
 295         }
 296         string print(int rows = 0, int cols = 0)
 297         {
 298             string str;
 299             str += fmt::format("M: [ {}, {}, {}, {} ]
", nDist, nModel, camModel, simPin);
 300             str += fmt::format("K: [ {}, {}, {}, {} ]
", K[0], K[1], K[2], K[3]);
 301             str += fmt::format("D: [ {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {} ]
", D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8], D[9], D[10], D[11]);
 302             str += fmt::format("ab: [ {}, {} ]
", ab[0], ab[1]);
 303             Vec6d fov; if (rows != 0 && cols != 0) fov = idealFOV(rows, cols);
 304             str += fmt::format("FOV: [ {}, {}, {}, {}, {}, {}, {} ]
", fov[0], fov[1], fov[2], fov[3], fov[4], fov[5], limitFOV());
 305             return str;
 306         }
 307         //const double pi = 3.1415926535897932384626433832795;
 308         //const double deg2rad = pi / 180;
 309         //const double rad2deg = 180 / pi;
 310         //inline double ellip2DLen(double r1, double r2) { return r1 > r2 ? 2 * pi * r2 + 4 * (r1 - r2) : 2 * pi * r1 + 4 * (r2 - r1); }
 311         //inline double ellip2DArea(double r1, double r2) { return 4 * pi * r1 * r2; }
 312         //inline double ellip3DArea(double r1, double r2, double r3) { double p = 1.6075; return (r1 == r2 && r2 == r3) ? 4 * pi * r1 * r1 : 4 * pi * pow((pow(r1 * r2, p) + pow(r1 * r3, p) + pow(r2 * r3, p)) / 3., 1. / p); }
 313         //inline double ellip3DVolume(double r1, double r2, double r3) { return 4. / 3. * pi * r1 * r2 * r3; }
 314     };
 315 
 316     using SolidPose = ModelRotation::SolidPose;
 317 
 318     struct CamObservation
 319     {
 320         vector<SolidPose> poses;
 321         vector<vector<Point3f>> point3D;
 322         vector<vector<Point2f>> point2D;
 323         vector<bool> oksView;//can be used for calibration
 324         vector<vector<uint>> ids3D; //can be used for location and calibration
 325         void clear() { poses.clear(); point3D.clear(); point2D.clear(); oksView.clear(); ids3D.clear(); }
 326     };
 327 
 328 public:
 329     static Mat_<Vec2d> reprojectPoints(Mat_<Vec2d> point2D, double* K, double* D, double* ab, int nDist, int nModel, int camModel, bool simPin, double* R = 0, double* P = 0,
 330         Mat_<Vec3d>* point3DDir = 0, Mat_<Vec3d>* point3DCir = 0, TermCriteria criteria = TermCriteria(TermCriteria::COUNT, 5, 0.01)) //criteria is fixed as (10, 1e-8) in opencv4.5 KB
 331     {
 332         //1.Projection params
 333         double fx = K[0], ifx = 1. / fx;
 334         double fy = K[1], ify = 1. / fy;
 335         double cx = K[2], icx = -ifx * cx;
 336         double cy = K[3], icy = -ify * cy;
 337         double a = 0; if (nModel > 0) a = ab[0];
 338         double b = 1; if (nModel > 1) b = ab[1];
 339 
 340         //2.Distortion params
 341         double k1 = 0, k2 = 0, p1 = 0, p2 = 0;
 342         double k3 = 0, k4 = 0, k5 = 0, k6 = 0;
 343         double s1 = 0, s2 = 0, s3 = 0, s4 = 0;
 344         if (nDist > 0) { k1 = D[0]; k2 = D[1]; }
 345         if (nDist > 2) { p1 = D[2]; p2 = D[3]; }
 346         if (nDist > 4) k3 = D[4];
 347         if (nDist > 5) { k4 = D[5]; k5 = D[6]; k6 = D[7]; }
 348         if (nDist > 8) { s1 = D[8]; s2 = D[9]; s3 = D[10]; s4 = D[11]; }
 349 
 350         //3.Reprojection matrix
 351         Matx33d A = A.eye();
 352         if (P != 0) A = A * Matx33d(P[0], 0, P[2], 0, P[1], P[3], 0, 0, 1);
 353         if (R != 0) A = A * Matx33d(R);
 354 
 355         //4.Reprojection actions
 356         Mat_<Vec2d> point2DNorm(point2D.rows, 1);
 357         if (point3DDir != 0 && point3DDir->size() != point2D.size()) point3DDir->create(point2D.size());
 358         if (point3DCir != 0 && point3DCir->size() != point2D.size()) point3DCir->create(point2D.size());
 359         for (int k = 0; k < point2D.rows; ++k)
 360         {
 361             //4.1 DistortedPixelPoint-->DistortedPhysicsPoint
 362             double u = point2D(k)[0], xd = (u - cx) * ifx;
 363             double v = point2D(k)[1], yd = (v - cy) * ify;
 364 
 365             //4.2 DistortedPhysics-->StandardPhysicsPoint
 366             double xn = xd, yn = yd;
 367             if (camModel == VisionCM::KBCM)
 368             {
 369                 if (nDist == 0)
 370                 {
 371 
 372                 }
 373                 else
 374                 {
 375                     double thetad = sqrt(xd * xd + yd * yd), halfPI = 3.1415926535897932384626433832795 / 2.;
 376                     thetad = thetad > halfPI ? halfPI : thetad < -halfPI ? -halfPI : thetad;//same as opencv which only focuses on 0~180 fov
 377                     double thetan = thetad;//it not conflict with projection using thetan
 378                     int i = 0;
 379                     for (; i < criteria.maxCount; ++i)
 380                     {
 381                         double thetan2 = thetan * thetan, k1thetan2 = k1 * thetan2;
 382                         double thetan4 = thetan2 * thetan2, k2thetan4 = k2 * thetan4;
 383                         double thetan6 = thetan2 * thetan4, k3thetan6 = p1 * thetan6;
 384                         double thetan8 = thetan2 * thetan6, k4thetan8 = p2 * thetan8; //p1=k3 p2=k4 here
 385                         double deltaTheta = (thetan * (1 + k1thetan2 + k2thetan4 + k3thetan6 + k4thetan8) - thetad) / (1 + 3 * k1thetan2 + 5 * k2thetan4 + 7 * k3thetan6 + 9 * k4thetan8);
 386                         thetan -= deltaTheta;
 387                         if ((criteria.type & TermCriteria::EPS) && abs(deltaTheta) < criteria.epsilon) break;
 388                     }
 389                     if (i >= criteria.maxCount || thetad * thetan < 0) //same as opencv4.5 for consistence test and can be removed
 390                     {
 391                         point2DNorm(k) = Vec2d(-1000000.0, -1000000.0);
 392                         if (point3DDir) (*point3DDir)(k) = Vec3d(0, 0, 0);
 393                         if (point3DCir) (*point3DCir)(k) = Vec3d(0, 0, 0);
 394                         continue;
 395                     }
 396                     double scaled = tan(thetan) / thetad;
 397                     xn *= scaled;
 398                     yn *= scaled;
 399                 }
 400             }
 401             else
 402             {
 403                 if (nDist == 0) {/*nothing to do*/ }
 404                 else
 405                 {
 406                     for (int i = 0; i < criteria.maxCount; ++i)
 407                     {
 408                         double rn2 = xn * xn + yn * yn;//it not conflict with projection using rn2 anbn deltaX deltaY
 409                         double anbn = (1 + ((k6 * rn2 + k5) * rn2 + k4) * rn2) / (1 + ((k3 * rn2 + k2) * rn2 + k1) * rn2);
 410                         if (anbn < 0) { xn = (u - cx) * ifx; yn = (v - cy) * ify; break; } // test: undistortPoints.regression_14583
 411                         double deltaX = 2 * p1 * xn * yn + p2 * (rn2 + 2 * xn * xn) + s1 * rn2 + s2 * rn2 * rn2;
 412                         double deltaY = p1 * (rn2 + 2 * yn * yn) + 2 * p2 * xn * yn + s3 * rn2 + s4 * rn2 * rn2;
 413                         xn = (xd - deltaX) * anbn;
 414                         yn = (yd - deltaY) * anbn;
 415 
 416                         if (criteria.type & TermCriteria::EPS)
 417                         {
 418                             double xn2 = xn * xn;
 419                             double yn2 = yn * yn;
 420                             double r2 = xn2 + yn2;
 421                             double xnyn = 2 * xn * yn;
 422                             double r4 = r2 * r2;
 423                             double r6 = r4 * r2;
 424                             double r2xn2 = r2 + 2 * xn2;
 425                             double r2yn2 = r2 + 2 * yn2;
 426                             double an = 1 + k1 * r2 + k2 * r4 + k3 * r6;
 427                             double bn = 1. / (1 + k4 * r2 + k5 * r4 + k6 * r6);
 428                             double xdd = xn * an * bn + p1 * xnyn + p2 * r2xn2 + s1 * r2 + s2 * r4;
 429                             double ydd = yn * an * bn + p1 * r2yn2 + p2 * xnyn + s3 * r2 + s4 * r4;
 430                             double uerr = xdd * fx + cx - u;
 431                             double verr = ydd * fy + cy - v;
 432                             if (sqrt(uerr * uerr + verr * verr) < criteria.epsilon) break;
 433                         }
 434                     }
 435                 }
 436             }
 437 
 438             //4.3 StandardPhysicsPoint-->StandardPixelPoint
 439             double rn2 = xn * xn + yn * yn;
 440             double xc = xn, yc = yn, zc = (1 - a * a * b * rn2) / (1 + a * sqrt(1 + (1 - a * a) * b * rn2));
 441             double is = 1. / (A.val[6] * xc + A.val[7] * yc + A.val[8] * zc);
 442             point2DNorm(k)[0] = (A.val[0] * xc + A.val[1] * yc + A.val[2] * zc) * is;
 443             point2DNorm(k)[1] = (A.val[3] * xc + A.val[4] * yc + A.val[5] * zc) * is;
 444 
 445             //4.4 StandardPhysicsPoint-->ScaledCameraPoint
 446             if (point3DDir || point3DCir)
 447             {
 448                 double xcc = xc, ycc = yc, zcc = zc;
 449                 if (R)
 450                 {
 451                     xcc = xc * R[0] + yc * R[1] + zc * R[2];
 452                     ycc = xc * R[3] + yc * R[4] + zc * R[5];
 453                     zcc = xc * R[6] + yc * R[7] + zc * R[8];
 454                 }
 455                 if (point3DDir != 0) (*point3DDir)(k) = Vec3d(xcc, ycc, zcc);
 456                 if (point3DCir != 0) { double ratio = 1. / sqrt(xcc * xcc + ycc * ycc + zcc * zcc); (*point3DCir)(k) = Vec3d(xcc * ratio, ycc * ratio, zcc * ratio); }
 457             }
 458         }
 459         return point2DNorm;
 460     }
 461 
 462 public:
 463     static Mat_<Vec2d> calcJacobians(Mat_<Vec3d> point3D, double* r, double* t, double* K, double* D, int nDist, int nModel, int idDist, int idModel, bool simPin, Mat_<double>& jacobians) { return Mat_<Vec2d>(); }
 464 
 465     static Mat_<Vec2d> calcJacobiansOfRTCM(Mat_<Vec3d> point3D, double* r, double* t, double* K, double* D, int nDist,
 466         Mat_<double>* dpdr = 0, Mat_<double>* dpdt = 0, Mat_<double>* dpdK = 0, Mat_<double>* dpdD = 0, Mat_<double>* dpdP = 0,
 467         int derivRotWay = 0)
 468     {
 469         //0.FormatInput
 470         if (dpdK) dpdK->release();
 471         if (dpdD) dpdD->release();
 472         if (dpdt) dpdt->release();
 473         if (dpdr) dpdr->release();
 474         if (dpdP) dpdP->release();
 475         Mat_<Vec2d> point2D(point3D.size());
 476 
 477         //1.Projection params
 478         double fx = K[0];
 479         double fy = K[1];
 480         double cx = K[2];
 481         double cy = K[3];
 482 
 483         //2.Distortion params
 484         double k1 = D[0];
 485         double k2 = D[1];
 486         double p1 = D[2];
 487         double p2 = D[3];
 488         double k3 = 0, k4 = 0, k5 = 0, k6 = 0;
 489         double s1 = 0, s2 = 0, s3 = 0, s4 = 0;
 490         if (nDist > 4) k3 = D[4];
 491         if (nDist > 5) { k4 = D[5]; k5 = D[6]; k6 = D[7]; }
 492         if (nDist > 8) { s1 = D[8]; s2 = D[9]; s3 = D[10]; s4 = D[11]; }
 493 
 494         //3.Rotation params
 495         double R[9];
 496         Mat_<double> dRdr;
 497         cv::Rodrigues(Mat_<double>(3, 1, r), Mat_<double>(3, 3, R), (dpdr && derivRotWay == 0) ? dRdr : noArray());
 498         if (dRdr.empty() == false) dRdr = dRdr.t();
 499 
 500         //4.Projection actions
 501         Vec2d* data2D = point2D.ptr<Vec2d>();
 502         Vec3d* data3D = point3D.ptr<Vec3d>();
 503         for (int k = 0; k < point3D.rows; ++k)
 504         {
 505             //4.1 WorldPoint
 506             double X = data3D[k][0];
 507             double Y = data3D[k][1];
 508             double Z = data3D[k][2];
 509 
 510             //4.2 CameraPoint
 511             double x = R[0] * X + R[1] * Y + R[2] * Z + t[0];
 512             double y = R[3] * X + R[4] * Y + R[5] * Z + t[1];
 513             double z = R[6] * X + R[7] * Y + R[8] * Z + t[2];
 514 
 515             //4.3 StandardPhysicsPoint
 516             double iz = z ? 1. / z : 1;
 517             double xn = x * iz;
 518             double yn = y * iz;
 519 
 520             //4.4 DistortedPhysicsPoint
 521             double xn2 = xn * xn;
 522             double yn2 = yn * yn;
 523             double r2 = xn2 + yn2;
 524             double xnyn = 2 * xn * yn;
 525             double r4 = r2 * r2;
 526             double r6 = r2 * r4;
 527             double r2xn2 = r2 + 2 * xn2;
 528             double r2yn2 = r2 + 2 * yn2;
 529             double an = 1 + k1 * r2 + k2 * r4 + k3 * r6;
 530             double bn = 1. / (1 + k4 * r2 + k5 * r4 + k6 * r6);
 531             double xd = xn * an * bn + p1 * xnyn + p2 * r2xn2 + s1 * r2 + s2 * r4;
 532             double yd = yn * an * bn + p2 * xnyn + p1 * r2yn2 + s3 * r2 + s4 * r4;
 533 
 534             //4.5 DistortedPixelPoint
 535             data2D[k][0] = xd * fx + cx;
 536             data2D[k][1] = yd * fy + cy;
 537 
 538             //5.6 dpdK: derivatives with respect to the camera parameters
 539             if (dpdK) dpdK->push_back(Mat_<double>({ 2, 4 }, { xd, 0, 1, 0, 0, yd, 0, 1 }));
 540 
 541             //5.7 dpdD: derivatives with respect to the distortion coefficients
 542             if (dpdD)
 543             {
 544                 Mat_<double> dpDdpCD(2, 2);
 545                 dpDdpCD << fx, 0, 0, fy;
 546 
 547                 Mat_<double> dpCDdD(2, nDist);
 548                 if (nDist < 5)
 549                 {
 550                     dpCDdD <<
 551                         xn * r2 * bn, xn* r4* bn, xnyn, r2xn2,
 552                         yn* r2* bn, yn* r4* bn, r2yn2, xnyn;
 553                 }
 554                 else if (nDist < 8)
 555                 {
 556                     dpCDdD <<
 557                         xn * r2 * bn, xn* r4* bn, xnyn, r2xn2, xn* r6* bn,
 558                         yn* r2* bn, yn* r4* bn, r2yn2, xnyn, yn* r6* bn;
 559                 }
 560                 else if (nDist < 12)
 561                 {
 562                     dpCDdD <<
 563                         xn * r2 * bn, xn* r4* bn, xnyn, r2xn2, xn* r6* bn, -xn * r2 * an * bn * bn, -xn * r4 * an * bn * bn, -xn * r6 * an * bn * bn,
 564                         yn* r2* bn, yn* r4* bn, r2yn2, xnyn, yn* r6* bn, -yn * r2 * an * bn * bn, -yn * r4 * an * bn * bn, -yn * r6 * an * bn * bn;
 565                 }
 566                 else if (nDist < 14)
 567                 {
 568                     dpCDdD <<
 569                         xn * r2 * bn, xn* r4* bn, xnyn, r2xn2, xn* r6* bn, -xn * r2 * an * bn * bn, -xn * r4 * an * bn * bn, -xn * r6 * an * bn * bn, r2, r4, 0, 0,
 570                         yn* r2* bn, yn* r4* bn, r2yn2, xnyn, yn* r6* bn, -yn * r2 * an * bn * bn, -yn * r4 * an * bn * bn, -yn * r6 * an * bn * bn, 0, 0, r2, r4;
 571                 }
 572                 dpdD->push_back(dpDdpCD * dpCDdD);
 573             }
 574 
 575             //5.8 dpdr&&dpdt: derivatives with respect to the pose
 576             if (dpdr || dpdt || dpdP)
 577             {
 578                 Mat_<double> dpDdpCD({ 2, 2 }, { fx, 0, 0, fy });
 579 
 580                 Mat_<double> dpCDdh({ 2, 5 }, {
 581                     xn * bn, -xn * an * bn * bn, p2 + s1 + 2 * s2 * r2, an * bn + 2 * p1 * yn + 4 * p2 * xn, 2 * p1 * xn,
 582                     yn * bn, -yn * an * bn * bn, p1 + s3 + 2 * s4 * r2, 2 * p2 * yn, an * bn + 2 * p2 * xn + 4 * p1 * yn });
 583 
 584                 Mat_<double> dhdpC({ 5, 2 }, {
 585                     xn * (2 * k1 + 4 * k2 * r2 + 6 * k3 * r4), yn * (2 * k1 + 4 * k2 * r2 + 6 * k3 * r4),
 586                     xn * (2 * k4 + 4 * k5 * r2 + 6 * k6 * r4), yn * (2 * k4 + 4 * k5 * r2 + 6 * k6 * r4),
 587                     2 * xn, 2 * yn,
 588                     1, 0,
 589                     0, 1 });
 590 
 591                 Mat_<double> dpCdPC({ 2, 3 }, {
 592                     iz, 0, -xn * iz,
 593                     0, iz, -yn * iz });
 594 
 595                 //1.dpdt: derivatives with respect to the translation vector
 596                 if (dpdt) dpdt->push_back(dpDdpCD * dpCDdh * dhdpC * dpCdPC);
 597 
 598                 //2.dpdr: derivatives with respect to the rotation vector
 599                 if (dpdr)
 600                 {
 601                     //2.1 Exponential map
 602                     if (derivRotWay == 0)
 603                     {
 604                         Mat_<double> dPCdR({ 3, 9 }, {
 605                             X, Y, Z, 0, 0, 0, 0, 0, 0,
 606                             0, 0, 0, X, Y, Z, 0, 0, 0,
 607                             0, 0, 0, 0, 0, 0, X, Y, Z });
 608                         dpdr->push_back(dpDdpCD * dpCDdh * dhdpC * dpCdPC * dPCdR * dRdr);
 609                     }
 610 
 611                     //2.2 BCH formula
 612                     else if (derivRotWay == 1)
 613                     {
 614                         Mat_<double> _skewPC({ 3, 3 }, {
 615                             0, (z - t[2]), -(y - t[1]),
 616                             -(z - t[2]), 0, (x - t[0]),
 617                             (y - t[1]), -(x - t[0]), 0 });
 618                         double theta = sqrt(r[0] * r[0] + r[1] * r[1] + r[2] * r[2]);
 619                         double itheta = 1 / theta;
 620                         double sn = sin(theta);
 621                         double cs1 = 1 - cos(theta);
 622                         Matx31d n(r[0] * itheta, r[1] * itheta, r[2] * itheta);
 623                         Matx33d nskew(0, -n.val[2], n.val[1], n.val[2], 0, -n.val[0], -n.val[1], n.val[0], 0);
 624                         Matx33d Jl = itheta * sn * Matx33d::eye() + itheta * cs1 * nskew + (1 - itheta * sn) * n * n.t();
 625                         dpdr->push_back(dpDdpCD * dpCDdh * dhdpC * dpCdPC * _skewPC * Jl);
 626                     }
 627 
 628                     //2.3 Perturbance without LocalParameterization::ComputeJacobian
 629                     else if (derivRotWay == 2)
 630                     {
 631                         Mat_<double> _skewPC({ 3, 3 }, {
 632                             0, (z - t[2]), -(y - t[1]),
 633                             -(z - t[2]), 0, (x - t[0]),
 634                             (y - t[1]), -(x - t[0]), 0 });
 635                         dpdr->push_back(dpDdpCD * dpCDdh * dhdpC * dpCdPC * _skewPC);
 636                     }
 637 
 638                     //2.4 Perturbance with LocalParameterization::ComputeJacobian
 639                     if (derivRotWay == 3)
 640                     {
 641                         Mat_<double> dPCdR({ 3, 9 }, {
 642                             X, Y, Z, 0, 0, 0, 0, 0, 0,
 643                             0, 0, 0, X, Y, Z, 0, 0, 0,
 644                             0, 0, 0, 0, 0, 0, X, Y, Z });
 645                         dpdr->push_back(dpDdpCD * dpCDdh * dhdpC * dpCdPC * dPCdR);
 646                     }
 647                 }
 648 
 649                 //3.dpdP: derivatives with respect to the worldcoordinate
 650                 if (dpdP) dpdP->push_back(dpDdpCD * dpCDdh * dhdpC * dpCdPC * Mat_<double>(3, 3, R));
 651             }
 652         }
 653         return point2D;
 654     }
 655 
 656 public:
 657     static void rectBino(double* R1, double* R2, double* R, double* t, double* delta = 0, double ratio1 = 0.5, double* tnew = 0)
 658     {
 659         //1.Let left and right image plane be coplanar
 660         Matx31d r; cv::Rodrigues(Matx33d(R), r);
 661         Matx33d RCop1; cv::Rodrigues(ratio1 * r, RCop1);
 662         Matx33d RCop2; cv::Rodrigues(ratio1 * r - r, RCop2);
 663         Matx31d tCop = RCop2 * Matx31d(t);
 664 
 665         //2.Let left and right image plane are epipolar
 666         Matx31d xAxis(tCop(0) > 0 ? 1 : -1, 0, 0);
 667         Matx31d rEpi = Vec3d(tCop.val).cross(Vec3d(xAxis.val));
 668         if (norm(rEpi, NORM_L2) > 0.) rEpi *= (acos(fabs(tCop(0)) / norm(tCop, NORM_L2)) / norm(rEpi, NORM_L2));
 669         Matx33d REpi; cv::Rodrigues(rEpi, REpi);
 670 
 671         //3.Additional rotation for reprojection at expectation angle
 672         Matx33d RDelta = RDelta.eye();
 673         if (delta != 0) cv::Rodrigues(Vec3d(delta), RDelta);
 674 
 675         //4.Final transformation
 676         memcpy(R1, (RDelta * REpi * RCop1).val, 9 * sizeof(double));
 677         memcpy(R2, (RDelta * REpi * RCop2).val, 9 * sizeof(double));
 678         if (tnew != 0) memcpy(tnew, (Matx33d(R2) * Matx31d(t)).val, 3 * sizeof(double));
 679     }
 680 
 681     static void rectBino2(double* R1, double* R2, double* R, double* t, double* delta = 0, double ratio1 = 0.0, double* tnew = 0)
 682     {
 683         //1.Let left and right image plane be coplanar
 684         Matx31d r; cv::Rodrigues(Matx33d(R), r);
 685         Matx33d RCop1; cv::Rodrigues(r * ratio1, RCop1);
 686         Matx33d RCop2; cv::Rodrigues(r * ratio1 - r, RCop2);
 687         Matx31d tCop = RCop2 * Matx31d(t);
 688 
 689         //2.Let left and right image plane be epipolar
 690         Vec3d e1((-tCop).val);
 691         e1 = e1 / cv::norm(e1);
 692         Vec3d e2(-e1(1), e1(0), 0);
 693         e2 = e2 / cv::norm(e2);
 694         Vec3d e3 = e1.cross(e2);
 695         e3 = e3 / cv::norm(e3);
 696         Matx33d REpi(e1(0), e1(1), e1(2), e2(0), e2(1), e2(2), e3(0), e3(1), e3(2));
 697 
 698         //3.Additional rotation for reprojection at expectation angle
 699         Matx33d RDelta = RDelta.eye();
 700         if (delta != 0) cv::Rodrigues(Vec3d(delta), RDelta);
 701 
 702         //4.Final transformation
 703         memcpy(R1, (RDelta * REpi * RCop1).val, 9 * sizeof(double));
 704         memcpy(R2, (RDelta * REpi * RCop2).val, 9 * sizeof(double));
 705         if (tnew != 0) memcpy(tnew, (Matx33d(R2) * Matx31d(t)).val, 3 * sizeof(double));
 706 
 707         if (1)//OpenCV codes
 708         {
 709             //1.Let left and right image plane be coplanar
 710             Matx33d RCop = Matx33d(R).t();
 711             Matx31d tCop = -Matx33d(R).t() * Matx31d(t);//here should not has minus sign and use tCop(0) sign later like stereoRectify and fisheye::stereoRectify
 712 
 713             //2.Let left and right image plane be epipolar
 714             Vec3d e1(tCop.val);
 715             e1 = e1 / cv::norm(e1);
 716             Vec3d e2(-e1(1), e1(0), 0);
 717             e2 = e2 / cv::norm(e2);
 718             Vec3d e3 = e1.cross(e2);
 719             e3 = e3 / cv::norm(e3);
 720             Matx33d REpi(e1(0), e1(1), e1(2), e2(0), e2(1), e2(2), e3(0), e3(1), e3(2));
 721 
 722             //3.Additional rotation for reprojection at expectation angle
 723             Matx33d RDelta = RDelta.eye();
 724             if (delta != 0) cv::Rodrigues(Vec3d(delta), RDelta);
 725 
 726             //4.Final transformation
 727             memcpy(R1, (RDelta * REpi).val, 9 * sizeof(double));
 728             memcpy(R2, (RDelta * REpi * RCop).val, 9 * sizeof(double));
 729             if (tnew != 0) memcpy(tnew, (Matx33d(R2) * Matx31d(t)).val, 3 * sizeof(double));
 730         }
 731     }
 732 
 733 public:
 734     static void TestRTCM(int argc = 0, char** argv = 0)
 735     {
 736         int N = 999;
 737         for (int k = 0; k < N; ++k)
 738         {
 739             //0.GenerateData
 740             static const int nDist = 5;//fix nRot=3 nModel=0
 741             Mat_<Vec3d> point3D(2, 1); cv::randu(point3D, -999, 999);
 742             Mat_<double> K(4, 1); cv::randu(K, 111, 999);
 743             Mat_<double> D(nDist, 1); cv::randu(D, -1.0, 1.0);
 744             Mat_<double> r(3, 1); cv::randu(r, -999, 999);
 745             Mat_<double> t(3, 1); cv::randu(t, -999, 999);
 746 
 747             //1.CalcByOpenCV
 748             Mat_<Vec2d> point2D1;
 749             Mat_<double> dpdKDT1;
 750             projectPoints(point3D, r, t, Matx33d(K(0), 0, K(2), 0, K(1), K(3), 0, 0, 1), D, point2D1, dpdKDT1);
 751             Mat_<double> dpdr1 = dpdKDT1.colRange(0, 3);
 752             Mat_<double> dpdt1 = dpdKDT1.colRange(3, 6);
 753             Mat_<double> dpdK1 = dpdKDT1.colRange(6, 10);
 754             Mat_<double> dpdD1 = dpdKDT1.colRange(10, 10 + D.rows);
 755 
 756             //2.CalcByCeresExp
 757             Mat_<double> dpdr2(point3D.rows * 2, 3);
 758             Mat_<double> dpdt2(point3D.rows * 2, 3);
 759             Mat_<double> dpdK2(point3D.rows * 2, 4);
 760             Mat_<double> dpdD2(point3D.rows * 2, nDist);
 761             Mat_<Vec2d> point2D2(point3D.rows, 1);
 762             ceres::AutoDiffCostFunction<VisionCM, ceres::DYNAMIC, 3, 3, 4, nDist> autoDeriv(new VisionCM(point3D.ptr<double>(), point3D.rows, 3, nDist, 0, VisionCM::RTCM, true), point3D.rows * 2);
 763             double* parameters[4] = { r.ptr<double>(), t.ptr<double>(), K.ptr<double>(), D.ptr<double>() };
 764             double* jacobians[4] = { dpdr2.ptr<double>(), dpdt2.ptr<double>(), dpdK2.ptr<double>(), dpdD2.ptr<double>() };
 765             double* residuals = point2D2.ptr<double>();
 766             autoDeriv.Evaluate(parameters, residuals, jacobians);
 767 
 768             //3.AnalyzeError
 769             double infpts1pts2 = norm(point2D1, point2D2, NORM_INF);
 770             double infdpdr1r2 = norm(dpdr1, dpdr2, NORM_INF);
 771             double infdpdt1t2 = norm(dpdt1, dpdt2, NORM_INF);
 772             double infdpdK1K2 = norm(dpdK1, dpdK2, NORM_INF);
 773             double infdpdD1D2 = norm(dpdD1, dpdD2, NORM_INF);
 774 
 775             //4.PrintError
 776             cout << fmt::format("LoopCount: {}
", k);
 777             if (infpts1pts2 > 1e-9 || infdpdr1r2 > 1e-9 || infdpdt1t2 > 1e-9 || infdpdK1K2 > 1e-9 || infdpdD1D2 > 1e-9)
 778             {
 779                 cout << endl << "infpts1pts2: " << infpts1pts2 << endl;
 780                 cout << endl << "infdpdr1r2: " << infdpdr1r2 << endl;
 781                 cout << endl << "infdpdt1t2: " << infdpdt1t2 << endl;
 782                 cout << endl << "infdpdK1K2: " << infdpdK1K2 << endl;
 783                 cout << endl << "infdpdD1D2: " << infdpdD1D2 << endl;
 784                 if (0)
 785                 {
 786                     //cout << endl << "K: " << K.t() << endl;
 787                     //cout << endl << "D: " << D.t() << endl;
 788                     //cout << endl << "r: " << r.t() << endl;
 789                     //cout << endl << "t: " << t.t() << endl;
 790                     cout << endl << "point3D: " << point3D << endl;
 791                     cout << endl << "point2D1: " << point2D1 << endl;
 792                     cout << endl << "point2D2: " << point2D2 << endl;
 793                 }
 794                 cout << "Press any key to continue" << endl; getchar();
 795             }
 796         }
 797     }
 798     static void TestKBCM(int argc = 0, char** argv = 0)
 799     {
 800         int N = 999;
 801         for (int k = 0; k < N; ++k)
 802         {
 803             //0.GenerateData //fix nRot=3 nDist=4 nModel=0
 804             Mat_<Vec3d> point3D(2, 1); cv::randu(point3D, -999, 999);
 805             Mat_<double> K(4, 1); cv::randu(K, 111, 999);
 806             Mat_<double> D(4, 1); cv::randu(D, -1.0, 1.0);
 807             Mat_<double> r(3, 1); cv::randu(r, -999, 999);
 808             Mat_<double> t(3, 1); cv::randu(t, -999, 999);
 809 
 810             //1.CalcByOpenCV
 811             Mat_<Vec2d> point2D1;
 812             Mat_<double> dpdKDT1;
 813             fisheye::projectPoints(point3D, point2D1, r, t, Matx33d(K(0), 0, K(2), 0, K(1), K(3), 0, 0, 1), D, 0, dpdKDT1);
 814             Mat_<double> dpdK1 = dpdKDT1.colRange(0, 4);
 815             Mat_<double> dpdD1 = dpdKDT1.colRange(4, 8);
 816             Mat_<double> dpdr1 = dpdKDT1.colRange(8, 11);
 817             Mat_<double> dpdt1 = dpdKDT1.colRange(11, 14);
 818 
 819             //2.CalcByCeresExp
 820             Mat_<double> dpdr2(point3D.rows * 2, 3);
 821             Mat_<double> dpdt2(point3D.rows * 2, 3);
 822             Mat_<double> dpdK2(point3D.rows * 2, 4);
 823             Mat_<double> dpdD2(point3D.rows * 2, 4);
 824             Mat_<double> dpda2(point3D.rows * 2, 1);
 825             Mat_<Vec2d> point2D2(point3D.rows, 1);
 826             ceres::AutoDiffCostFunction<VisionCM, ceres::DYNAMIC, 3, 3, 4, 4> autoDeriv(new VisionCM(point3D.ptr<double>(), point3D.rows, 3, 4, 0, VisionCM::KBCM, true), point3D.rows * 2);
 827             double* parameters[4] = { r.ptr<double>(), t.ptr<double>(), K.ptr<double>(), D.ptr<double>() };
 828             double* jacobians[4] = { dpdr2.ptr<double>(), dpdt2.ptr<double>(), dpdK2.ptr<double>(), dpdD2.ptr<double>() };
 829             double* residuals = point2D2.ptr<double>();
 830             autoDeriv.Evaluate(parameters, residuals, jacobians);
 831 
 832             //3.AnalyzeError
 833             double infpts1pts2 = norm(point2D1, point2D2, NORM_INF);
 834             double infdpdr1r2 = norm(dpdr1, dpdr2, NORM_INF);
 835             double infdpdt1t2 = norm(dpdt1, dpdt2, NORM_INF);
 836             double infdpdK1K2 = norm(dpdK1, dpdK2, NORM_INF);
 837             double infdpdD1D2 = norm(dpdD1, dpdD2, NORM_INF);
 838 
 839             //4.PrintError
 840             cout << fmt::format("LoopCount: {}
", k);
 841             if (infpts1pts2 > 1e-9 || infdpdr1r2 > 1e-9 || infdpdt1t2 > 1e-9 || infdpdK1K2 > 1e-9 || infdpdD1D2 > 1e-9)
 842             {
 843                 cout << endl << "infpts1pts2: " << infpts1pts2 << endl;
 844                 cout << endl << "infdpdr1r2: " << infdpdr1r2 << endl;
 845                 cout << endl << "infdpdt1t2: " << infdpdt1t2 << endl;
 846                 cout << endl << "infdpdK1K2: " << infdpdK1K2 << endl;
 847                 cout << endl << "infdpdD1D2: " << infdpdD1D2 << endl;
 848                 if (0)
 849                 {
 850                     //cout << endl << "K: " << K.t() << endl;
 851                     //cout << endl << "D: " << D.t() << endl;
 852                     //cout << endl << "r: " << r.t() << endl;
 853                     //cout << endl << "t: " << t.t() << endl;
 854                     cout << endl << "point3D: " << point3D << endl;
 855                     cout << endl << "point2D1: " << point2D1 << endl;
 856                     cout << endl << "point2D2: " << point2D2 << endl;
 857                 }
 858                 cout << "Press any key to continue" << endl; getchar();
 859             }
 860         }
 861     }
 862 
 863     static void TestRTEUCM(int argc = 0, char** argv = 0)
 864     {
 865         int N = 999;
 866         for (int k = 0; k < N; ++k)
 867         {
 868             //0.GenerateData
 869             static const int nDist = 4;//fix nRot=3 nModel=2
 870             Mat_<Vec3d> point3D(2, 1); cv::randu(point3D, -999, 999);
 871             Mat_<double> K(4, 1); cv::randu(K, 111, 999);
 872             Mat_<double> D(nDist, 1); cv::randu(D, -1, 1);
 873             Mat_<double> ab(2, 1); cv::randu(ab, 0, 9); double A = ab(0); if (k % 2 == 0) ab(1) = 1;
 874             Mat_<double> r(3, 1); cv::randu(r, -999, 999);
 875             Mat_<double> t(3, 1); cv::randu(t, -999, 999);
 876 
 877             //1.CalcByOpenCV(nRot=3&&nDist=4&&&simPin=false)
 878             Mat_<Vec2d> point2D1;
 879             Mat_<double> dpdKDT1;
 880             omnidir::projectPoints(point3D, point2D1, r, t, Matx33d(K(0), 0, K(2), 0, K(1), K(3), 0, 0, 1), ab(0), D.rowRange(0, 4), dpdKDT1);
 881             Mat_<double> dpdr1 = dpdKDT1.colRange(0, 3);
 882             Mat_<double> dpdt1 = dpdKDT1.colRange(3, 6);
 883             Mat_<double> dpdK1 = dpdKDT1.colRange(6, 11);
 884             Mat_<double> dpds1 = dpdK1.col(2).clone(); dpdK1.col(3).copyTo(dpdK1.col(2)); dpdK1.col(4).copyTo(dpdK1.col(3)); dpds1.copyTo(dpdK1.col(4));
 885             Mat_<double> dpda1 = dpdKDT1.colRange(11, 12);
 886             Mat_<double> dpdD1 = dpdKDT1.colRange(12, 16);
 887 
 888             //2.CalcByCeresExpSameAsOpenCV(nRot=3&&nDist=4&&&&simPin=false)
 889             Mat_<double> dpdr2(point3D.rows * 2, 3);
 890             Mat_<double> dpdt2(point3D.rows * 2, 3);
 891             Mat_<double> dpdK2(point3D.rows * 2, 4);
 892             Mat_<double> dpdD2(point3D.rows * 2, nDist);
 893             Mat_<double> dpdab2(point3D.rows * 2, 2);
 894             Mat_<Vec2d> point2D2(point3D.rows, 1);
 895             ceres::AutoDiffCostFunction<VisionCM, ceres::DYNAMIC, 3, 3, 4, nDist, 2> autoDeriv2(new VisionCM(point3D.ptr<double>(), point3D.rows, 3, nDist, 2, VisionCM::RTEUCM, false), point3D.rows * 2);
 896             double* parameters2[5] = { r.ptr<double>(), t.ptr<double>(), K.ptr<double>(), D.ptr<double>(), ab.ptr<double>() };
 897             double* jacobians2[5] = { dpdr2.ptr<double>(), dpdt2.ptr<double>(), dpdK2.ptr<double>(), dpdD2.ptr<double>(), dpdab2.ptr<double>() };
 898             double* residuals2 = point2D2.ptr<double>();
 899             autoDeriv2.Evaluate(parameters2, residuals2, jacobians2);
 900 
 901             //3.CalcByCeresExpSameAsOpenCV(nRot=3&&nDist=4&&simPin=true)
 902             ab(0) = VisionCM::Mei2Pin(ab(0), K.ptr<double>(), K.ptr<double>() + 1, 0, D.ptr<double>(), D.rows, true);
 903             Mat_<double> dpdr3(point3D.rows * 2, 3);
 904             Mat_<double> dpdt3(point3D.rows * 2, 3);
 905             Mat_<double> dpdK3(point3D.rows * 2, 4);
 906             Mat_<double> dpdD3(point3D.rows * 2, nDist);
 907             Mat_<double> dpdab3(point3D.rows * 2, 2);
 908             Mat_<Vec2d> point2D3(point3D.rows, 1);
 909             ceres::AutoDiffCostFunction<VisionCM, ceres::DYNAMIC, 3, 3, 4, nDist, 2> autoDeriv3(new VisionCM(point3D.ptr<double>(), point3D.rows, 3, nDist, 2, VisionCM::RTEUCM, true), point3D.rows * 2);
 910             double* parameters3[5] = { r.ptr<double>(), t.ptr<double>(), K.ptr<double>(), D.ptr<double>(), ab.ptr<double>() };
 911             double* jacobians3[5] = { dpdr3.ptr<double>(), dpdt3.ptr<double>(), dpdK3.ptr<double>(), dpdD3.ptr<double>(), dpdab3.ptr<double>() };
 912             double* residuals3 = point2D3.ptr<double>();
 913             autoDeriv3.Evaluate(parameters3, residuals3, jacobians3);
 914 
 915             //4.AnalyzeError
 916             double infpts1pts2 = (nDist == 4 && ab(1) == 1) ? cv::norm(point2D1, point2D2, NORM_INF) : 0;
 917             double infpts1pts3 = (nDist == 4 && ab(1) == 1) ? cv::norm(point2D1, point2D3, NORM_INF) : 0;
 918             double infpts2pts3 = cv::norm(point2D2, point2D3, NORM_INF);
 919             double infdpdr1r2 = (nDist == 4 && ab(1) == 1) ? cv::norm(dpdr1, dpdr2, NORM_INF) : 0;
 920             double infdpdt1t2 = (nDist == 4 && ab(1) == 1) ? cv::norm(dpdt1, dpdt2, NORM_INF) : 0;
 921             double infdpdK1K2 = (nDist == 4 && ab(1) == 1) ? cv::norm(dpdK1.colRange(0, 4), dpdK2, NORM_INF) : 0;
 922             double infdpdD1D2 = (nDist == 4 && ab(1) == 1) ? cv::norm(dpdD1, dpdD2, NORM_INF) : 0;
 923             double infdpda1a2 = (nDist == 4 && ab(1) == 1) ? cv::norm(dpda1, dpdab2.col(0), NORM_INF) : 0;
 924 
 925             //5.PrintError
 926             cout << fmt::format("LoopCount: {}   A: {:.12f}   a: {:.12f}   b: {:.12f}
", k, A, ab(0), ab(1), nDist);
 927             if (infpts1pts2 > 1e-9 || infpts1pts3 > 1e-9 || infpts2pts3 > 1e-9 || infdpdr1r2 > 1e-9 || infdpdt1t2 > 1e-9 || infdpdK1K2 > 1e-9 || infdpdD1D2 > 1e-9 || infdpda1a2 > 1e-9)
 928             {
 929                 cout << endl << "infpts1pts2: " << infpts1pts2 << endl;
 930                 cout << endl << "infpts1pts3: " << infpts1pts3 << endl;
 931                 cout << endl << "infpts2pts3: " << infpts2pts3 << endl;
 932                 cout << endl << "infdpdr1r2: " << infdpdr1r2 << endl;
 933                 cout << endl << "infdpdt1t2: " << infdpdt1t2 << endl;
 934                 cout << endl << "infdpdK1K2: " << infdpdK1K2 << endl;
 935                 cout << endl << "infdpdD1D2: " << infdpdD1D2 << endl;
 936                 cout << endl << "infdpda1a2: " << infdpda1a2 << endl;
 937                 if (0)
 938                 {
 939                     //cout << endl << "K: " << K.t() << endl;
 940                     //cout << endl << "D: " << D.t() << endl;
 941                     //cout << endl << "ab: " << ab.t() << endl;
 942                     //cout << endl << "r: " << r.t() << endl;
 943                     //cout << endl << "t: " << t.t() << endl;
 944                     cout << endl << "point3D: " << point3D << endl;
 945                     cout << endl << "point2D1:" << point2D1 << endl;
 946                     cout << endl << "point2D2:" << point2D2 << endl;
 947                     cout << endl << "point2D3:" << point2D3 << endl;
 948                 }
 949                 cout << "Press any key to continue" << endl; getchar();
 950             }
 951 
 952         }
 953     }
 954     static void TestRTDSCM(int argc = 0, char** argv = 0) {}
 955 
 956 public:
 957     static void TestInvRTCM(int argc = 0, char** argv = 0)
 958     {
 959         int N = 999;
 960         for (int k = 0; k < N; ++k)
 961         {
 962             //0.GenerateData
 963             const int nDist = k % 4 == 0 ? 4 : k % 4 == 1 ? 5 : k % 4 == 2 ? 8 : 12;
 964             Mat_<Vec3d> point3D(22, 1); cv::randu(point3D, -999, 999);
 965             Mat_<double> K(4, 1); cv::randu(K, 111, 999);
 966             Mat_<double> D(nDist, 1); cv::randu(D, -0.2, 0.2);
 967             Mat_<double> r(3, 1); cv::randu(r, -999, 999);
 968             Mat_<double> t(3, 1); cv::randu(t, -999, 999);
 969             Mat_<Vec2d> point2D; projectPoints(point3D, r, t, Matx33d(K(0), 0, K(2), 0, K(1), K(3), 0, 0, 1), D, point2D);
 970 
 971             //1.CalcGT
 972             Mat_<Vec2d> point2D0; projectPoints(point3D, r, t, Mat_<double>::eye(3, 3), Mat_<double>::zeros(D.rows, 1), point2D0);
 973 
 974             //2.CalcByOpenCV
 975             Mat_<Vec2d> point2D1; undistortPoints(point2D, point2D1, Matx33d(K(0), 0, K(2), 0, K(1), K(3), 0, 0, 1), D, noArray(), noArray(), TermCriteria(TermCriteria::COUNT, 8, 1e-3));
 976 
 977             //3.CalcByDIY
 978             Mat_<Vec2d> point2D2 = reprojectPoints(point2D, K.ptr<double>(), D.ptr<double>(), 0, D.rows, 0, VisionCM::RTCM, true, 0, 0, 0, 0, TermCriteria(TermCriteria::COUNT, 8, 1e-3));
 979 
 980             //4.AnalyzeError
 981             double infpts0pts1 = norm(point2D0, point2D1, NORM_INF);
 982             double infpts0pts2 = norm(point2D0, point2D2, NORM_INF);
 983             double infpts1pts2 = norm(point2D1, point2D2, NORM_INF);
 984             TermCriteria criteria(TermCriteria::COUNT + TermCriteria::EPS, k % 20 + 2, 1. / pow(10, k % 10 + 2));
 985             Mat_<double> P(4, 1); cv::randu(P, 111, 999);
 986             Mat_<double> R(3, 3); cv::Rodrigues(Vec3d::randu(-1, 1), R);
 987             Mat_<Vec2d> point2D3; undistortPoints(point2D, point2D3, Matx33d(K(0), 0, K(2), 0, K(1), K(3), 0, 0, 1), D, R, Matx33d(P(0), 0, P(2), 0, P(1), P(3), 0, 0, 1), criteria);
 988             Mat_<Vec2d> point2D4 = reprojectPoints(point2D, K.ptr<double>(), D.ptr<double>(), 0, D.rows, 0, VisionCM::RTCM, true, R.ptr<double>(), P.ptr<double>(), 0, 0, criteria);
 989             double infpts3pts4 = norm(point2D3, point2D4, NORM_INF);
 990 
 991             //5.PrintError
 992             cout << endl << "LoopCount: " << k << endl;
 993             if (/*infpts0pts1 > 1E-12 || infpts0pts2 > 1E-12 || */infpts1pts2 > 1E-12 || infpts3pts4 > 1E-12)
 994             {
 995                 cout << endl << "infpts0pts1: " << infpts0pts1 << endl;
 996                 cout << endl << "infpts0pts2: " << infpts0pts2 << endl;
 997                 cout << endl << "infpts1pts2: " << infpts1pts2 << endl;
 998                 cout << endl << "infpts3pts4: " << infpts3pts4 << endl;
 999                 if (0)
1000                 {
1001                     cout << endl << "point3D: " << point3D << endl;
1002                     cout << endl << "point2D0: " << point2D0 << endl;
1003                     cout << endl << "point2D1: " << point2D1 << endl;
1004                     cout << endl << "point2D2: " << point2D2 << endl;
1005                     cout << endl << "point2D3: " << point2D3 << endl;
1006                     cout << endl << "point2D4: " << point2D4 << endl;
1007                 }
1008                 cout << endl << "Press any key to continue" << endl; getchar();
1009             }
1010         }
1011     }
1012     static void TestInvKBCM(int argc = 0, char** argv = 0)
1013     {
1014         int N = 999;
1015         for (int k = 0; k < N; ++k)
1016         {
1017             //0.GenerateData
1018             static const int nDist = 4;
1019             Mat_<Vec3d> point3D(22, 1); cv::randu(point3D, -999, 999);
1020             Mat_<double> K(4, 1); cv::randu(K, 111, 999);
1021             Mat_<double> D(4, 1); cv::randu(D, -0.2, 0.2);
1022             Mat_<double> r(3, 1); cv::randu(r, -999, 999);
1023             Mat_<double> t(3, 1); cv::randu(t, -999, 999);
1024             Mat_<Vec2d> point2D; fisheye::projectPoints(point3D, point2D, r, t, Matx33d(K(0), 0, K(2), 0, K(1), K(3), 0, 0, 1), D);
1025 
1026             //1.CalcGT
1027             Mat_<Vec2d> point2D0; fisheye::projectPoints(point3D, point2D0, r, t, Mat_<double>::eye(3, 3), Mat_<double>::zeros(D.rows, 1));
1028 
1029             //2.CalcByOpenCV
1030             Mat_<Vec2d> point2D1; fisheye::undistortPoints(point2D, point2D1, Matx33d(K(0), 0, K(2), 0, K(1), K(3), 0, 0, 1), D, noArray(), noArray());
1031 
1032             //3.CalcByDIY
1033             Mat_<Vec2d> point2D2 = reprojectPoints(point2D, K.ptr<double>(), D.ptr<double>(), 0, D.rows, 0, VisionCM::KBCM, false, 0, 0, 0, 0,
1034                 TermCriteria(TermCriteria::COUNT + TermCriteria::COUNT, 10, 1e-8));//criteria is fixed as (10, 1e-8) in opencv4.5 KB
1035 
1036             //4.AnalyzeError
1037             double infpts0pts1 = norm(point2D0, point2D1, NORM_INF);
1038             double infpts0pts2 = norm(point2D0, point2D2, NORM_INF);
1039             double infpts1pts2 = norm(point2D1, point2D2, NORM_INF);
1040             TermCriteria criteria(TermCriteria::COUNT + TermCriteria::EPS, k % 20 + 2, 1. / pow(10, k % 10 + 2));
1041             Mat_<double> P(4, 1); cv::randu(P, 111, 999);
1042             Mat_<double> R(3, 3); cv::Rodrigues(Vec3d::randu(-1, 1), R);
1043             Mat_<Vec2d> point2D3; fisheye::undistortPoints(point2D, point2D3, Matx33d(K(0), 0, K(2), 0, K(1), K(3), 0, 0, 1), D, R, Matx33d(P(0), 0, P(2), 0, P(1), P(3), 0, 0, 1));
1044             Mat_<Vec2d> point2D4 = reprojectPoints(point2D, K.ptr<double>(), D.ptr<double>(), 0, D.rows, 0, VisionCM::KBCM, false, R.ptr<double>(), P.ptr<double>(), 0, 0,
1045                 TermCriteria(TermCriteria::COUNT + TermCriteria::COUNT, 10, 1e-8));//criteria is fixed as (10, 1e-8) in opencv4.5 KB
1046             double infpts3pts4 = norm(point2D3, point2D4, NORM_INF);
1047 
1048             //5.PrintError
1049             cout << endl << "LoopCount: " << k << endl;
1050             if (/*infpts0pts1 > 1E-9 || infpts0pts2 > 1E-9 ||*/ infpts1pts2 > 1E-9 || infpts3pts4 > 1E-9)
1051             {
1052                 cout << endl << "infpts0pts1: " << infpts0pts1 << endl;
1053                 cout << endl << "infpts0pts2: " << infpts0pts2 << endl;
1054                 cout << endl << "infpts1pts2: " << infpts1pts2 << endl;
1055                 cout << endl << "infpts3pts4: " << infpts3pts4 << endl;
1056                 if (0)
1057                 {
1058                     cout << endl << "point3D: " << point3D << endl;
1059                     cout << endl << "point2D0: " << point2D0 << endl;
1060                     cout << endl << "point2D1: " << point2D1 << endl;
1061                     cout << endl << "point2D2: " << point2D2 << endl;
1062                     cout << endl << "point2D3: " << point2D3 << endl;
1063                     cout << endl << "point2D4: " << point2D4 << endl;
1064                 }
1065                 cout << endl << "Press any key to continue" << endl; getchar();
1066             }
1067         }
1068     }
1069 
1070     static void TestInvRTEUCM(int argc = 0, char** argv = 0) {}
1071     static void TestInvRTDSCM(int argc = 0, char** argv = 0) {}
1072 
1073 public:
1074     static void TestJacobianRTCM(int argc = 0, char** argv = 0)
1075     {
1076         int N = 999;
1077         for (int k = 0; k < N; ++k)
1078         {
1079             static const int nRot = 3;//fixed
1080             static const int nDist = 5;//varied
1081             static const int nModel = 0;//fixed
1082             Mat_<Vec3d> point3D(2, 1); cv::randu(point3D, -999, 999);
1083             Mat_<double> K(4, 1); cv::randu(K, 111, 999);
1084             Mat_<double> D(nDist, 1); cv::randu(D, -1.0, 1.0);
1085             Mat_<double> r(3, 1); cv::randu(r, -999, 999);
1086             Mat_<double> t(3, 1); cv::randu(t, -999, 999);
1087 
1088             //1.CalcByOpenCV
1089             Mat_<Vec2d> point2D1;
1090             Mat_<double> dpdKDT1;
1091             projectPoints(point3D, r, t, Matx33d(K(0), 0, K(2), 0, K(1), K(3), 0, 0, 1), D, point2D1, dpdKDT1);
1092             Mat_<double> dpdr1 = dpdKDT1.colRange(0, 3);
1093             Mat_<double> dpdt1 = dpdKDT1.colRange(3, 6);
1094             Mat_<double> dpdK1 = dpdKDT1.colRange(6, 10);
1095             Mat_<double> dpdD1 = dpdKDT1.colRange(10, 10 + D.rows);
1096 
1097             //2.CalcByManuMap
1098             Mat_<double> dpdr2, dpdt2, dpdK2, dpdD2;
1099             Mat_<Vec2d> point2D2 = calcJacobiansOfRTCM(point3D, r.ptr<double>(), t.ptr<double>(), K.ptr<double>(), D.ptr<double>(), D.rows,
1100                 &dpdr2, &dpdt2, &dpdK2, &dpdD2, 0, 0);
1101 
1102             //2.CalcByManuBCH
1103             Mat_<double> dpdr3, dpdt3, dpdK3, dpdD3;
1104             Mat_<Vec2d> point2D3 = calcJacobiansOfRTCM(point3D, r.ptr<double>(), t.ptr<double>(), K.ptr<double>(), D.ptr<double>(), D.rows,
1105                 &dpdr3, &dpdt3, &dpdK3, &dpdD3, 0, 1);
1106 
1107             //4.AnalyzeError
1108             double infpts1pts2 = norm(point2D1, point2D2, NORM_INF);
1109             double infpts1pts3 = norm(point2D1, point2D3, NORM_INF);
1110             double infdpdr1r2 = norm(dpdr1, dpdr2, NORM_INF);
1111             double infdpdr1r3 = norm(dpdr1, dpdr3, NORM_INF);
1112             double infdpdr2r3 = norm(dpdr2, dpdr3, NORM_INF);
1113             double infdpdt1t2 = norm(dpdt1, dpdt2, NORM_INF);
1114             double infdpdt1t3 = norm(dpdt1, dpdt3, NORM_INF);
1115             double infdpdt2t3 = norm(dpdt2, dpdt3, NORM_INF);
1116             double infdpdK1K2 = norm(dpdK1, dpdK2, NORM_INF);
1117             double infdpdK1K3 = norm(dpdK1, dpdK3, NORM_INF);
1118             double infdpdK2K3 = norm(dpdK2, dpdK3, NORM_INF);
1119             double infdpdD1D2 = norm(dpdD1, dpdD2, NORM_INF);
1120             double infdpdD1D3 = norm(dpdD1, dpdD3, NORM_INF);
1121             double infdpdD2D3 = norm(dpdD2, dpdD3, NORM_INF);
1122 
1123             //5.PrintError
1124             cout << fmt::format("LoopCount: {}
", k);
1125             if (infpts1pts2 > 1e-9 || infpts1pts3 > 1e-9 ||
1126                 infdpdr1r2 > 1e-9 || infdpdr1r3 > 1e-9 || infdpdr2r3 > 1e-9 || infdpdt1t2 > 1e-9 || infdpdt1t3 > 1e-9 || infdpdt2t3 > 1e-9 ||
1127                 infdpdK1K2 > 1e-9 || infdpdK1K3 > 1e-9 || infdpdK2K3 > 1e-9 || infdpdD1D2 > 1e-9 || infdpdD1D3 > 1e-9 || infdpdD2D3 > 1e-9)
1128             {
1129                 cout << endl << "infpts1pts2: " << infpts1pts2 << endl;
1130                 cout << endl << "infpts1pts3: " << infpts1pts3 << endl;
1131                 cout << endl << "infdpdr1r2: " << infdpdr1r2 << endl;
1132                 cout << endl << "infdpdr1r3: " << infdpdr1r3 << endl;
1133                 cout << endl << "infdpdr2r3: " << infdpdr2r3 << endl;
1134                 cout << endl << "infdpdt1t2: " << infdpdt1t2 << endl;
1135                 cout << endl << "infdpdt1t3: " << infdpdt1t3 << endl;
1136                 cout << endl << "infdpdt2t3: " << infdpdt2t3 << endl;
1137                 cout << endl << "infdpdK1K2: " << infdpdK1K2 << endl;
1138                 cout << endl << "infdpdK1K3: " << infdpdK1K3 << endl;
1139                 cout << endl << "infdpdK2K3: " << infdpdK2K3 << endl;
1140                 cout << endl << "infdpdD1D2: " << infdpdD1D2 << endl;
1141                 cout << endl << "infdpdD1D3: " << infdpdD1D3 << endl;
1142                 cout << endl << "infdpdD2D3: " << infdpdD2D3 << endl;
1143                 if (0)
1144                 {
1145                     cout << "point2D1: " << point2D1 << endl;
1146                     cout << "point2D2: " << point2D2 << endl;
1147                     cout << "point2D3: " << point2D3 << endl;
1148                 }
1149                 cout << "Press any key to continue" << endl; getchar();
1150             }
1151         }
1152     }
1153 
1154 public:
1155     static void TestRectBino(int argc = 0, char** argv = 0)
1156     {
1157         for (int k = 0; k < 999; ++k)
1158         {
1159             //0.GenerateData
1160             Mat_<double> R(3, 3); ceres::EulerAnglesToRotationMatrix(Matx31d::randu(-15, 15).val, 0, R.ptr<double>());
1161             Mat_<double> t({ 3, 1 }, { -Matx21d::randu(10, 20)(0), Matx21d::randu(-5, 5)(0), Matx21d::randu(-5, 5)(0) });//left camera x must be negative in right camera system which echoes opencv
1162             Matx31d P = P.randu(-999, 999); if ((P(2) < 0)) P(2) = -P(2);//P from phyCam1 to virCam1 == P from phyCam1 to phyCam2 to virCam2. virCam1 == virCam2 when no translation
1163 
1164             //1.CalcByOpenCVPin
1165             Matx33d R11, R12;
1166             stereoRectify(Matx33d::eye(), Mat(), Matx33d::eye(), Mat(), Size(480, 640), R, t, R11, R12, Matx34d(), Matx34d(), noArray());
1167             double infP11P12 = cv::norm(R11 * P, R12 * R * P, NORM_INF);
1168 
1169             //2.CalcByOpenCVKB
1170             Matx33d R21, R22;
1171             fisheye::stereoRectify(Matx33d::eye(), Matx41d::zeros(), Matx33d::eye(), Matx41d::zeros(), Size(480, 640), R, t, R21, R22, Matx34d(), Matx34d(), noArray(), 0);
1172             double infP21P22 = cv::norm(R21 * P, R22 * R * P, NORM_INF);
1173 
1174             //3.CalcByDIYPin
1175             Matx33d R31, R32;
1176             rectBino(R31.val, R32.val, R.ptr<double>(), t.ptr<double>());
1177             double infP31P32 = cv::norm(R31 * P, R32 * R * P, NORM_INF);
1178 
1179             //4.CalcByDIYMUCM
1180             Matx33d R41, R42;
1181             rectBino2(R41.val, R42.val, R.ptr<double>(), t.ptr<double>());
1182             double infP41P42 = cv::norm(R41 * P, R42 * R * P, NORM_INF);
1183 
1184             //5.CalcByOpenCVMUCM
1185             Matx33d R51, R52;
1186             omnidir::stereoRectify(R, t, R51, R52);
1187             double infP51P52 = cv::norm(R51 * P, R52 * R * P, NORM_INF);
1188             string ss = "   It is OK to rectify the last line {_R2 = R21 * _R1} in omnidir::stereoRectify {_R2 = _R1 * R21}";
1189 
1190             //6.AnalyzeError
1191             double infR11R21 = norm(R11, R21, NORM_INF);
1192             double infR11R31 = norm(R11, R31, NORM_INF);
1193             double infR41R51 = norm(R41, R51, NORM_INF);
1194             double infR12R22 = norm(R12, R22, NORM_INF);
1195             double infR12R32 = norm(R12, R32, NORM_INF);
1196             double infR42R52 = norm(R42, R52, NORM_INF);
1197 
1198             //7.PrintError
1199             cout << endl << "LoopCount: " << k << ss << endl;
1200             if (infP11P12 > 1E-12 || infP21P22 > 1E-12 || infP31P32 > 1E-12 || infP41P42 > 1E-12 || //infP51P52 > 1E-12 ||
1201                 infR11R21 > 1E-12 || infR11R31 > 1E-12 || infR12R22 > 1E-12 || infR12R32 > 1E-12 /*|| infR41R51 > 1E-12 || infR42R52 > 1E-12*/)
1202             {
1203                 cout << endl << "infP11P12: " << infP11P12 << endl;
1204                 cout << endl << "infP21P22: " << infP21P22 << endl;
1205                 cout << endl << "infP31P32: " << infP31P32 << endl;
1206                 cout << endl << "infP41P42: " << infP41P42 << endl;
1207                 cout << endl << "infP51P52: " << infP51P52 << endl;
1208                 cout << endl << "infR11R21: " << infR11R21 << endl;
1209                 cout << endl << "infR11R31: " << infR11R31 << endl;
1210                 cout << endl << "infR11R51: " << infR41R51 << endl;
1211                 cout << endl << "infR12R22: " << infR12R22 << endl;
1212                 cout << endl << "infR12R32: " << infR12R32 << endl;
1213                 cout << endl << "infR12R52: " << infR42R52 << endl;
1214                 if (1)
1215                 {
1216                     cout << endl << "R: " << R << endl << endl;
1217                     cout << endl << "t: " << t << endl << endl;
1218                     cout << endl << "P: " << P << endl << endl;
1219                     //cout << endl << "R11: " << R11 << endl;
1220                     //cout << endl << "R12: " << R12 << endl;
1221                     //cout << endl << "R21: " << R21 << endl;
1222                     //cout << endl << "R22: " << R22 << endl;
1223                     //cout << endl << "R31: " << R31 << endl;
1224                     //cout << endl << "R32: " << R32 << endl;
1225                     //cout << endl << "R41: " << R41 << endl;
1226                     //cout << endl << "R42: " << R42 << endl;
1227                     //cout << endl << "R51: " << R51 << endl;
1228                     //cout << endl << "R52: " << R52 << endl;
1229                 }
1230                 cout << endl << "Press any key to continue" << endl; getchar();
1231             }
1232         }
1233     }
1234 };
1235 
1236 #if 0
1237 int main(int argc, char** argv) { ModelCamera::TestRectBino(argc, argv); return 0; }
1238 int main1(int argc, char** argv) { ModelCamera::TestRTCM(argc, argv); return 0; }
1239 int main2(int argc, char** argv) { ModelCamera::TestKBCM(argc, argv); return 0; }
1240 int main3(int argc, char** argv) { ModelCamera::TestRTEUCM(argc, argv); return 0; }
1241 int main4(int argc, char** argv) { ModelCamera::TestRTDSCM(argc, argv); return 0; }
1242 int main5(int argc, char** argv) { ModelCamera::TestInvRTCM(argc, argv); return 0; }
1243 int main6(int argc, char** argv) { ModelCamera::TestInvKBCM(argc, argv); return 0; }
1244 int main7(int argc, char** argv) { ModelCamera::TestInvRTEUCM(argc, argv); return 0; }
1245 int main8(int argc, char** argv) { ModelCamera::TestInvRTDSCM(argc, argv); return 0; }
1246 int main11(int argc, char** argv) { ModelCamera::TestJacobianRTCM(argc, argv); return 0; }
1247 int main21(int argc, char** argv) { ModelCamera::TestRectBino(argc, argv); return 0; }
1248 #endif
View Code
相关阅读:
Linux设备树(四中断)
Linux设备树(三属性)
Linux设备树(二节点)
责任链设计模式
 获取服务器内存和可用线程
 秒杀抢购思路解析
 Hystrix 用法及注解用法
 object is not an instance of declaring class
sqlserver 截取字符串
 sqlserver 转化函数
原文地址：https://www.cnblogs.com/dzyBK/p/14100364.html