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

Posted on 2020-01-31 23:19 dzyBK 阅读(621) 评论(0) 编辑收藏举报
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成像模型(编码为MUCM)，解决UCM建模鱼眼镜头差的问题。
优化工程：于2013年，Heng和Li及Pollefeys。
完善工程：于2016年，Khomutenko和Garcia及Martinet提出EUCM成像模型，解决了UCM径向畸变和建模鱼眼镜头差的问题及MUCM非闭环解问题。
完善工程：于2018年，Usenko和Demmel及Cremers提出DSCM成像模型，相对于EUCM、MUCM、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、MUCM。其中RTCM、KBCM认可度最高，实现在Main模块，其次是MUCM，实现在Extra模块。
通过查看源码，MUCM实现较粗糙，如calibrateCamera和stereoCalibrate对CALIB_USE_GUESS都无效，查看源码发现initializeStereoCalibration忽视了此值，直接重新进行单目标定。然而，即使initializeStereoCalibration不忽视，你会发现单目标定calibrate也忽视了此值。
3.本文要点说明
本文基于OpenCV、Ceres、Spdlog来阐述以下数学模型：
(1)成像模型：RTCM、KBCM、MUCM、EUCM、DSCM，含所有模型的正向投影、逆向投影及偏导矩阵。
(2)模型转换：对MUCM/EUCM/DSCM系列的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+BasaltCM、CalibSolid+CamObservation、TestRTCM、TestKBCM、TestMUCM、TestEUCM、TestDSCM
(2)reproject、TestInvRTCM、TestInvKBCM、TestInvMUCM、TestInvEUCM、TestInvDSCM
(3)derivRTCM、、、、TestJacRTCM
(4)rectBino、rectTrino、TestRectBino、testRectTrino
测试来源：模型RTCM&MUCM对比OpenCV，模型KBCM&EUCM&DSCM对比basalt，偏导对比ceres。
遗留问题：逆投影和外参偏导数的完善和验证。
   1 #ifndef __ModelCamera_h__
   2 #define __ModelCamera_h__
   3 
   4 #include <cscv/ModelRotation.h>
   5 #include <opencv2/ccalib/omnidir.hpp>
   6 #include <basalt/camera/unified_camera.hpp>
   7 #include <basalt/camera/extended_camera.hpp>
   8 #include <basalt/camera/double_sphere_camera.hpp>
   9 #include <basalt/camera/kannala_brandt_camera4.hpp>
  10 
  11 class ModelCamera
  12 {
  13 public:
  14     struct VisionCM
  15     {
  16         enum { RTCM, KBCM, MUCM, EUCM, DSCM };//(1)EUCM and DSCM can use rt distortion (2)MUCM can be replaced with EUCM with nDist=4 and b=1
  17         int nRot;//3 4 9 and for rvec quat rmat respectively
  18         int nDist;//0 4 5 and echo AutoDiffCostFunction template parameter number except for 0. 0 uses overloaded operator.
  19         int nModel;//0 1 2 and echo AutoDiffCostFunction template parameter number except for 0. 0 uses overloaded operator.
  20         int camModel;//connect model with distortion
  21         bool simPin;//valid only for XUCM
  22         Mat_<Vec3d> point3D;
  23         void inline checkInput()
  24         {
  25             if (camModel == RTCM && nModel != 0) assert(0);
  26             if (camModel == KBCM && nModel != 0) assert(0);
  27             if (camModel == MUCM && nModel != 1) assert(0);
  28             if (camModel == EUCM && nModel != 2) assert(0);
  29             if (camModel == DSCM && nModel != 2) assert(0);
  30             if (nRot != 3 && nRot != 4 && nRot != 9) assert(0);
  31             if (nDist != 0 && nDist != 4 && nDist != 5) assert(0);
  32             if (nModel != 0 && nModel != 1 && nModel != 2) assert(0);
  33         }
  34 
  35         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); }
  36 
  37         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); }
  38 
  39         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
  40 
  41         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
  42 
  43         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
  44         {
  45             //1.Projection params
  46             tp fx = K[0];
  47             tp fy = K[1];
  48             tp cx = K[2];
  49             tp cy = K[3];
  50             tp a; if (nModel > 0) a = ab[0];
  51             tp b; if (nModel > 1) b = ab[1];
  52 
  53             //2.Distortion params
  54             tp k1, k2, p1, p2, k3;
  55             if (nDist == 4) { k1 = D[0]; k2 = D[1]; p1 = D[2]; p2 = D[3]; } //k1=k1 k2=k2 p1=k3 p2=k4 k5=k3 for KBCM
  56             else if (nDist == 5) { k1 = D[0]; k2 = D[1]; p1 = D[2]; p2 = D[3]; k3 = D[4];}
  57             else if (nDist != 0) spdlog::critical("Invalid distortion configuration");
  58 
  59             //3.Rotation params
  60             tp R[9]; if (nRot == 9) memcpy(R, rot, sizeof(R));
  61             else if (nRot == 4) ceres::QuaternionToRotation(rot, R);
  62             else if (nRot == 3) ceres::AngleAxisToRotationMatrix(rot, ceres::RowMajorAdapter3x3(R));
  63             else spdlog::critical("Invalid rotation configuration");
  64 
  65             //4.Projection actions
  66             Vec<tp, 2>* data2D = (Vec<tp, 2>*)data2D0;
  67             const Vec3d* data3D = point3D.ptr<Vec3d>();
  68             for (int k = 0; k < point3D.rows; ++k)
  69             {
  70                 //4.1 WorldPoint
  71                 double X = data3D[k][0];
  72                 double Y = data3D[k][1];
  73                 double Z = data3D[k][2];
  74 
  75                 //4.2 CameraPoint
  76                 tp x = R[0] * X + R[1] * Y + R[2] * Z + t[0];
  77                 tp y = R[3] * X + R[4] * Y + R[5] * Z + t[1];
  78                 tp z = R[6] * X + R[7] * Y + R[8] * Z + t[2];
  79 
  80                 //4.3 StandardPhysicsPoint
  81                 tp iz; if (camModel == RTCM) iz = tp(1) / z;
  82                 else if (camModel == KBCM) 
  83                 {
  84                     if (z == tp(0)) { data2D[k][0] = data2D[k][1] = tp(0); continue; }
  85                     iz = tp(1) / z;
  86                 }
  87                 else if (camModel == MUCM)
  88                 {
  89                     tp d = sqrt(x * x + y * y + z * z);
  90                     iz = simPin ? tp(1) / (a * d + (tp(1) - a) * z) : tp(1) / (a * d + z);
  91                     //tp A = simPin ? a / (tp(1) - a) : a;
  92                     //tp flag = A <= tp(1) ? (A * d + z) : (d + A * z);
  93                     //if (flag <= tp(0)) { data2D[k][0] = data2D[k][1] = tp(0); continue; }
  94                 }
  95                 else if (camModel == EUCM)
  96                 {
  97                     tp d = sqrt(b * (x * x + y * y) + z * z);
  98                     iz = simPin ? tp(1) / (a * d + (tp(1) - a) * z) : tp(1) / (a * d + z);
  99                     //tp A = simPin ? a / (tp(1) - a) : a;
 100                     //tp flag = A <= tp(1) ? (A * d + z) : (d + A * z);
 101                     //if (flag <= tp(0)) { data2D[k][0] = data2D[k][1] = tp(0); continue; }
 102                 }
 103                 else if (camModel == DSCM)
 104                 {
 105                     tp d1 = sqrt(x * x + y * y + z * z);
 106                     tp d2 = sqrt(x * x + y * y + (b * d1 + z) * (b * d1 + z));
 107                     iz = simPin ? tp(1) / (a * d2 + (tp(1) - a) * (b * d1 + z)) : tp(1) / (a * d2 + (b * d1 + z));
 108                     tp A = simPin ? a / (tp(1) - a) : a;
 109                     tp flag = A <= tp(1) ? z * sqrt(tp(2) * A * b + b * b + tp(1)) + d1 * (A + b) : z * sqrt(tp(2) * A * b + A * A * (b * b + tp(1))) + d1 * (tp(1) + A * b);
 110                     if (flag <= tp(0)) { data2D[k][0] = data2D[k][1] = tp(0); continue; }
 111                 }
 112                 else spdlog::critical("Invalid model configuration");
 113                 tp xn = x * iz;
 114                 tp yn = y * iz;
 115 
 116                 //4.4 DistortedPhysicsPoint
 117                 tp xd, yd;
 118                 if (camModel == KBCM)//KBDistortion
 119                 {
 120                     tp sgz = z > tp(0) ? tp(1) : tp(-1);
 121                     if (nDist == 0)
 122                     {
 123                         tp rn = sqrt(xn * xn + yn * yn);
 124                         tp irn = tp(1) / rn;
 125                         tp thetan = atan2(rn, sgz);
 126                         xd = xn * irn * thetan * sgz;
 127                         yd = yn * irn * thetan * sgz;
 128                     }
 129                     else
 130                     {
 131                         tp rn = sqrt(xn * xn + yn * yn);
 132                         tp irn = tp(1) / rn;
 133                         tp thetan = atan2(rn, sgz);
 134                         tp thetan2 = thetan * thetan;
 135                         tp thetan4 = thetan2 * thetan2;
 136                         tp thetan6 = thetan2 * thetan4;
 137                         tp thetan8 = thetan2 * thetan6;
 138                         tp thetad = thetan * (tp(1) + k1 * thetan2 + k2 * thetan4 + p1 * thetan6 + p2 * thetan8 + (nDist == 4 ? tp(0) : k3 * thetan2 * thetan8)); //p1=k3 p2=k4 k3=k5 here
 139                         xd = xn * irn * thetad * sgz;
 140                         yd = yn * irn * thetad * sgz;
 141                     }
 142                 }
 143                 else //RTDistortion
 144                 {
 145                     if (nDist == 0) { xd = xn; yd = yn; }
 146                     else
 147                     {
 148                         tp xn2 = xn * xn;
 149                         tp yn2 = yn * yn;
 150                         tp rn2 = xn2 + yn2;
 151                         tp rn4 = rn2 * rn2;
 152                         tp xnyn = tp(2) * xn * yn;
 153                         tp r2xn2 = rn2 + tp(2) * xn2;
 154                         tp r2yn2 = rn2 + tp(2) * yn2;
 155                         tp an = tp(1) + k1 * rn2 + k2 * rn4 + (nDist == 4 ? tp(0) : k3 * rn2 * rn4);
 156                         xd = xn * an + p1 * xnyn + p2 * r2xn2;
 157                         yd = yn * an + p2 * xnyn + p1 * r2yn2;
 158                     }
 159                 }
 160 
 161                 //4.5 DistortedPixelPoint
 162                 data2D[k][0] = xd * fx + cx;
 163                 data2D[k][1] = yd * fy + cy;
 164             }
 165             return true;
 166         }
 167 
 168         static double Mei2Pin(double a, double* fx = 0, double* fy = 0, double* s = 0, double* D = 0, int nDist = 4, bool forward = true)
 169         {
 170             a = forward ? a / (1 + a) : a / (1 - a);
 171             double ratio1 = forward ? 1 - a : 1 + a;
 172             double ratio2 = ratio1 * ratio1;
 173             double ratio3 = ratio1 * ratio2;
 174             double ratio4 = ratio1 * ratio3;
 175             double ratio6 = ratio3 * ratio3;
 176             if (fx) fx[0] *= ratio1;
 177             if (fy) fy[0] *= ratio1;
 178             if (s) s[0] = ratio1 * s[0];
 179             if (nDist > 0) D[0] *= ratio2;//k1
 180             if (nDist > 1) D[1] *= ratio4;//k2
 181             if (nDist > 2) D[2] *= ratio1;//p1
 182             if (nDist > 3) D[3] *= ratio1;//p2
 183             if (nDist > 4) D[4] *= ratio6;//k3
 184             if (nDist > 5) D[5] *= ratio2;//k4
 185             if (nDist > 6) D[6] *= ratio4;//k5
 186             if (nDist > 7) D[7] *= ratio6;//k6
 187             if (nDist > 8) D[8] *= ratio1;//s1
 188             if (nDist > 9) D[9] *= ratio3;//s2
 189             if (nDist > 10) D[10] *= ratio1;//s3
 190             if (nDist > 11) D[11] *= ratio3;//s4
 191             return a;
 192         }
 193     };
 194 
 195     struct BasaltCM : VisionCM
 196     {
 197         BasaltCM(float* data3D, int nPoint, int nRot0 = 9, int nDist0 = 4, int nModel0 = 1, int camModel0 = RTCM, bool simPin0 = false) : VisionCM(data3D, nPoint, nRot0, nDist0, nModel0, camModel0, simPin0) { }
 198 
 199         BasaltCM(double* data3D, int nPoint, int nRot0 = 9, int nDist0 = 4, int nModel0 = 1, int camModel0 = RTCM, bool simPin0 = false) : VisionCM(data3D, nPoint, nRot0, nDist0, nModel0, camModel0, simPin0) { }
 200 
 201         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
 202 
 203         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
 204 
 205         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
 206         {
 207             //1.Projection params
 208             tp fx = K[0];
 209             tp fy = K[1];
 210             tp cx = K[2];
 211             tp cy = K[3];
 212             tp a; if (nModel > 0) a = ab[0];
 213             tp b; if (nModel > 1) b = ab[1];
 214             if(!simPin) assert(0);
 215 
 216             //2.Distortion params
 217             tp k1, k2, p1, p2, k3;
 218             if (nDist == 4) { k1 = D[0]; k2 = D[1]; p1 = D[2]; p2 = D[3]; } //k1=k1 k2=k2 p1=k3 p2=k4 k5=k3 for KBCM
 219             else if (nDist == 5) { k1 = D[0]; k2 = D[1]; p1 = D[2]; p2 = D[3]; k3 = D[4]; }
 220             else if (nDist != 0) spdlog::critical("Invalid distortion configuration");
 221 
 222             //3.Rotation params
 223             tp R[9]; if (nRot == 9) memcpy(R, rot, sizeof(R));
 224             else if (nRot == 4) ceres::QuaternionToRotation(rot, R);
 225             else if (nRot == 3) ceres::AngleAxisToRotationMatrix(rot, ceres::RowMajorAdapter3x3(R));
 226             else spdlog::critical("Invalid rotation configuration");
 227 
 228             //4.Projection actions
 229             Vec<tp, 2>* data2D = (Vec<tp, 2>*)data2D0;
 230             const Vec3d* data3D = point3D.ptr<Vec3d>();
 231             shared_ptr<basalt::KannalaBrandtCamera4<tp>> kbcm;
 232             shared_ptr<basalt::ExtendedUnifiedCamera<tp>> eucm;
 233             shared_ptr<basalt::DoubleSphereCamera<tp>> dscm;
 234             for (int k = 0; k < point3D.rows; ++k)
 235             {
 236                 //4.1 WorldPoint
 237                 double X = data3D[k][0];
 238                 double Y = data3D[k][1];
 239                 double Z = data3D[k][2];
 240 
 241                 //4.2 CameraPoint
 242                 tp x = R[0] * X + R[1] * Y + R[2] * Z + t[0];
 243                 tp y = R[3] * X + R[4] * Y + R[5] * Z + t[1];
 244                 tp z = R[6] * X + R[7] * Y + R[8] * Z + t[2];
 245 
 246                 //4.3 Projection
 247                 if (camModel == KBCM)
 248                 {
 249                     if (k == 0)
 250                     {
 251                         basalt::KannalaBrandtCamera4<tp>::VecN camParam; camParam << fx, fy, cx, cy, k1, k2, p1, p2;
 252                         kbcm.reset(new basalt::KannalaBrandtCamera4<tp>(camParam));
 253                     }
 254                     basalt::KannalaBrandtCamera4<tp>::Vec4 p3D(x, y, z, tp(1));
 255                     basalt::KannalaBrandtCamera4<tp>::Vec2 p2D;
 256                     kbcm->project(p3D, p2D);
 257                     data2D[k][0] = p2D[0];
 258                     data2D[k][1] = p2D[1];
 259                 }
 260                 else if(camModel == EUCM)
 261                 {
 262                     if (k == 0)
 263                     {
 264                         basalt::ExtendedUnifiedCamera<tp>::VecN camParam; camParam << fx, fy, cx, cy, a, b;
 265                         eucm.reset(new basalt::ExtendedUnifiedCamera<tp>(camParam));
 266                     }
 267                     basalt::ExtendedUnifiedCamera<tp>::Vec4 p3D(x, y, z, tp(1));
 268                     basalt::ExtendedUnifiedCamera<tp>::Vec2 p2D;
 269                     eucm->project(p3D, p2D);
 270                     data2D[k][0] = p2D[0];
 271                     data2D[k][1] = p2D[1];
 272                 }
 273                 else if(camModel == DSCM)
 274                 {
 275                     if (k == 0)
 276                     {
 277                         basalt::DoubleSphereCamera<tp>::VecN camParam; camParam << fx, fy, cx, cy, a, b;
 278                         dscm.reset(new basalt::DoubleSphereCamera<tp>(camParam));
 279                     }
 280                     basalt::DoubleSphereCamera<tp>::Vec4 p3D(x, y, z, tp(1));
 281                     basalt::DoubleSphereCamera<tp>::Vec2 p2D;
 282                     dscm->project(p3D, p2D);
 283                     data2D[k][0] = p2D[0];
 284                     data2D[k][1] = p2D[1];
 285                 }
 286             }
 287             return true;
 288         }
 289     };
 290 
 291     struct CamSolid
 292     {
 293         double data[21] = { 0 };
 294         int& nDist = ((int*)data)[0];//same as VisionCM
 295         int& nModel = ((int*)data)[1];//same as VisionCM
 296         int& camModel = ((int*)data)[2];//same as VisionCM
 297         int& simPin = ((int*)data)[3];//same as VisionCM
 298         int& kbcmPi = ((int*)data)[4];//same as VisionCM
 299         int& reserve = ((int*)data)[5];
 300         double* K = data + 3;//not use skew param
 301         double* D = data + 7;//new max memory
 302         double* ab = data + 19;//new max memory
 303         CamSolid() {}
 304         CamSolid(const CamSolid& other) { memcpy(data, other.data, sizeof(data)); }
 305         CamSolid& operator=(const CamSolid& other) { memcpy(data, other.data, sizeof(data)); return *this; }
 306         double limitFOV()
 307         {
 308             double A = simPin ? ab[0] / (1 - ab[0]) : ab[0];
 309             if (camModel == VisionCM::RTCM) return 180;
 310             else if (camModel == VisionCM::KBCM) return 360;
 311             else if (camModel == VisionCM::MUCM) return 2 * acos(-1 / A) * rad2deg;
 312             else if (camModel == VisionCM::EUCM) return 2 * atan2(sqrt(A * A - 1), -sqrt(ab[1])) * rad2deg;
 313             else if (camModel == VisionCM::DSCM) return 2 * acos(-1 / A) * rad2deg;
 314             return 0;
 315         }
 316         Vec6d idealFOV(int rows, int cols)
 317         {
 318             double hfov1 = 0, hfov2 = 0, vfov1 = 0, vfov2 = 0;
 319             if (camModel == VisionCM::RTCM)//geometric inference
 320             {
 321                 hfov1 = atan(K[2] / K[0]);
 322                 hfov2 = atan((cols - K[2]) / K[0]);
 323                 vfov1 = atan(K[3] / K[1]);
 324                 vfov2 = atan((rows - K[3]) / K[1]);
 325             }
 326             else if (camModel == VisionCM::KBCM)//geometric inference
 327             {
 328                 hfov1 = K[2] / K[0];
 329                 hfov2 = (cols - K[2]) / K[0];
 330                 vfov1 = K[3] / K[1];
 331                 vfov2 = (rows - K[3]) / K[1];
 332             }
 333             else if (camModel == VisionCM::MUCM || camModel == VisionCM::EUCM)//geometric inference
 334             {
 335                 auto CalcHalfFOV = [](double F, double c, double A, double b)->double
 336                 {
 337                     double B = A * c / F;
 338                     double Y = A * (b * B * B - sqrt(A * A - b * A * A * B * B + b * B * B));
 339                     double X = B * (A * A + sqrt(A * A - b * A * A * B * B + b * B * B));
 340                     if (0)
 341                     {
 342                         Y = b * A * A * c * c - F * sqrt(A * A * F * F - b * A * A * A * A * c * c + b * A * A * c * c);
 343                         X = A * A * F * c + c * sqrt(A * A * F * F - b * A * A * A * A * c * c + b * A * A * c * c);
 344                     }
 345                     return atan2(1, -Y / X);
 346                 };
 347                 auto CalcHalfFOV2 = [](double F, double c, double A, double b)->double//intersection angle formula
 348                 {
 349                     double ro = c / F;
 350                     Vec3d m(0, 0, 1 / (1 + A));
 351                     Vec3d n(0, ro, (1 - A * A * b * ro * ro) / (1 + A * sqrt(1 + (1 - A * A) * b * ro * ro)));
 352                     double theta = acos(m.dot(n) / sqrt(m.dot(m)) / sqrt(n.dot(n)));
 353                     return theta;
 354                 };
 355                 if (0)
 356                 {
 357                     vector<double> As(99), Fs(99), cs(99), bs(99); cv::randu(As, 0.1, 1.9);
 358                     cv::randu(Fs, 800, 1200); cv::randu(cs, 400, 600); cv::randu(bs, 0.1, 1.9);
 359                     for (int k = 0; k < As.size(); ++k)
 360                     {
 361                         double ang1 = CalcHalfFOV(Fs[k], cs[k], As[k], bs[k]) * rad2deg;
 362                         double ang2 = CalcHalfFOV2(Fs[k], cs[k], As[k], bs[k]) * rad2deg;
 363                         cout << endl << fmt::format("A1-A2: {} - {} = {:.6f}", ang1, ang2, ang1 - ang2) << endl;
 364                     }
 365                 }
 366                 double a = ab[0];
 367                 double b = camModel == VisionCM::MUCM ? 1 : ab[1];
 368                 hfov1 = CalcHalfFOV(simPin ? K[0] / (1 - a) : K[0], K[2], simPin ? a / (1 - a) : a, b);
 369                 hfov2 = CalcHalfFOV(simPin ? K[0] / (1 - a) : K[0], cols - K[2], simPin ? a / (1 - a) : a, b);
 370                 vfov1 = CalcHalfFOV(simPin ? K[1] / (1 - a) : K[1], K[3], simPin ? a / (1 - a) : a, b);
 371                 vfov2 = CalcHalfFOV(simPin ? K[1] / (1 - a) : K[1], rows - K[3], simPin ? a / (1 - a) : a, b);
 372             }
 373             else if (camModel == VisionCM::DSCM)//intersection angle formula
 374             {
 375                 auto CalcHalfFOV = [](double F, double c, double A, double b)->double
 376                 {
 377                     double ro = c / F;
 378                     double zo = (1 - A * A * ro * ro) / (1 + A * sqrt(1 + (1 - A * A) * ro * ro));
 379                     double wo = (b * zo + sqrt(zo * zo + (1 - b * b) * ro * ro)) / (zo * zo + ro * ro);
 380                     Vec3d m(0, 0, 1);
 381                     Vec3d n(0, wo * ro, wo * zo - b);
 382                     double theta = acos(m.dot(n) / sqrt(m.dot(m)) / sqrt(n.dot(n)));
 383                     return theta;
 384                 };
 385                 hfov1 = CalcHalfFOV(simPin ? K[0] / (1 - ab[0]) : K[0], K[2], simPin ? ab[0] / (1 - ab[0]) : ab[0], ab[1]);
 386                 hfov2 = CalcHalfFOV(simPin ? K[0] / (1 - ab[0]) : K[0], cols - K[2], simPin ? ab[0] / (1 - ab[0]) : ab[0], ab[1]);
 387                 vfov1 = CalcHalfFOV(simPin ? K[1] / (1 - ab[0]) : K[1], K[3], simPin ? ab[0] / (1 - ab[0]) : ab[0], ab[1]);
 388                 vfov2 = CalcHalfFOV(simPin ? K[1] / (1 - ab[0]) : K[1], rows - K[3], simPin ? ab[0] / (1 - ab[0]) : ab[0], ab[1]);
 389             }
 390             return Vec6d(hfov1, hfov2, hfov1 + hfov2, vfov1, vfov2, vfov1 + vfov2) * (180 / 3.1415926535897932384626433832795);
 391         }
 392         string print(int rows = 0, int cols = 0)
 393         {
 394             string str;
 395             str += fmt::format("M: [ {}, {}, {}, {} ]\n", nDist, nModel, camModel, simPin);
 396             str += fmt::format("K: [ {}, {}, {}, {} ]\n", K[0], K[1], K[2], K[3]);
 397             str += fmt::format("D: [ {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {}, {} ]\n", D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8], D[9], D[10], D[11]);
 398             str += fmt::format("ab: [ {}, {} ]\n", ab[0], ab[1]);
 399             Vec6d fov; if (rows != 0 && cols != 0) fov = idealFOV(rows, cols);
 400             str += fmt::format("FOV: [ {}, {}, {}, {}, {}, {}, {} ]\n", fov[0], fov[1], fov[2], fov[3], fov[4], fov[5], limitFOV());
 401             return str;
 402         }
 403         //inline double ellip2DLen(double r1, double r2) { return r1 > r2 ? 2 * pi * r2 + 4 * (r1 - r2) : 2 * pi * r1 + 4 * (r2 - r1); }
 404         //inline double ellip2DArea(double r1, double r2) { return 4 * pi * r1 * r2; }
 405         //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); }
 406         //inline double ellip3DVolume(double r1, double r2, double r3) { return 4. / 3. * pi * r1 * r2 * r3; }
 407     };
 408 
 409     using SolidPose = ModelRotation::SolidPose;
 410 
 411     struct CamObservation
 412     {
 413         vector<SolidPose> poses;
 414         vector<vector<Point3f>> point3D;
 415         vector<vector<Point2f>> point2D;
 416         vector<bool> oksView;//can be used for calibration
 417         vector<vector<uint>> ids3D; //can be used for location and calibration
 418         void clear() { poses.clear(); point3D.clear(); point2D.clear(); oksView.clear(); ids3D.clear(); }
 419     };
 420 
 421 public:
 422     static Mat reproject(Mat_<Vec2d> point2D, double* K, double* D, double* ab, int nDist, int nModel, int camModel,//Only support !simPin for UCMSerials and also just verify my formula with DSCMPapers'
 423         double* R = 0, double* P = 0, TermCriteria criteria = TermCriteria(TermCriteria::COUNT, 5, 0.01)) //criteria is fixed as (10, 1e-8) in opencv4.5 KBCM
 424     {
 425         //0.Supported models
 426         if (nDist != 0 && nDist != 4 && nDist != 5) assert(0);
 427         if (nModel != 0 && nModel != 1 && nModel != 2) assert(0);
 428 
 429         //1.Projection params
 430         double fx = K[0], ifx = 1. / fx;
 431         double fy = K[1], ify = 1. / fy;
 432         double cx = K[2], icx = -ifx * cx;
 433         double cy = K[3], icy = -ify * cy;
 434         double a = 0; if (nModel > 0) a = ab[0];
 435         double b = 1; if (nModel > 1) b = ab[1];
 436 
 437         //2.Distortion params
 438         double k1 = 0, k2 = 0, p1 = 0, p2 = 0, k3 = 0;
 439         if (nDist == 4) { k1 = D[0]; k2 = D[1]; p1 = D[2]; p2 = D[3]; }
 440         else if(nDist == 5) { k1 = D[0]; k2 = D[1]; p1 = D[2]; p2 = D[3]; k3 = D[4]; }
 441         else spdlog::critical("Invalid distortion configuration");
 442 
 443         //3.Reprojection matrix
 444         Matx33d A = A.eye();
 445         if (P != 0) A = A * Matx33d(P[0], 0, P[2], 0, P[1], P[3], 0, 0, 1);
 446         if (R != 0) A = A * Matx33d(R);
 447 
 448         //4.Reprojection actions
 449         Mat unPoint(point2D.rows, 1, camModel == VisionCM::RTCM || camModel == VisionCM::KBCM ? CV_64FC2 : CV_64FC3);
 450         Vec2d* data2D = unPoint.ptr<Vec2d>();
 451         Vec3d* data3D = unPoint.ptr<Vec3d>();
 452         for (int k = 0; k < point2D.rows; ++k)
 453         {
 454             //4.1 DistortedPixelPoint-->DistortedPhysicsPoint
 455             double u = point2D(k)[0], xd = (u - cx) * ifx;
 456             double v = point2D(k)[1], yd = (v - cy) * ify;
 457 
 458             //4.2 DistortedPhysics-->StandardPhysicsPoint
 459             double xn = xd, yn = yd;
 460             if (camModel == VisionCM::KBCM)
 461             {
 462                 if (nDist == 0)
 463                 {
 464                     double thetan = sqrt(xn * xn + yn * yn);
 465                     double scaled = tan(thetan) / thetan;
 466                     xn *= scaled;
 467                     yn *= scaled;
 468                 }
 469                 else
 470                 {
 471                     double thetad = sqrt(xd * xd + yd * yd), halfPI = 3.1415926535897932384626433832795 / 2.;
 472                     thetad = thetad > halfPI ? halfPI : thetad < -halfPI ? -halfPI : thetad;//same as opencv which only focuses on 0~180 fov
 473                     double thetan = thetad;//it not conflict with projection using thetan
 474                     int i = 0;
 475                     for (; i < criteria.maxCount; ++i)
 476                     {
 477                         double thetan2 = thetan * thetan, k1thetan2 = k1 * thetan2;
 478                         double thetan4 = thetan2 * thetan2, k2thetan4 = k2 * thetan4;
 479                         double thetan6 = thetan2 * thetan4, k3thetan6 = p1 * thetan6;
 480                         double thetan8 = thetan2 * thetan6, k4thetan8 = p2 * thetan8; //p1=k3 p2=k4 here
 481                         double deltaTheta = (thetan * (1 + k1thetan2 + k2thetan4 + k3thetan6 + k4thetan8) - thetad) / (1 + 3 * k1thetan2 + 5 * k2thetan4 + 7 * k3thetan6 + 9 * k4thetan8);
 482                         thetan -= deltaTheta;
 483                         if ((criteria.type & TermCriteria::EPS) && abs(deltaTheta) < criteria.epsilon) break;
 484                     }
 485                     if (i >= criteria.maxCount || thetad * thetan < 0) //same as opencv4.5 for consistence test and can be removed
 486                     {
 487                         data2D[k] = Vec2d(-1000000.0, -1000000.0);
 488                         continue;
 489                     }
 490                     double scaled = tan(thetan) / thetad;
 491                     xn *= scaled;
 492                     yn *= scaled;
 493                 }
 494             }
 495             else//RTCM&MUCM&EUCM&DSCM
 496             {
 497                 if (nDist == 0) {/*nothing to do*/ }
 498                 else
 499                 {
 500                     for (int i = 0; i < criteria.maxCount; ++i)
 501                     {
 502                         double rn2 = xn * xn + yn * yn;//it not conflict with projection using rn2 anbn deltaX deltaY
 503                         double anbn = 1 / (1 + ((k3 * rn2 + k2) * rn2 + k1) * rn2);
 504                         if (anbn < 0) { xn = (u - cx) * ifx; yn = (v - cy) * ify; break; } // test: undistortPoints.regression_14583
 505                         double deltaX = 2 * p1 * xn * yn + p2 * (rn2 + 2 * xn * xn);
 506                         double deltaY = p1 * (rn2 + 2 * yn * yn) + 2 * p2 * xn * yn;
 507                         xn = (xd - deltaX) * anbn;
 508                         yn = (yd - deltaY) * anbn;
 509 
 510                         if (criteria.type & TermCriteria::EPS)
 511                         {
 512                             double xn2 = xn * xn;
 513                             double yn2 = yn * yn;
 514                             double rn2 = xn2 + yn2;
 515                             double rn4 = rn2 * rn2;
 516                             double xnyn = 2 * xn * yn;
 517                             double r2xn2 = rn2 + 2 * xn2;
 518                             double r2yn2 = rn2 + 2 * yn2;
 519                             double an = 1 + k1 * rn2 + k2 * rn4 + k3 * rn4 * rn2;
 520                             double xdd = xn * an + p1 * xnyn + p2 * r2xn2;
 521                             double ydd = yn * an + p1 * r2yn2 + p2 * xnyn;
 522                             double uerr = xdd * fx + cx - u;
 523                             double verr = ydd * fy + cy - v;
 524                             if (sqrt(uerr * uerr + verr * verr) < criteria.epsilon) break;
 525                         }
 526                     }
 527                 }
 528             }
 529 
 530             //4.3 StandardPhysicsPoint-->StandardPixelPoint
 531             double xc = xn, yc = yn, zc = 1;
 532             if (camModel == VisionCM::RTCM || camModel == VisionCM::KBCM)
 533             {
 534                 double is = 1. / (A.val[6] * xc + A.val[7] * yc + A.val[8] * zc);
 535                 data2D[k][0] = (A.val[0] * xc + A.val[1] * yc + A.val[2] * zc) * is;
 536                 data2D[k][1] = (A.val[3] * xc + A.val[4] * yc + A.val[5] * zc) * is;
 537             }
 538             else
 539             {
 540                 double rn2 = xn * xn + yn * yn;
 541                 if (camModel == VisionCM::DSCM)
 542                 {
 543                     zc = (1 - a * a * rn2) / (1 + a * sqrt(1 + (1 - a * a) * rn2));
 544                     double wc = (b * zc + sqrt(zc * zc + (1 - b * b) * rn2)) / (zc * zc + rn2);
 545                     zc = zc - b / wc;
 546                 }
 547                 else zc = (1 - a * a * b * rn2) / (1 + a * sqrt(1 + (1 - a * a) * b * rn2));//MUCM&EUCM
 548 
 549                 //4.4 StandardPhysicsPoint-->ScaledCameraPoint
 550                 double xcc = xc, ycc = yc, zcc = zc;
 551                 if (R)
 552                 {
 553                     xcc = xc * R[0] + yc * R[1] + zc * R[2];
 554                     ycc = xc * R[3] + yc * R[4] + zc * R[5];
 555                     zcc = xc * R[6] + yc * R[7] + zc * R[8];
 556                 }
 557                 data3D[k] = Vec3d(xcc, ycc, zcc);
 558             }
 559         }
 560         return unPoint;
 561     }
 562 
 563 public:
 564     static Mat_<Vec2d> derivRTCM(Mat_<Vec3d> point3D, double* r, double* t, double* K, double* D, int nDist,
 565         Mat_<double>* dpdr = 0, Mat_<double>* dpdt = 0, Mat_<double>* dpdK = 0, Mat_<double>* dpdD = 0, Mat_<double>* dpdP = 0,
 566         int derivRotWay = 0)
 567     {
 568         //0.FormatInput
 569         if (dpdK) dpdK->release();
 570         if (dpdD) dpdD->release();
 571         if (dpdt) dpdt->release();
 572         if (dpdr) dpdr->release();
 573         if (dpdP) dpdP->release();
 574         Mat_<Vec2d> point2D(point3D.size());
 575 
 576         //1.Projection params
 577         double fx = K[0];
 578         double fy = K[1];
 579         double cx = K[2];
 580         double cy = K[3];
 581 
 582         //2.Distortion params
 583         double k1 = D[0];
 584         double k2 = D[1];
 585         double p1 = D[2];
 586         double p2 = D[3];
 587         double k3 = 0, k4 = 0, k5 = 0, k6 = 0;
 588         double s1 = 0, s2 = 0, s3 = 0, s4 = 0;
 589         if (nDist > 4) k3 = D[4];
 590         if (nDist > 5) { k4 = D[5]; k5 = D[6]; k6 = D[7]; }
 591         if (nDist > 8) { s1 = D[8]; s2 = D[9]; s3 = D[10]; s4 = D[11]; }
 592 
 593         //3.Rotation params
 594         double R[9];
 595         Mat_<double> dRdr;
 596         cv::Rodrigues(Mat_<double>(3, 1, r), Mat_<double>(3, 3, R), (dpdr && derivRotWay == 0) ? dRdr : noArray());
 597         if (dRdr.empty() == false) dRdr = dRdr.t();
 598 
 599         //4.Projection actions
 600         Vec2d* data2D = point2D.ptr<Vec2d>();
 601         Vec3d* data3D = point3D.ptr<Vec3d>();
 602         for (int k = 0; k < point3D.rows; ++k)
 603         {
 604             //4.1 WorldPoint
 605             double X = data3D[k][0];
 606             double Y = data3D[k][1];
 607             double Z = data3D[k][2];
 608 
 609             //4.2 CameraPoint
 610             double x = R[0] * X + R[1] * Y + R[2] * Z + t[0];
 611             double y = R[3] * X + R[4] * Y + R[5] * Z + t[1];
 612             double z = R[6] * X + R[7] * Y + R[8] * Z + t[2];
 613 
 614             //4.3 StandardPhysicsPoint
 615             double iz = z ? 1. / z : 1;
 616             double xn = x * iz;
 617             double yn = y * iz;
 618 
 619             //4.4 DistortedPhysicsPoint
 620             double xn2 = xn * xn;
 621             double yn2 = yn * yn;
 622             double rn2 = xn2 + yn2;
 623             double xnyn = 2 * xn * yn;
 624             double rn4 = rn2 * rn2;
 625             double rn6 = rn2 * rn4;
 626             double r2xn2 = rn2 + 2 * xn2;
 627             double r2yn2 = rn2 + 2 * yn2;
 628             double an = 1 + k1 * rn2 + k2 * rn4 + k3 * rn6;
 629             double bn = 1. / (1 + k4 * rn2 + k5 * rn4 + k6 * rn6);
 630             double xd = xn * an * bn + p1 * xnyn + p2 * r2xn2 + s1 * rn2 + s2 * rn4;
 631             double yd = yn * an * bn + p2 * xnyn + p1 * r2yn2 + s3 * rn2 + s4 * rn4;
 632 
 633             //4.5 DistortedPixelPoint
 634             data2D[k][0] = xd * fx + cx;
 635             data2D[k][1] = yd * fy + cy;
 636 
 637             //5.6 dpdK: derivatives with respect to the camera parameters
 638             if (dpdK) dpdK->push_back(Mat_<double>({ 2, 4 }, { xd, 0, 1, 0, 0, yd, 0, 1 }));
 639 
 640             //5.7 dpdD: derivatives with respect to the distortion coefficients
 641             if (dpdD)
 642             {
 643                 Mat_<double> dpDdpCD(2, 2);
 644                 dpDdpCD << fx, 0, 0, fy;
 645 
 646                 Mat_<double> dpCDdD(2, nDist);
 647                 if (nDist < 5)
 648                 {
 649                     dpCDdD <<
 650                         xn * rn2 * bn, xn* rn4* bn, xnyn, r2xn2,
 651                         yn* rn2* bn, yn* rn4* bn, r2yn2, xnyn;
 652                 }
 653                 else if (nDist < 8)
 654                 {
 655                     dpCDdD <<
 656                         xn * rn2 * bn, xn* rn4* bn, xnyn, r2xn2, xn* rn6* bn,
 657                         yn* rn2* bn, yn* rn4* bn, r2yn2, xnyn, yn* rn6* bn;
 658                 }
 659                 else if (nDist < 12)
 660                 {
 661                     dpCDdD <<
 662                         xn * rn2 * bn, xn* rn4* bn, xnyn, r2xn2, xn* rn6* bn, -xn * rn2 * an * bn * bn, -xn * rn4 * an * bn * bn, -xn * rn6 * an * bn * bn,
 663                         yn* rn2* bn, yn* rn4* bn, r2yn2, xnyn, yn* rn6* bn, -yn * rn2 * an * bn * bn, -yn * rn4 * an * bn * bn, -yn * rn6 * an * bn * bn;
 664                 }
 665                 else if (nDist < 14)
 666                 {
 667                     dpCDdD <<
 668                         xn * rn2 * bn, xn* rn4* bn, xnyn, r2xn2, xn* rn6* bn, -xn * rn2 * an * bn * bn, -xn * rn4 * an * bn * bn, -xn * rn6 * an * bn * bn, rn2, rn4, 0, 0,
 669                         yn* rn2* bn, yn* rn4* bn, r2yn2, xnyn, yn* rn6* bn, -yn * rn2 * an * bn * bn, -yn * rn4 * an * bn * bn, -yn * rn6 * an * bn * bn, 0, 0, rn2, rn4;
 670                 }
 671                 dpdD->push_back(dpDdpCD * dpCDdD);
 672             }
 673 
 674             //5.8 dpdr&&dpdt: derivatives with respect to the pose
 675             if (dpdr || dpdt || dpdP)
 676             {
 677                 Mat_<double> dpDdpCD({ 2, 2 }, { fx, 0, 0, fy });
 678 
 679                 Mat_<double> dpCDdh({ 2, 5 }, {
 680                     xn * bn, -xn * an * bn * bn, p2 + s1 + 2 * s2 * rn2, an * bn + 2 * p1 * yn + 4 * p2 * xn, 2 * p1 * xn,
 681                     yn * bn, -yn * an * bn * bn, p1 + s3 + 2 * s4 * rn2, 2 * p2 * yn, an * bn + 2 * p2 * xn + 4 * p1 * yn });
 682 
 683                 Mat_<double> dhdpC({ 5, 2 }, {
 684                     xn * (2 * k1 + 4 * k2 * rn2 + 6 * k3 * rn4), yn * (2 * k1 + 4 * k2 * rn2 + 6 * k3 * rn4),
 685                     xn * (2 * k4 + 4 * k5 * rn2 + 6 * k6 * rn4), yn * (2 * k4 + 4 * k5 * rn2 + 6 * k6 * rn4),
 686                     2 * xn, 2 * yn,
 687                     1, 0,
 688                     0, 1 });
 689 
 690                 Mat_<double> dpCdPC({ 2, 3 }, {
 691                     iz, 0, -xn * iz,
 692                     0, iz, -yn * iz });
 693 
 694                 //1.dpdt: derivatives with respect to the translation vector
 695                 if (dpdt) dpdt->push_back(dpDdpCD * dpCDdh * dhdpC * dpCdPC);
 696 
 697                 //2.dpdr: derivatives with respect to the rotation vector
 698                 if (dpdr)
 699                 {
 700                     //2.1 Exponential map
 701                     if (derivRotWay == 0)
 702                     {
 703                         Mat_<double> dPCdR({ 3, 9 }, {
 704                             X, Y, Z, 0, 0, 0, 0, 0, 0,
 705                             0, 0, 0, X, Y, Z, 0, 0, 0,
 706                             0, 0, 0, 0, 0, 0, X, Y, Z });
 707                         dpdr->push_back(dpDdpCD * dpCDdh * dhdpC * dpCdPC * dPCdR * dRdr);
 708                     }
 709 
 710                     //2.2 BCH formula
 711                     else if (derivRotWay == 1)
 712                     {
 713                         Mat_<double> _skewPC({ 3, 3 }, {
 714                             0, (z - t[2]), -(y - t[1]),
 715                             -(z - t[2]), 0, (x - t[0]),
 716                             (y - t[1]), -(x - t[0]), 0 });
 717                         double theta = sqrt(r[0] * r[0] + r[1] * r[1] + r[2] * r[2]);
 718                         double itheta = 1 / theta;
 719                         double sn = sin(theta);
 720                         double cs1 = 1 - cos(theta);
 721                         Matx31d n(r[0] * itheta, r[1] * itheta, r[2] * itheta);
 722                         Matx33d nskew(0, -n.val[2], n.val[1], n.val[2], 0, -n.val[0], -n.val[1], n.val[0], 0);
 723                         Matx33d Jl = itheta * sn * Matx33d::eye() + itheta * cs1 * nskew + (1 - itheta * sn) * n * n.t();
 724                         dpdr->push_back(dpDdpCD * dpCDdh * dhdpC * dpCdPC * _skewPC * Jl);
 725                     }
 726 
 727                     //2.3 Perturbance without LocalParameterization::ComputeJacobian
 728                     else if (derivRotWay == 2)
 729                     {
 730                         Mat_<double> _skewPC({ 3, 3 }, {
 731                             0, (z - t[2]), -(y - t[1]),
 732                             -(z - t[2]), 0, (x - t[0]),
 733                             (y - t[1]), -(x - t[0]), 0 });
 734                         dpdr->push_back(dpDdpCD * dpCDdh * dhdpC * dpCdPC * _skewPC);
 735                     }
 736 
 737                     //2.4 Perturbance with LocalParameterization::ComputeJacobian
 738                     if (derivRotWay == 3)
 739                     {
 740                         Mat_<double> dPCdR({ 3, 9 }, {
 741                             X, Y, Z, 0, 0, 0, 0, 0, 0,
 742                             0, 0, 0, X, Y, Z, 0, 0, 0,
 743                             0, 0, 0, 0, 0, 0, X, Y, Z });
 744                         dpdr->push_back(dpDdpCD * dpCDdh * dhdpC * dpCdPC * dPCdR);
 745                     }
 746                 }
 747 
 748                 //3.dpdP: derivatives with respect to the worldcoordinate
 749                 if (dpdP) dpdP->push_back(dpDdpCD * dpCDdh * dhdpC * dpCdPC * Mat_<double>(3, 3, R));
 750             }
 751         }
 752         return point2D;
 753     }
 754 
 755 public:
 756     static void rectBino(double* R1, double* R2, double* R, double* t, double* delta = 0, double ratio1 = 0.5, double* tnew = 0)
 757     {
 758         //1.Let left and right image plane be coplanar
 759         Matx31d r; cv::Rodrigues(Matx33d(R), r);
 760         Matx33d RCop1; cv::Rodrigues(ratio1 * r, RCop1);
 761         Matx33d RCop2; cv::Rodrigues(ratio1 * r - r, RCop2);
 762         Matx31d tCop = RCop2 * Matx31d(t);
 763 
 764         //2.Let left and right image plane are epipolar
 765         Matx31d xAxis(tCop(0) > 0 ? 1 : -1, 0, 0);
 766         Matx31d rEpi = Vec3d(tCop.val).cross(Vec3d(xAxis.val));
 767         if (norm(rEpi, NORM_L2) > 0.) rEpi *= (acos(fabs(tCop(0)) / norm(tCop, NORM_L2)) / norm(rEpi, NORM_L2));
 768         Matx33d REpi; cv::Rodrigues(rEpi, REpi);
 769 
 770         //3.Additional rotation for reprojection at expectation angle
 771         Matx33d RDelta = RDelta.eye();
 772         if (delta != 0) cv::Rodrigues(Vec3d(delta), RDelta);
 773 
 774         //4.Final transformation
 775         memcpy(R1, (RDelta * REpi * RCop1).val, 9 * sizeof(double));
 776         memcpy(R2, (RDelta * REpi * RCop2).val, 9 * sizeof(double));
 777         if (tnew != 0) memcpy(tnew, (Matx33d(R2) * Matx31d(t)).val, 3 * sizeof(double));
 778     }
 779 
 780     static void rectBino2(double* R1, double* R2, double* R, double* t, double* delta = 0, double ratio1 = 0.0, double* tnew = 0)
 781     {
 782         //1.Let left and right image plane be coplanar
 783         Matx31d r; cv::Rodrigues(Matx33d(R), r);
 784         Matx33d RCop1; cv::Rodrigues(r * ratio1, RCop1);
 785         Matx33d RCop2; cv::Rodrigues(r * ratio1 - r, RCop2);
 786         Matx31d tCop = RCop2 * Matx31d(t);
 787 
 788         //2.Let left and right image plane be epipolar
 789         Vec3d e1((-tCop).val);
 790         e1 = e1 / cv::norm(e1);
 791         Vec3d e2(-e1(1), e1(0), 0);
 792         e2 = e2 / cv::norm(e2);
 793         Vec3d e3 = e1.cross(e2);
 794         e3 = e3 / cv::norm(e3);
 795         Matx33d REpi(e1(0), e1(1), e1(2), e2(0), e2(1), e2(2), e3(0), e3(1), e3(2));
 796 
 797         //3.Additional rotation for reprojection at expectation angle
 798         Matx33d RDelta = RDelta.eye();
 799         if (delta != 0) cv::Rodrigues(Vec3d(delta), RDelta);
 800 
 801         //4.Final transformation
 802         memcpy(R1, (RDelta * REpi * RCop1).val, 9 * sizeof(double));
 803         memcpy(R2, (RDelta * REpi * RCop2).val, 9 * sizeof(double));
 804         if (tnew != 0) memcpy(tnew, (Matx33d(R2) * Matx31d(t)).val, 3 * sizeof(double));
 805 
 806         if (1)//OpenCV codes
 807         {
 808             //1.Let left and right image plane be coplanar
 809             Matx33d RCop = Matx33d(R).t();
 810             Matx31d tCop = -Matx33d(R).t() * Matx31d(t);//here should not has minus sign and use tCop(0) sign later like stereoRectify and fisheye::stereoRectify
 811 
 812             //2.Let left and right image plane be epipolar
 813             Vec3d e1(tCop.val);
 814             e1 = e1 / cv::norm(e1);
 815             Vec3d e2(-e1(1), e1(0), 0);
 816             e2 = e2 / cv::norm(e2);
 817             Vec3d e3 = e1.cross(e2);
 818             e3 = e3 / cv::norm(e3);
 819             Matx33d REpi(e1(0), e1(1), e1(2), e2(0), e2(1), e2(2), e3(0), e3(1), e3(2));
 820 
 821             //3.Additional rotation for reprojection at expectation angle
 822             Matx33d RDelta = RDelta.eye();
 823             if (delta != 0) cv::Rodrigues(Vec3d(delta), RDelta);
 824 
 825             //4.Final transformation
 826             memcpy(R1, (RDelta * REpi).val, 9 * sizeof(double));
 827             memcpy(R2, (RDelta * REpi * RCop).val, 9 * sizeof(double));
 828             if (tnew != 0) memcpy(tnew, (Matx33d(R2) * Matx31d(t)).val, 3 * sizeof(double));
 829         }
 830     }
 831 
 832 public:
 833     static void TestRTCM(int argc = 0, char** argv = 0)
 834     {
 835         for (int k = 0; k < 999; ++k)
 836         {
 837             //0.GenerateData
 838             static const int nDist = 5;//fix nRot=3 nModel=0
 839             Mat_<Vec3d> point3D(2, 1); cv::randu(point3D, -999, 999);
 840             Mat_<double> K(4, 1); cv::randu(K, 111, 999);
 841             Mat_<double> D(nDist, 1); cv::randu(D, -1.0, 1.0);
 842             Mat_<double> r(3, 1); cv::randu(r, -999, 999);
 843             Mat_<double> t(3, 1); cv::randu(t, -999, 999);
 844 
 845             //1.CalcByOpenCV
 846             Mat_<Vec2d> point2D1;
 847             Mat_<double> dpdKDT1;
 848             projectPoints(point3D, r, t, Matx33d(K(0), 0, K(2), 0, K(1), K(3), 0, 0, 1), D, point2D1, dpdKDT1);
 849             Mat_<double> dpdr1 = dpdKDT1.colRange(0, 3);
 850             Mat_<double> dpdt1 = dpdKDT1.colRange(3, 6);
 851             Mat_<double> dpdK1 = dpdKDT1.colRange(6, 10);
 852             Mat_<double> dpdD1 = dpdKDT1.colRange(10, 10 + D.rows);
 853 
 854             //2.CalcByCeresExp
 855             Mat_<double> dpdr2(point3D.rows * 2, 3);
 856             Mat_<double> dpdt2(point3D.rows * 2, 3);
 857             Mat_<double> dpdK2(point3D.rows * 2, 4);
 858             Mat_<double> dpdD2(point3D.rows * 2, nDist);
 859             Mat_<Vec2d> point2D2(point3D.rows, 1);
 860             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);
 861             double* parameters[4] = { r.ptr<double>(), t.ptr<double>(), K.ptr<double>(), D.ptr<double>() };
 862             double* jacobians[4] = { dpdr2.ptr<double>(), dpdt2.ptr<double>(), dpdK2.ptr<double>(), dpdD2.ptr<double>() };
 863             double* residuals = point2D2.ptr<double>();
 864             autoDeriv.Evaluate(parameters, residuals, jacobians);
 865 
 866             //3.AnalyzeError
 867             double infpts1pts2 = norm(point2D1, point2D2, NORM_INF);
 868             double infdpdr1r2 = norm(dpdr1, dpdr2, NORM_INF);
 869             double infdpdt1t2 = norm(dpdt1, dpdt2, NORM_INF);
 870             double infdpdK1K2 = norm(dpdK1, dpdK2, NORM_INF);
 871             double infdpdD1D2 = norm(dpdD1, dpdD2, NORM_INF);
 872 
 873             //4.PrintError
 874             cout << fmt::format("LoopCount: {}\n", k);
 875             if (infpts1pts2 > 1e-9 || infdpdr1r2 > 1e-9 || infdpdt1t2 > 1e-9 || infdpdK1K2 > 1e-9 || infdpdD1D2 > 1e-9)
 876             {
 877                 cout << endl << "infpts1pts2: " << infpts1pts2 << endl;
 878                 cout << endl << "infdpdr1r2: " << infdpdr1r2 << endl;
 879                 cout << endl << "infdpdt1t2: " << infdpdt1t2 << endl;
 880                 cout << endl << "infdpdK1K2: " << infdpdK1K2 << endl;
 881                 cout << endl << "infdpdD1D2: " << infdpdD1D2 << endl;
 882                 if (0)
 883                 {
 884                     //cout << endl << "K: " << K.t() << endl;
 885                     //cout << endl << "D: " << D.t() << endl;
 886                     //cout << endl << "r: " << r.t() << endl;
 887                     //cout << endl << "t: " << t.t() << endl;
 888                     cout << endl << "point3D: " << point3D << endl;
 889                     cout << endl << "point2D1: " << point2D1 << endl;
 890                     cout << endl << "point2D2: " << point2D2 << endl;
 891                 }
 892                 cout << "Press any key to continue" << endl; getchar();
 893             }
 894         }
 895     }
 896     static void TestKBCM(int argc = 0, char** argv = 0)
 897     {
 898         for (int k = 0; k < 999; ++k)
 899         {
 900             //0.GenerateData //fix nRot=3 nDist=4 nModel=0
 901             Mat_<Vec3d> point3D(2, 1); cv::randu(point3D, -999, 999);
 902             Mat_<double> K(4, 1); cv::randu(K, 111, 999);
 903             Mat_<double> D(4, 1); cv::randu(D, -1.0, 1.0);
 904             Mat_<double> r(3, 1); cv::randu(r, -999, 999);
 905             Mat_<double> t(3, 1); cv::randu(t, -999, 999);
 906 
 907             //1.CalcByBasaltExp
 908             Mat_<double> dpdr1(point3D.rows * 2, 3);
 909             Mat_<double> dpdt1(point3D.rows * 2, 3);
 910             Mat_<double> dpdK1(point3D.rows * 2, 4);
 911             Mat_<double> dpdD1(point3D.rows * 2, 4);
 912             Mat_<Vec2d> point2D1(point3D.rows, 1);
 913             ceres::AutoDiffCostFunction<BasaltCM, ceres::DYNAMIC, 3, 3, 4, 4> autoDeriv1(new BasaltCM(point3D.ptr<double>(), point3D.rows, 3, 4, 0, BasaltCM::KBCM, true), point3D.rows * 2);
 914             double* parameters1[4] = { r.ptr<double>(), t.ptr<double>(), K.ptr<double>(), D.ptr<double>() };
 915             double* jacobians1[4] = { dpdr1.ptr<double>(), dpdt1.ptr<double>(), dpdK1.ptr<double>(), dpdD1.ptr<double>() };
 916             double* residuals1 = point2D1.ptr<double>();
 917             autoDeriv1.Evaluate(parameters1, residuals1, jacobians1);
 918 
 919             //2.CalcByCeresExp
 920             Mat_<double> dpdr2(point3D.rows * 2, 3);
 921             Mat_<double> dpdt2(point3D.rows * 2, 3);
 922             Mat_<double> dpdK2(point3D.rows * 2, 4);
 923             Mat_<double> dpdD2(point3D.rows * 2, 4);
 924             Mat_<Vec2d> point2D2(point3D.rows, 1);
 925             ceres::AutoDiffCostFunction<VisionCM, ceres::DYNAMIC, 3, 3, 4, 4> autoDeriv2(new VisionCM(point3D.ptr<double>(), point3D.rows, 3, 4, 0, VisionCM::KBCM, true), point3D.rows * 2);
 926             double* parameters2[4] = { r.ptr<double>(), t.ptr<double>(), K.ptr<double>(), D.ptr<double>() };
 927             double* jacobians2[4] = { dpdr2.ptr<double>(), dpdt2.ptr<double>(), dpdK2.ptr<double>(), dpdD2.ptr<double>() };
 928             double* residuals2 = point2D2.ptr<double>();
 929             autoDeriv2.Evaluate(parameters2, residuals2, jacobians2);
 930 
 931             //3.AnalyzeError
 932             double infpts1pts2 = norm(point2D1, point2D2, NORM_INF);
 933             double infdpdr1r2 = norm(dpdr1, dpdr2, NORM_INF);
 934             double infdpdt1t2 = norm(dpdt1, dpdt2, NORM_INF);
 935             double infdpdK1K2 = norm(dpdK1, dpdK2, NORM_INF);
 936             double infdpdD1D2 = norm(dpdD1, dpdD2, NORM_INF);
 937 
 938             //4.PrintError
 939             cout << fmt::format("LoopCount: {}\n", k);
 940             if (infpts1pts2 > 1e-9 || infdpdr1r2 > 1e-9 || infdpdt1t2 > 1e-9 || infdpdK1K2 > 1e-9 || infdpdD1D2 > 1e-9)
 941             {
 942                 cout << endl << "infpts1pts2: " << infpts1pts2 << endl;
 943                 cout << endl << "infdpdr1r2: " << infdpdr1r2 << endl;
 944                 cout << endl << "infdpdt1t2: " << infdpdt1t2 << endl;
 945                 cout << endl << "infdpdK1K2: " << infdpdK1K2 << endl;
 946                 cout << endl << "infdpdD1D2: " << infdpdD1D2 << endl;
 947                 if (0)
 948                 {
 949                     //cout << endl << "K: " << K.t() << endl;
 950                     //cout << endl << "D: " << D.t() << endl;
 951                     //cout << endl << "r: " << r.t() << endl;
 952                     //cout << endl << "t: " << t.t() << endl;
 953                     cout << endl << "point3D: " << point3D << endl;
 954                     cout << endl << "point2D1: " << point2D1 << endl;
 955                     cout << endl << "point2D2: " << point2D2 << endl;
 956                 }
 957                 cout << "Press any key to continue" << endl; getchar();
 958             }
 959         }
 960     }
 961 
 962     static void TestMUCM(int argc = 0, char** argv = 0)
 963     {
 964         for (int k = 0; k < 999; ++k)
 965         {
 966             //0.GenerateData //fix nRot=3 nDist=4 nModel=1
 967             Mat_<Vec3d> point3D(2, 1); cv::randu(point3D, -999, 999);
 968             Mat_<double> K(4, 1); cv::randu(K, 111, 999);
 969             Mat_<double> D(4, 1); cv::randu(D, -1, 1);
 970             Mat_<double> a(1, 1); cv::randu(a, 1, 9); double A = a(0);//only for fisheye and so A>1
 971             Mat_<double> r(3, 1); cv::randu(r, -999, 999);
 972             Mat_<double> t(3, 1); cv::randu(t, -999, 999);
 973 
 974             //1.CalcByOpenCV(nRot=3&&&simPin=false)
 975             Mat_<Vec2d> point2D1;
 976             Mat_<double> dpdKDT1;
 977             omnidir::projectPoints(point3D, point2D1, r, t, Matx33d(K(0), 0, K(2), 0, K(1), K(3), 0, 0, 1), a(0), D.rowRange(0, 4), dpdKDT1);
 978             Mat_<double> dpdr1 = dpdKDT1.colRange(0, 3);
 979             Mat_<double> dpdt1 = dpdKDT1.colRange(3, 6);
 980             Mat_<double> dpdK1 = dpdKDT1.colRange(6, 11);
 981             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));
 982             Mat_<double> dpda1 = dpdKDT1.colRange(11, 12);
 983             Mat_<double> dpdD1 = dpdKDT1.colRange(12, 16);
 984 
 985             //2.CalcByCeresExpSameAsOpenCV(nRot=3&&simPin=false)
 986             Mat_<double> dpdr2(point3D.rows * 2, 3);
 987             Mat_<double> dpdt2(point3D.rows * 2, 3);
 988             Mat_<double> dpdK2(point3D.rows * 2, 4);
 989             Mat_<double> dpdD2(point3D.rows * 2, 4);
 990             Mat_<double> dpda2(point3D.rows * 2, 1);
 991             Mat_<Vec2d> point2D2(point3D.rows, 1);
 992             ceres::AutoDiffCostFunction<VisionCM, ceres::DYNAMIC, 3, 3, 4, 4, 1> autoDeriv2(new VisionCM(point3D.ptr<double>(), point3D.rows, 3, 4, 1, VisionCM::MUCM, false), point3D.rows * 2);
 993             double* parameters2[5] = { r.ptr<double>(), t.ptr<double>(), K.ptr<double>(), D.ptr<double>(), a.ptr<double>() };
 994             double* jacobians2[5] = { dpdr2.ptr<double>(), dpdt2.ptr<double>(), dpdK2.ptr<double>(), dpdD2.ptr<double>(), dpda2.ptr<double>() };
 995             double* residuals2 = point2D2.ptr<double>();
 996             autoDeriv2.Evaluate(parameters2, residuals2, jacobians2);
 997 
 998             //3.CalcByCeresExpSameAsOpenCV(nRot=3&&simPin=true)
 999             a(0) = VisionCM::Mei2Pin(a(0), K.ptr<double>(), K.ptr<double>() + 1, 0, D.ptr<double>(), D.rows, true);
1000             Mat_<double> dpdr3(point3D.rows * 2, 3);
1001             Mat_<double> dpdt3(point3D.rows * 2, 3);
1002             Mat_<double> dpdK3(point3D.rows * 2, 4);
1003             Mat_<double> dpdD3(point3D.rows * 2, 4);
1004             Mat_<double> dpda3(point3D.rows * 2, 1);
1005             Mat_<Vec2d> point2D3(point3D.rows, 1);
1006             ceres::AutoDiffCostFunction<VisionCM, ceres::DYNAMIC, 3, 3, 4, 4, 1> autoDeriv3(new VisionCM(point3D.ptr<double>(), point3D.rows, 3, 4, 1, VisionCM::MUCM, true), point3D.rows * 2);
1007             double* parameters3[5] = { r.ptr<double>(), t.ptr<double>(), K.ptr<double>(), D.ptr<double>(), a.ptr<double>() };
1008             double* jacobians3[5] = { dpdr3.ptr<double>(), dpdt3.ptr<double>(), dpdK3.ptr<double>(), dpdD3.ptr<double>(), dpda3.ptr<double>() };
1009             double* residuals3 = point2D3.ptr<double>();
1010             autoDeriv3.Evaluate(parameters3, residuals3, jacobians3);
1011 
1012             //4.AnalyzeError
1013             double infpts1pts2 = cv::norm(point2D1, point2D2, NORM_INF);
1014             double infpts1pts3 = cv::norm(point2D1, point2D3, NORM_INF);
1015             double infpts2pts3 = cv::norm(point2D2, point2D3, NORM_INF);
1016             double infdpdr1r2 = cv::norm(dpdr1, dpdr2, NORM_INF);
1017             double infdpdt1t2 = cv::norm(dpdt1, dpdt2, NORM_INF);
1018             double infdpdK1K2 = cv::norm(dpdK1.colRange(0, 4), dpdK2, NORM_INF);
1019             double infdpdD1D2 = cv::norm(dpdD1, dpdD2, NORM_INF);
1020             double infdpda1a2 = cv::norm(dpda1, dpda2, NORM_INF);
1021 
1022             //5.PrintError
1023             cout << fmt::format("LoopCount: {}   A: {:.12f}   a: {:.12f}\n", k, A, a(0));
1024             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)
1025             {
1026                 cout << endl << "infpts1pts2: " << infpts1pts2 << endl;
1027                 cout << endl << "infpts1pts3: " << infpts1pts3 << endl;
1028                 cout << endl << "infpts2pts3: " << infpts2pts3 << endl;
1029                 cout << endl << "infdpdr1r2: " << infdpdr1r2 << endl;
1030                 cout << endl << "infdpdt1t2: " << infdpdt1t2 << endl;
1031                 cout << endl << "infdpdK1K2: " << infdpdK1K2 << endl;
1032                 cout << endl << "infdpdD1D2: " << infdpdD1D2 << endl;
1033                 cout << endl << "infdpda1a2: " << infdpda1a2 << endl;
1034                 if (0)
1035                 {
1036                     //cout << endl << "K: " << K.t() << endl;
1037                     //cout << endl << "D: " << D.t() << endl;
1038                     //cout << endl << "ab: " << ab.t() << endl;
1039                     //cout << endl << "r: " << r.t() << endl;
1040                     //cout << endl << "t: " << t.t() << endl;
1041                     cout << endl << "point3D: " << point3D << endl;
1042                     cout << endl << "point2D1:" << point2D1 << endl;
1043                     cout << endl << "point2D2:" << point2D2 << endl;
1044                     cout << endl << "point2D3:" << point2D3 << endl;
1045                 }
1046                 cout << "Press any key to continue" << endl; getchar();
1047             }
1048 
1049         }
1050     }
1051 
1052     static void TestEUCM(int argc = 0, char** argv = 0) 
1053     {
1054         for (int k = 0; k < 999; ++k)
1055         {
1056             //0.GenerateData //fix nRot=3 nDist=0 nModel=2
1057             Mat_<Vec3d> point3D(22, 1); cv::randu(point3D, -999, 999);
1058             Mat_<double> K(4, 1); cv::randu(K, 111, 999);
1059             Mat_<double> ab(2, 1); cv::randu(ab.row(0), 0.5, 0.91); cv::randu(ab.row(1), 0, 9.9); //only for fisheye and so A>1
1060             Mat_<double> r(3, 1); cv::randu(r, -999, 999);
1061             Mat_<double> t(3, 1); cv::randu(t, -999, 999);
1062  
1063             //1.CalcByBasaltExp
1064             Mat_<double> dpdr1(point3D.rows * 2, 3);
1065             Mat_<double> dpdt1(point3D.rows * 2, 3);
1066             Mat_<double> dpdK1(point3D.rows * 2, 4);
1067             Mat_<double> dpdab1(point3D.rows * 2, 2);
1068             Mat_<Vec2d> point2D1(point3D.rows, 1);
1069             ceres::AutoDiffCostFunction<BasaltCM, ceres::DYNAMIC, 3, 3, 4, 2> autoDeriv1(new BasaltCM(point3D.ptr<double>(), point3D.rows, 3, 0, 2, BasaltCM::EUCM, true), point3D.rows * 2);
1070             double* parameters1[4] = { r.ptr<double>(), t.ptr<double>(), K.ptr<double>(), ab.ptr<double>() };
1071             double* jacobians1[4] = { dpdr1.ptr<double>(), dpdt1.ptr<double>(), dpdK1.ptr<double>(), dpdab1.ptr<double>() };
1072             double* residuals1 = point2D1.ptr<double>();
1073             autoDeriv1.Evaluate(parameters1, residuals1, jacobians1);
1074 
1075             //2.CalcByCeresExp
1076             Mat_<double> dpdr2(point3D.rows * 2, 3);
1077             Mat_<double> dpdt2(point3D.rows * 2, 3);
1078             Mat_<double> dpdK2(point3D.rows * 2, 4);
1079             Mat_<double> dpdab2(point3D.rows * 2, 2);
1080             Mat_<Vec2d> point2D2(point3D.rows, 1);
1081             ceres::AutoDiffCostFunction<VisionCM, ceres::DYNAMIC, 3, 3, 4, 2> autoDeriv2(new VisionCM(point3D.ptr<double>(), point3D.rows, 3, 0, 2, VisionCM::EUCM, true), point3D.rows * 2);
1082             double* parameters2[4] = { r.ptr<double>(), t.ptr<double>(), K.ptr<double>(), ab.ptr<double>() };
1083             double* jacobians2[4] = { dpdr2.ptr<double>(), dpdt2.ptr<double>(), dpdK2.ptr<double>(),dpdab2.ptr<double>() };
1084             double* residuals2 = point2D2.ptr<double>();
1085             autoDeriv2.Evaluate(parameters2, residuals2, jacobians2);
1086 
1087             //3.AnalyzeError
1088             double infpts1pts2 = norm(point2D1, point2D2, NORM_INF);
1089             double infdpdr1r2 = norm(dpdr1, dpdr2, NORM_INF);
1090             double infdpdt1t2 = norm(dpdt1, dpdt2, NORM_INF);
1091             double infdpdK1K2 = norm(dpdK1, dpdK2, NORM_INF);
1092             double infdpdab1ab2 = norm(dpdab1, dpdab2, NORM_INF);
1093 
1094             //4.PrintError
1095             cout << fmt::format("LoopCount: {}\n", k);
1096             if (infpts1pts2 > 1e-9 || infdpdr1r2 > 1e-9 || infdpdt1t2 > 1e-9 || infdpdK1K2 > 1e-9 || infdpdab1ab2 > 1e-9)
1097             {
1098                 cout << endl << "infpts1pts2: " << infpts1pts2 << endl;
1099                 cout << endl << "infdpdr1r2: " << infdpdr1r2 << endl;
1100                 cout << endl << "infdpdt1t2: " << infdpdt1t2 << endl;
1101                 cout << endl << "infdpdK1K2: " << infdpdK1K2 << endl;
1102                 cout << endl << "infdpdab1ab2: " << infdpdab1ab2 << endl;
1103                 //if (0)
1104                 {
1105                     //cout << endl << "K: " << K.t() << endl;
1106                     //cout << endl << "D: " << D.t() << endl;
1107                     //cout << endl << "r: " << r.t() << endl;
1108                     //cout << endl << "t: " << t.t() << endl;
1109                     cout << endl << "point3D: " << point3D << endl;
1110                     cout << endl << "point2D1: " << point2D1 << endl;
1111                     cout << endl << "point2D2: " << point2D2 << endl;
1112                 }
1113                 cout << "Press any key to continue" << endl; getchar();
1114             }
1115         }
1116     }
1117     static void TestDSCM(int argc = 0, char** argv = 0) 
1118     {
1119         for (int k = 0; k < 999; ++k)
1120         {
1121             //0.GenerateData //fix nRot=3 nDist=0 nModel=2
1122             Mat_<Vec3d> point3D(22, 1); cv::randu(point3D, -999, 999);
1123             Mat_<double> K(4, 1); cv::randu(K, 111, 999);
1124             Mat_<double> ab(2, 1); cv::randu(ab.row(0), 0.5, 0.91); cv::randu(ab.row(1), -9.9, 9.9); //only for fisheye and so A>1
1125             Mat_<double> ba({ 2, 1 }, { ab(1), ab(0) });//Basalt use different order
1126             Mat_<double> r(3, 1); cv::randu(r, -999, 999);
1127             Mat_<double> t(3, 1); cv::randu(t, -999, 999);
1128 
1129             //1.CalcByBasaltExp
1130             Mat_<double> dpdr1(point3D.rows * 2, 3);
1131             Mat_<double> dpdt1(point3D.rows * 2, 3);
1132             Mat_<double> dpdK1(point3D.rows * 2, 4);
1133             Mat_<double> dpdab1(point3D.rows * 2, 2);
1134             Mat_<Vec2d> point2D1(point3D.rows, 1);
1135             ceres::AutoDiffCostFunction<BasaltCM, ceres::DYNAMIC, 3, 3, 4, 2> autoDeriv1(new BasaltCM(point3D.ptr<double>(), point3D.rows, 3, 0, 2, BasaltCM::DSCM, true), point3D.rows * 2);
1136             double* parameters1[4] = { r.ptr<double>(), t.ptr<double>(), K.ptr<double>(), ba.ptr<double>() };
1137             double* jacobians1[4] = { dpdr1.ptr<double>(), dpdt1.ptr<double>(), dpdK1.ptr<double>(), dpdab1.ptr<double>() };
1138             double* residuals1 = point2D1.ptr<double>();
1139             autoDeriv1.Evaluate(parameters1, residuals1, jacobians1);
1140 
1141             //2.CalcByCeresExp
1142             Mat_<double> dpdr2(point3D.rows * 2, 3);
1143             Mat_<double> dpdt2(point3D.rows * 2, 3);
1144             Mat_<double> dpdK2(point3D.rows * 2, 4);
1145             Mat_<double> dpdab2(point3D.rows * 2, 2);
1146             Mat_<Vec2d> point2D2(point3D.rows, 1);
1147             ceres::AutoDiffCostFunction<VisionCM, ceres::DYNAMIC, 3, 3, 4, 2> autoDeriv2(new VisionCM(point3D.ptr<double>(), point3D.rows, 3, 0, 2, VisionCM::DSCM, true), point3D.rows * 2);
1148             double* parameters2[4] = { r.ptr<double>(), t.ptr<double>(), K.ptr<double>(), ab.ptr<double>() };
1149             double* jacobians2[4] = { dpdr2.ptr<double>(), dpdt2.ptr<double>(), dpdK2.ptr<double>(),dpdab2.ptr<double>() };
1150             double* residuals2 = point2D2.ptr<double>();
1151             autoDeriv2.Evaluate(parameters2, residuals2, jacobians2);
1152 
1153             //3.AnalyzeError
1154             double infpts1pts2 = norm(point2D1, point2D2, NORM_INF);
1155             double infdpdr1r2 = norm(dpdr1, dpdr2, NORM_INF);
1156             double infdpdt1t2 = norm(dpdt1, dpdt2, NORM_INF);
1157             double infdpdK1K2 = norm(dpdK1, dpdK2, NORM_INF);
1158             double infdpdab1ab2 = max(norm(dpdab1.col(0), dpdab2.col(1), NORM_INF), norm(dpdab1.col(1), dpdab2.col(0), NORM_INF));
1159 
1160             //4.PrintError
1161             cout << fmt::format("LoopCount: {}\n", k);
1162             if (infpts1pts2 > 1e-9 || infdpdr1r2 > 1e-9 || infdpdt1t2 > 1e-9 || infdpdK1K2 > 1e-9 || infdpdab1ab2 > 1e-9)
1163             {
1164                 cout << endl << "infpts1pts2: " << infpts1pts2 << endl;
1165                 cout << endl << "infdpdr1r2: " << infdpdr1r2 << endl;
1166                 cout << endl << "infdpdt1t2: " << infdpdt1t2 << endl;
1167                 cout << endl << "infdpdK1K2: " << infdpdK1K2 << endl;
1168                 cout << endl << "infdpdab1ab2: " << infdpdab1ab2 << endl;
1169                 //if (0)
1170                 {
1171                     //cout << endl << "K: " << K.t() << endl;
1172                     //cout << endl << "D: " << D.t() << endl;
1173                     //cout << endl << "r: " << r.t() << endl;
1174                     //cout << endl << "t: " << t.t() << endl;
1175                     cout << endl << "point3D: " << point3D << endl;
1176                     cout << endl << "point2D1: " << point2D1 << endl;
1177                     cout << endl << "point2D2: " << point2D2 << endl;
1178                 }
1179                 cout << "Press any key to continue" << endl; getchar();
1180             }
1181         }
1182     }
1183 
1184 public:
1185     static void TestInvRTCM(int argc = 0, char** argv = 0)
1186     {
1187         for (int k = 0; k < 999; ++k)
1188         {
1189             //0.GenerateData
1190             const int nDist = k % 2 == 0 ? 4 : 5;
1191             Mat_<Vec3d> point3D(22, 1); cv::randu(point3D, -999, 999);
1192             Mat_<double> K(4, 1); cv::randu(K, 111, 999);
1193             Mat_<double> D(nDist, 1); cv::randu(D, -0.2, 0.2);
1194             Mat_<double> r(3, 1); cv::randu(r, -999, 999);
1195             Mat_<double> t(3, 1); cv::randu(t, -999, 999);
1196             Mat_<Vec2d> point2D; projectPoints(point3D, r, t, Matx33d(K(0), 0, K(2), 0, K(1), K(3), 0, 0, 1), D, point2D);
1197 
1198             //1.CalcGT
1199             Mat_<Vec2d> point2D0; projectPoints(point3D, r, t, Mat_<double>::eye(3, 3), Mat_<double>::zeros(D.rows, 1), point2D0);
1200 
1201             //2.CalcByOpenCV
1202             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));
1203 
1204             //3.CalcByDIY
1205             Mat_<Vec2d> point2D2 = reproject(point2D, K.ptr<double>(), D.ptr<double>(), 0, D.rows, 0, VisionCM::RTCM, 0, 0, TermCriteria(TermCriteria::COUNT, 8, 1e-3));
1206 
1207             //4.AnalyzeError
1208             double infpts0pts1 = norm(point2D0, point2D1, NORM_INF);
1209             double infpts0pts2 = norm(point2D0, point2D2, NORM_INF);
1210             double infpts1pts2 = norm(point2D1, point2D2, NORM_INF);
1211             TermCriteria criteria(TermCriteria::COUNT + TermCriteria::EPS, k % 20 + 2, 1. / pow(10, k % 10 + 2));
1212             Mat_<double> P(4, 1); cv::randu(P, 111, 999);
1213             Mat_<double> R(3, 3); cv::Rodrigues(Vec3d::randu(-999, 999), R);
1214             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);
1215             Mat_<Vec2d> point2D4 = reproject(point2D, K.ptr<double>(), D.ptr<double>(), 0, D.rows, 0, VisionCM::RTCM, R.ptr<double>(), P.ptr<double>(), criteria);
1216             double infpts3pts4 = norm(point2D3, point2D4, NORM_INF);
1217 
1218             //5.PrintError
1219             cout << endl << "LoopCount: " << k << endl;
1220             if (/*infpts0pts1 > 1E-12 || infpts0pts2 > 1E-12 || */infpts1pts2 > 1E-12 || infpts3pts4 > 1E-12)
1221             {
1222                 cout << endl << "infpts0pts1: " << infpts0pts1 << endl;
1223                 cout << endl << "infpts0pts2: " << infpts0pts2 << endl;
1224                 cout << endl << "infpts1pts2: " << infpts1pts2 << endl;
1225                 cout << endl << "infpts3pts4: " << infpts3pts4 << endl;
1226                 if (0)
1227                 {
1228                     cout << endl << "point3D: " << point3D << endl;
1229                     cout << endl << "point2D0: " << point2D0 << endl;
1230                     cout << endl << "point2D1: " << point2D1 << endl;
1231                     cout << endl << "point2D2: " << point2D2 << endl;
1232                     cout << endl << "point2D3: " << point2D3 << endl;
1233                     cout << endl << "point2D4: " << point2D4 << endl;
1234                 }
1235                 cout << endl << "Press any key to continue" << endl; getchar();
1236             }
1237         }
1238     }
1239     static void TestInvKBCM(int argc = 0, char** argv = 0)
1240     {
1241         for (int k = 0; k < 999; ++k)
1242         {
1243             //0.GenerateData
1244             static const int nDist = 4;
1245             Mat_<Vec3d> point3D(22, 1); cv::randu(point3D, -999, 999);
1246             Mat_<double> K(4, 1); cv::randu(K, 111, 999);
1247             Mat_<double> D(4, 1); cv::randu(D, -0.2, 0.2);
1248             Mat_<double> r(3, 1); cv::randu(r, -999, 999);
1249             Mat_<double> t(3, 1); cv::randu(t, -999, 999);
1250             Mat_<Vec2d> point2D; fisheye::projectPoints(point3D, point2D, r, t, Matx33d(K(0), 0, K(2), 0, K(1), K(3), 0, 0, 1), D);
1251 
1252             //1.CalcGT
1253             Mat_<Vec2d> point2D0; fisheye::projectPoints(point3D, point2D0, r, t, Mat_<double>::eye(3, 3), Mat_<double>::zeros(D.rows, 1));
1254 
1255             //2.CalcByOpenCV
1256             Mat_<Vec2d> point2D1; fisheye::undistortPoints(point2D, point2D1, Matx33d(K(0), 0, K(2), 0, K(1), K(3), 0, 0, 1), D, noArray(), noArray());
1257 
1258             //3.CalcByDIY
1259             Mat_<Vec2d> point2D2 = reproject(point2D, K.ptr<double>(), D.ptr<double>(), 0, D.rows, 0, VisionCM::KBCM, 0, 0,
1260                 TermCriteria(TermCriteria::COUNT + TermCriteria::COUNT, 10, 1e-8));//criteria is fixed as (10, 1e-8) in opencv4.5 KBCM
1261 
1262             //4.AnalyzeError
1263             double infpts0pts1 = norm(point2D0, point2D1, NORM_INF);
1264             double infpts0pts2 = norm(point2D0, point2D2, NORM_INF);
1265             double infpts1pts2 = norm(point2D1, point2D2, NORM_INF);
1266             TermCriteria criteria(TermCriteria::COUNT + TermCriteria::EPS, k % 20 + 2, 1. / pow(10, k % 10 + 2));
1267             Mat_<double> P(4, 1); cv::randu(P, 111, 999);
1268             Mat_<double> R(3, 3); cv::Rodrigues(Vec3d::randu(-999, 999), R);
1269             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));
1270             Mat_<Vec2d> point2D4 = reproject(point2D, K.ptr<double>(), D.ptr<double>(), 0, D.rows, 0, VisionCM::KBCM, R.ptr<double>(), P.ptr<double>(),
1271                 TermCriteria(TermCriteria::COUNT + TermCriteria::COUNT, 10, 1e-8));//criteria is fixed as (10, 1e-8) in opencv4.5 KBCM
1272             double infpts3pts4 = norm(point2D3, point2D4, NORM_INF);
1273 
1274             //5.PrintError
1275             cout << endl << "LoopCount: " << k << endl;
1276             if (/*infpts0pts1 > 1E-9 || infpts0pts2 > 1E-9 ||*/ infpts1pts2 > 1E-9 || infpts3pts4 > 1E-9)
1277             {
1278                 cout << endl << "infpts0pts1: " << infpts0pts1 << endl;
1279                 cout << endl << "infpts0pts2: " << infpts0pts2 << endl;
1280                 cout << endl << "infpts1pts2: " << infpts1pts2 << endl;
1281                 cout << endl << "infpts3pts4: " << infpts3pts4 << endl;
1282                 if (0)
1283                 {
1284                     cout << endl << "point3D: " << point3D << endl;
1285                     cout << endl << "point2D0: " << point2D0 << endl;
1286                     cout << endl << "point2D1: " << point2D1 << endl;
1287                     cout << endl << "point2D2: " << point2D2 << endl;
1288                     cout << endl << "point2D3: " << point2D3 << endl;
1289                     cout << endl << "point2D4: " << point2D4 << endl;
1290                 }
1291                 cout << endl << "Press any key to continue" << endl; getchar();
1292             }
1293         }
1294     }
1295 
1296     static void TestInvMUCM()
1297     {
1298         auto omnidir_undistortPoints = [](InputArray distorted, OutputArray undistorted, InputArray K, InputArray D, InputArray xi, InputArray R)->Mat
1299         {
1300             CV_Assert(distorted.type() == CV_64FC2 || distorted.type() == CV_32FC2);
1301             CV_Assert(R.empty() || (!R.empty() && (R.size() == Size(3, 3) || R.total() * R.channels() == 3)
1302                 && (R.depth() == CV_64F || R.depth() == CV_32F)));
1303             CV_Assert((D.depth() == CV_64F || D.depth() == CV_32F) && D.total() == 4);
1304             CV_Assert(K.size() == Size(3, 3) && (K.depth() == CV_64F || K.depth() == CV_32F));
1305             CV_Assert(xi.total() == 1 && (xi.depth() == CV_64F || xi.depth() == CV_32F));
1306 
1307             undistorted.create(distorted.size(), distorted.type());
1308 
1309             cv::Vec2d f, c;
1310             double s = 0.0;
1311             if (K.depth() == CV_32F)
1312             {
1313                 Matx33f camMat = K.getMat();
1314                 f = Vec2f(camMat(0, 0), camMat(1, 1));
1315                 c = Vec2f(camMat(0, 2), camMat(1, 2));
1316                 s = (double)camMat(0, 1);
1317             }
1318             else if (K.depth() == CV_64F)
1319             {
1320                 Matx33d camMat = K.getMat();
1321                 f = Vec2d(camMat(0, 0), camMat(1, 1));
1322                 c = Vec2d(camMat(0, 2), camMat(1, 2));
1323                 s = camMat(0, 1);
1324             }
1325 
1326             Vec4d kp = D.depth() == CV_32F ? (Vec4d)*D.getMat().ptr<Vec4f>() : (Vec4d)*D.getMat().ptr<Vec4d>();
1327             Vec2d k = Vec2d(kp[0], kp[1]);
1328             Vec2d p = Vec2d(kp[2], kp[3]);
1329 
1330             double _xi = xi.depth() == CV_32F ? (double)*xi.getMat().ptr<float>() : *xi.getMat().ptr<double>();
1331             cv::Matx33d RR = cv::Matx33d::eye();
1332             // R is om
1333             if (!R.empty() && R.total() * R.channels() == 3)
1334             {
1335                 cv::Vec3d rvec;
1336                 R.getMat().convertTo(rvec, CV_64F);
1337                 cv::Rodrigues(rvec, RR);
1338             }
1339             else if (!R.empty() && R.size() == Size(3, 3))
1340             {
1341                 R.getMat().convertTo(RR, CV_64F);
1342             }
1343 
1344             const cv::Vec2d* srcd = distorted.getMat().ptr<cv::Vec2d>();
1345             const cv::Vec2f* srcf = distorted.getMat().ptr<cv::Vec2f>();
1346 
1347             cv::Vec2d* dstd = undistorted.getMat().ptr<cv::Vec2d>();
1348             cv::Vec2f* dstf = undistorted.getMat().ptr<cv::Vec2f>();
1349 
1350             Mat_<Vec3d> p3D;//############################################################Add
1351             int n = (int)distorted.total();
1352             for (int i = 0; i < n; i++)
1353             {
1354                 Vec2d pii = distorted.depth() == CV_32F ? (Vec2d)srcf[i] : (Vec2d)srcd[i];    // image point
1355                 Vec2d pp((pii[0] * f[1] - c[0] * f[1] - s * (pii[1] - c[1])) / (f[0] * f[1]), (pii[1] - c[1]) / f[1]); //plane
1356                 Vec2d pu = pp;    // points without distortion
1357 
1358                 // remove distortion iteratively
1359                 for (int j = 0; j < 20; j++)
1360                 {
1361                     double r2 = pu[0] * pu[0] + pu[1] * pu[1];
1362                     double r4 = r2 * r2;
1363                     pu[0] = (pp[0] - 2 * p[0] * pu[0] * pu[1] - p[1] * (r2 + 2 * pu[0] * pu[0])) / (1 + k[0] * r2 + k[1] * r4);
1364                     pu[1] = (pp[1] - 2 * p[1] * pu[0] * pu[1] - p[0] * (r2 + 2 * pu[1] * pu[1])) / (1 + k[0] * r2 + k[1] * r4);
1365                 }
1366 
1367                 // project to unit sphere
1368                 double r2 = pu[0] * pu[0] + pu[1] * pu[1];
1369                 double a = (r2 + 1);
1370                 double b = 2 * _xi * r2;
1371                 double cc = r2 * _xi * _xi - 1;
1372                 double Zs = (-b + sqrt(b * b - 4 * a * cc)) / (2 * a);
1373                 Vec3d Xw = Vec3d(pu[0] * (Zs + _xi), pu[1] * (Zs + _xi), Zs);
1374 
1375                 // rotate
1376                 Xw = RR * Xw;
1377                 p3D.push_back(Xw);//############################################################Add
1378 
1379                 // project back to sphere
1380                 Vec3d Xs = Xw / cv::norm(Xw);
1381 
1382                 // reproject to camera plane
1383                 Vec3d ppu = Vec3d(Xs[0] / (Xs[2] + _xi), Xs[1] / (Xs[2] + _xi), 1.0);
1384                 if (undistorted.depth() == CV_32F)
1385                 {
1386                     dstf[i] = Vec2f((float)ppu[0], (float)ppu[1]);
1387                 }
1388                 else if (undistorted.depth() == CV_64F)
1389                 {
1390                     dstd[i] = Vec2d(ppu[0], ppu[1]);
1391                 }
1392             }
1393             return p3D;//############################################################Add
1394         };
1395         for (int k = 0; k < 999; ++k)
1396         {
1397             //0.GenerateData
1398             Mat_<Vec3d> point3D(4, 1); cv::randu(point3D, -999, 999);
1399             Mat_<double> K(4, 1); cv::randu(K, 111, 999);
1400             Mat_<double> D(4, 1); cv::randu(D, -0, 0); //distortion is tested by testInvRTCM
1401             Mat_<double> a(1, 1); cv::randu(a, 1, 9); //only for fisheye and so A>1
1402             Mat_<double> r(3, 1); cv::randu(r, -999, 999);
1403             Mat_<double> t(3, 1); cv::randu(t, -999, 999);
1404             Mat_<Vec2d> point2D; omnidir::projectPoints(point3D, point2D, r, t, Mat_<double>({ 3, 3 }, { K(0), 0, K(2), 0, K(1), K(3), 0, 0, 1 }), a(0), D);
1405             Mat_<double> R; cv::Rodrigues(r, R);
1406 
1407             //1.CalcGT
1408             Mat_<Vec3d> point3D0;
1409             for (int i = 0; i < point3D.rows; ++i) point3D0.push_back(Vec3d(Mat_<double>(R * Mat_<double>(3, 1, point3D(i).val) + t).ptr<double>()));
1410 
1411             //2.CalcByOpenCV
1412             Mat_<Vec3d> point3D1 = ModelCamera::reproject(point2D, K.ptr<double>(), D.ptr<double>(), a.ptr<double>(), 4, 1, ModelCamera::VisionCM::MUCM, 0, 0);
1413 
1414             //3.CalcByDIY
1415             Mat unPt;
1416             Mat_<Vec3d> point3D2 = omnidir_undistortPoints(point2D, unPt, Mat_<double>({ 3, 3 }, { K(0), 0, K(2), 0, K(1), K(3), 0, 0, 1 }), D, a, Mat_<double>::eye(3, 3));
1417 
1418             //4.AnalyzeError
1419             Mat_<Vec3d> point3D3 = ModelCamera::reproject(point2D, K.ptr<double>(), D.ptr<double>(), a.ptr<double>(), 4, 1, ModelCamera::VisionCM::MUCM, R.ptr<double>(), 0);
1420             Mat_<Vec3d> point3D4 = omnidir_undistortPoints(point2D, unPt, Mat_<double>({ 3, 3 }, { K(0), 0, K(2), 0, K(1), K(3), 0, 0, 1 }), D, a, R);
1421             Mat_<double> infP0P1P2;
1422             for (int i = 0; i < point3D.rows; ++i)
1423             {
1424                 Vec3d P0 = point3D0(i);
1425                 Vec3d P1 = point3D1(i), P2 = point3D2(i);
1426                 Vec3d P3 = point3D3(i), P4 = point3D4(i);
1427                 double ang1 = acos(P0.dot(P1) / sqrt(P0.dot(P0) * P1.dot(P1))) * rad2deg;
1428                 double ang2 = acos(P0.dot(P2) / sqrt(P0.dot(P0) * P2.dot(P2))) * rad2deg;
1429                 double ang12 = acos(P1.dot(P2) / sqrt(P1.dot(P1) * P2.dot(P2))) * rad2deg;
1430                 double ang34 = acos(P3.dot(P4) / sqrt(P3.dot(P3) * P4.dot(P4))) * rad2deg;
1431                 infP0P1P2.push_back(Vec4d(ang1, ang2, ang12, ang34));
1432             }
1433 
1434             //5.PrintError
1435             cout << endl << "LoopCount: " << k << endl;//Half part is not close to ground truth because of random input
1436             if (infP0P1P2(0) > 1E-12 || infP0P1P2(1) > 1E-12 || infP0P1P2(2) > 1E-12)
1437             {
1438                 cout << endl << "infP0P1: " << infP0P1P2(0) << endl;
1439                 cout << endl << "infP0P2: " << infP0P1P2(1) << endl;
1440                 cout << endl << "infP1P2: " << infP0P1P2(2) << endl;
1441                 cout << endl << "infP3P4: " << infP0P1P2(3) << endl;
1442                 if (0)
1443                 {
1444                     cout << endl << "point3D0: " << point3D0 << endl;
1445                     cout << endl << "point3D1: " << point3D1 << endl;
1446                     cout << endl << "point3D2: " << point3D2 << endl;
1447                     cout << endl << "point3D3: " << point3D3 << endl;
1448                     cout << endl << "point3D4: " << point3D4 << endl;
1449                 }
1450                 cout << "Press any key to continue" << endl; getchar();
1451             }
1452         }
1453     }
1454 
1455     static void TestInvEUCM(int argc = 0, char** argv = 0) {}
1456     static void TestInvDSCM(int argc = 0, char** argv = 0) {}
1457 
1458 public:
1459     static void TestJacRTCM(int argc = 0, char** argv = 0)
1460     {
1461         for (int k = 0; k < 999; ++k)
1462         {
1463             static const int nRot = 3;//fixed
1464             static const int nDist = 5;//varied
1465             static const int nModel = 0;//fixed
1466             Mat_<Vec3d> point3D(2, 1); cv::randu(point3D, -999, 999);
1467             Mat_<double> K(4, 1); cv::randu(K, 111, 999);
1468             Mat_<double> D(nDist, 1); cv::randu(D, -1.0, 1.0);
1469             Mat_<double> r(3, 1); cv::randu(r, -999, 999);
1470             Mat_<double> t(3, 1); cv::randu(t, -999, 999);
1471 
1472             //1.CalcByOpenCV
1473             Mat_<Vec2d> point2D1;
1474             Mat_<double> dpdKDT1;
1475             projectPoints(point3D, r, t, Matx33d(K(0), 0, K(2), 0, K(1), K(3), 0, 0, 1), D, point2D1, dpdKDT1);
1476             Mat_<double> dpdr1 = dpdKDT1.colRange(0, 3);
1477             Mat_<double> dpdt1 = dpdKDT1.colRange(3, 6);
1478             Mat_<double> dpdK1 = dpdKDT1.colRange(6, 10);
1479             Mat_<double> dpdD1 = dpdKDT1.colRange(10, 10 + D.rows);
1480 
1481             //2.CalcByManuMap
1482             Mat_<double> dpdr2, dpdt2, dpdK2, dpdD2;
1483             Mat_<Vec2d> point2D2 = derivRTCM(point3D, r.ptr<double>(), t.ptr<double>(), K.ptr<double>(), D.ptr<double>(), D.rows,
1484                 &dpdr2, &dpdt2, &dpdK2, &dpdD2, 0, 0);
1485 
1486             //2.CalcByManuBCH
1487             Mat_<double> dpdr3, dpdt3, dpdK3, dpdD3;
1488             Mat_<Vec2d> point2D3 = derivRTCM(point3D, r.ptr<double>(), t.ptr<double>(), K.ptr<double>(), D.ptr<double>(), D.rows,
1489                 &dpdr3, &dpdt3, &dpdK3, &dpdD3, 0, 1);
1490 
1491             //4.AnalyzeError
1492             double infpts1pts2 = norm(point2D1, point2D2, NORM_INF);
1493             double infpts1pts3 = norm(point2D1, point2D3, NORM_INF);
1494             double infdpdr1r2 = norm(dpdr1, dpdr2, NORM_INF);
1495             double infdpdr1r3 = norm(dpdr1, dpdr3, NORM_INF);
1496             double infdpdr2r3 = norm(dpdr2, dpdr3, NORM_INF);
1497             double infdpdt1t2 = norm(dpdt1, dpdt2, NORM_INF);
1498             double infdpdt1t3 = norm(dpdt1, dpdt3, NORM_INF);
1499             double infdpdt2t3 = norm(dpdt2, dpdt3, NORM_INF);
1500             double infdpdK1K2 = norm(dpdK1, dpdK2, NORM_INF);
1501             double infdpdK1K3 = norm(dpdK1, dpdK3, NORM_INF);
1502             double infdpdK2K3 = norm(dpdK2, dpdK3, NORM_INF);
1503             double infdpdD1D2 = norm(dpdD1, dpdD2, NORM_INF);
1504             double infdpdD1D3 = norm(dpdD1, dpdD3, NORM_INF);
1505             double infdpdD2D3 = norm(dpdD2, dpdD3, NORM_INF);
1506 
1507             //5.PrintError
1508             cout << fmt::format("LoopCount: {}\n", k);
1509             if (infpts1pts2 > 1e-9 || infpts1pts3 > 1e-9 ||
1510                 infdpdr1r2 > 1e-9 || infdpdr1r3 > 1e-9 || infdpdr2r3 > 1e-9 || infdpdt1t2 > 1e-9 || infdpdt1t3 > 1e-9 || infdpdt2t3 > 1e-9 ||
1511                 infdpdK1K2 > 1e-9 || infdpdK1K3 > 1e-9 || infdpdK2K3 > 1e-9 || infdpdD1D2 > 1e-9 || infdpdD1D3 > 1e-9 || infdpdD2D3 > 1e-9)
1512             {
1513                 cout << endl << "infpts1pts2: " << infpts1pts2 << endl;
1514                 cout << endl << "infpts1pts3: " << infpts1pts3 << endl;
1515                 cout << endl << "infdpdr1r2: " << infdpdr1r2 << endl;
1516                 cout << endl << "infdpdr1r3: " << infdpdr1r3 << endl;
1517                 cout << endl << "infdpdr2r3: " << infdpdr2r3 << endl;
1518                 cout << endl << "infdpdt1t2: " << infdpdt1t2 << endl;
1519                 cout << endl << "infdpdt1t3: " << infdpdt1t3 << endl;
1520                 cout << endl << "infdpdt2t3: " << infdpdt2t3 << endl;
1521                 cout << endl << "infdpdK1K2: " << infdpdK1K2 << endl;
1522                 cout << endl << "infdpdK1K3: " << infdpdK1K3 << endl;
1523                 cout << endl << "infdpdK2K3: " << infdpdK2K3 << endl;
1524                 cout << endl << "infdpdD1D2: " << infdpdD1D2 << endl;
1525                 cout << endl << "infdpdD1D3: " << infdpdD1D3 << endl;
1526                 cout << endl << "infdpdD2D3: " << infdpdD2D3 << endl;
1527                 if (0)
1528                 {
1529                     cout << "point2D1: " << point2D1 << endl;
1530                     cout << "point2D2: " << point2D2 << endl;
1531                     cout << "point2D3: " << point2D3 << endl;
1532                 }
1533                 cout << "Press any key to continue" << endl; getchar();
1534             }
1535         }
1536     }
1537 
1538 public:
1539     static void TestRectBino(int argc = 0, char** argv = 0)
1540     {
1541         for (int k = 0; k < 999; ++k)
1542         {
1543             //0.GenerateData
1544             Mat_<double> R(3, 3); ceres::EulerAnglesToRotationMatrix(Matx31d::randu(-15, 15).val, 0, R.ptr<double>());
1545             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
1546             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
1547 
1548             //1.CalcByOpenCVPin
1549             Matx33d R11, R12;
1550             stereoRectify(Matx33d::eye(), Mat(), Matx33d::eye(), Mat(), Size(480, 640), R, t, R11, R12, Matx34d(), Matx34d(), noArray());
1551             double infP11P12 = cv::norm(R11 * P, R12 * R * P, NORM_INF);
1552 
1553             //2.CalcByOpenCVKB
1554             Matx33d R21, R22;
1555             fisheye::stereoRectify(Matx33d::eye(), Matx41d::zeros(), Matx33d::eye(), Matx41d::zeros(), Size(480, 640), R, t, R21, R22, Matx34d(), Matx34d(), noArray(), 0);
1556             double infP21P22 = cv::norm(R21 * P, R22 * R * P, NORM_INF);
1557 
1558             //3.CalcByDIYPin
1559             Matx33d R31, R32;
1560             rectBino(R31.val, R32.val, R.ptr<double>(), t.ptr<double>());
1561             double infP31P32 = cv::norm(R31 * P, R32 * R * P, NORM_INF);
1562 
1563             //4.CalcByDIYMUCM
1564             Matx33d R41, R42;
1565             rectBino2(R41.val, R42.val, R.ptr<double>(), t.ptr<double>());
1566             double infP41P42 = cv::norm(R41 * P, R42 * R * P, NORM_INF);
1567 
1568             //5.CalcByOpenCVMUCM
1569             Matx33d R51, R52;
1570             omnidir::stereoRectify(R, t, R51, R52);
1571             double infP51P52 = cv::norm(R51 * P, R52 * R * P, NORM_INF);
1572             string ss = "   It is OK to rectify the last line {_R2 = R21 * _R1} in omnidir::stereoRectify {_R2 = _R1 * R21}";
1573 
1574             //6.AnalyzeError
1575             double infR11R21 = norm(R11, R21, NORM_INF);
1576             double infR11R31 = norm(R11, R31, NORM_INF);
1577             double infR41R51 = norm(R41, R51, NORM_INF);
1578             double infR12R22 = norm(R12, R22, NORM_INF);
1579             double infR12R32 = norm(R12, R32, NORM_INF);
1580             double infR42R52 = norm(R42, R52, NORM_INF);
1581 
1582             //7.PrintError
1583             cout << endl << "LoopCount: " << k << ss << endl;
1584             if (infP11P12 > 1E-12 || infP21P22 > 1E-12 || infP31P32 > 1E-12 || infP41P42 > 1E-12 || //infP51P52 > 1E-12 ||
1585                 infR11R21 > 1E-12 || infR11R31 > 1E-12 || infR12R22 > 1E-12 || infR12R32 > 1E-12 /*|| infR41R51 > 1E-12 || infR42R52 > 1E-12*/)
1586             {
1587                 cout << endl << "infP11P12: " << infP11P12 << endl;
1588                 cout << endl << "infP21P22: " << infP21P22 << endl;
1589                 cout << endl << "infP31P32: " << infP31P32 << endl;
1590                 cout << endl << "infP41P42: " << infP41P42 << endl;
1591                 cout << endl << "infP51P52: " << infP51P52 << endl;
1592                 cout << endl << "infR11R21: " << infR11R21 << endl;
1593                 cout << endl << "infR11R31: " << infR11R31 << endl;
1594                 cout << endl << "infR11R51: " << infR41R51 << endl;
1595                 cout << endl << "infR12R22: " << infR12R22 << endl;
1596                 cout << endl << "infR12R32: " << infR12R32 << endl;
1597                 cout << endl << "infR12R52: " << infR42R52 << endl;
1598                 if (1)
1599                 {
1600                     cout << endl << "R: " << R << endl << endl;
1601                     cout << endl << "t: " << t << endl << endl;
1602                     cout << endl << "P: " << P << endl << endl;
1603                     //cout << endl << "R11: " << R11 << endl;
1604                     //cout << endl << "R12: " << R12 << endl;
1605                     //cout << endl << "R21: " << R21 << endl;
1606                     //cout << endl << "R22: " << R22 << endl;
1607                     //cout << endl << "R31: " << R31 << endl;
1608                     //cout << endl << "R32: " << R32 << endl;
1609                     //cout << endl << "R41: " << R41 << endl;
1610                     //cout << endl << "R42: " << R42 << endl;
1611                     //cout << endl << "R51: " << R51 << endl;
1612                     //cout << endl << "R52: " << R52 << endl;
1613                 }
1614                 cout << endl << "Press any key to continue" << endl; getchar();
1615             }
1616         }
1617     }
1618 };
1619 
1620 #ifdef ModelCameraTest
1621 int main(int argc, char** argv) { ModelCamera::TestRectBino(argc, argv); return 0; }
1622 int main1(int argc, char** argv) { ModelCamera::TestRTCM(argc, argv); return 0; }
1623 int main2(int argc, char** argv) { ModelCamera::TestKBCM(argc, argv); return 0; }
1624 int main3(int argc, char** argv) { ModelCamera::TestMUCM(argc, argv); return 0; }
1625 int main4(int argc, char** argv) { ModelCamera::TestEUCM(argc, argv); return 0; }
1626 int main5(int argc, char** argv) { ModelCamera::TestDSCM(argc, argv); return 0; }
1627 int main6(int argc, char** argv) { ModelCamera::TestInvRTCM(argc, argv); return 0; }
1628 int main7(int argc, char** argv) { ModelCamera::TestInvKBCM(argc, argv); return 0; }
1629 int main8(int argc, char** argv) { ModelCamera::TestInvMUCM(argc, argv); return 0; }
1630 int main9(int argc, char** argv) { ModelCamera::TestInvEUCM(argc, argv); return 0; }
1631 int main10(int argc, char** argv) { ModelCamera::TestInvDSCM(argc, argv); return 0; }
1632 int main11(int argc, char** argv) { ModelCamera::TestJacRTCM(argc, argv); return 0; }
1633 int main21(int argc, char** argv) { ModelCamera::TestRectBino(argc, argv); return 0; }
1634 #endif
1635 
1636 #endif
View Code
会员力量，点亮园子希望
刷新页面返回顶部
CV基础知识

导航

公告

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