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