之前已经介绍过Ceres自动求导的使用样例，详见《Ceres-Solver学习日志：自动求导使用样例与针孔成像器的应用》，这里介绍手动求导使用样例，并对比与cv::LMSolver的使用差异。
1.定义代价类
手动求导的核心就是继承CostFunction或DynamicCostFunction或SizedCostFunction，继承后的主要工作就是重载Evaluate函数，实现残差和雅可比矩阵的计算。
继承CostFunction或DynamicCostFunction适用于在参数数量和残差数量都不能在编译时确定的情况，两者的区别是设置参数数量和残差数量的接口不一样，DynamicCostFuntion的设置接口就是对CostFunction设置接口的简单封装，查看源码可知无本质区别，之所以定义DynamicCostFuntion类可能是为了与SizedCostFunction对应。
继承SizedCostFunction适用于至少参数数量能在编译时确定的情况，残差数量未知则将对应的模板参数设为DYNAMIC即可。
定义多少个代价类的规则同定义多少个残差类，具体参见自动求导使用样例一文。
2.使用代价类
与如何使用残差类相似，具体参见自动求导使用样例一文。
3.使用cv::LMSolver
对比使用样例代码即可知晓。
4.使用样例
提供两个使用样例，封装为两个类：
OptimizeRt：基于单次观察，优化此次观察的外参，定义了CeresCostRt和CvCallbackRt两个类。
OptimizeKDRt：基于多次观察，优化相机内参和所有观察的外参，定义了CeresCostKDRt和CvCallbackKDRt两个类。
其中类MotionSim用于生成仿真数据，使用说明参见《CV学习日志：CV开发之三维仿真器》。
存在可能不收敛的测试结果，属于正常现象，可修改初值精度来增加收敛性。
关于测试结果说明：从基于仿真数据的测试结果来看，Ceres收敛次数比cv::LMSolver多，但在收敛的情况下，cv::LMSolver在迭代次数和收敛精度上比Ceres好。
以下是详细代码，依赖于C++14、OpenCV4.x、Ceres和Spdlog。
1 #include <opencv2/opencv.hpp> 2 #include <opencv2/viz.hpp> 3 #include <spdlog/spdlog.h> 4 #include <ceres/ceres.h> 5 using namespace std; 6 using namespace cv; 7 8 class MotionSim 9 { 10 public: 11 static void TestMe(int argc, char** argv) 12 { 13 MotionSim motionSim(false); 14 motionSim.camFovX = 45; 15 motionSim.camFovY = 30; 16 motionSim.camRand = 10; 17 motionSim.enableVerbose = false; 18 motionSim.runMotion(false, false, 7); 19 motionSim.visMotion(); 20 } 21 22 public: 23 struct MotionView 24 { 25 Mat_<double> r = Mat_<double>(3, 1); 26 Mat_<double> t = Mat_<double>(3, 1); 27 Mat_<double> q = Mat_<double>(4, 1); 28 Mat_<double> rt = Mat_<double>(6, 1); 29 Mat_<double> radian = Mat_<double>(3, 1); 30 Mat_<double> degree = Mat_<double>(3, 1); 31 Mat_<double> R = Mat_<double>(3, 3); 32 Mat_<double> T = Mat_<double>(3, 4); 33 Mat_<double> K; 34 Mat_<double> D; 35 Mat_<Vec3d> point3D; 36 Mat_<Vec2d> point2D; 37 Mat_<int> point3DIds; 38 string print(string savePath = "") 39 { 40 string str; 41 str += fmt::format("r: {} ", cvarr2str(r.t())); 42 str += fmt::format("t: {} ", cvarr2str(t.t())); 43 str += fmt::format("q: {} ", cvarr2str(q.t())); 44 str += fmt::format("rt: {} ", cvarr2str(rt.t())); 45 str += fmt::format("radian: {} ", cvarr2str(radian.t())); 46 str += fmt::format("degree: {} ", cvarr2str(degree.t())); 47 str += fmt::format("R: {} ", cvarr2str(R)); 48 str += fmt::format("T: {} ", cvarr2str(T)); 49 str += fmt::format("K: {} ", cvarr2str(K)); 50 str += fmt::format("D: {} ", cvarr2str(D.t())); 51 if (savePath.empty() == false) { FILE* out = fopen(savePath.c_str(), "w"); fprintf(out, str.c_str()); fclose(out); } 52 return str; 53 } 54 }; 55 static string cvarr2str(InputArray v) 56 { 57 Ptr<Formatted> fmtd = cv::format(v, Formatter::FMT_DEFAULT); 58 string dst; fmtd->reset(); 59 for (const char* str = fmtd->next(); str; str = fmtd->next()) dst += string(str); 60 return dst; 61 } 62 static void euler2matrix(double e[3], double R[9], bool forward = true, int argc = 0, char** argv = 0) 63 { 64 if (argc > 0) 65 { 66 int N = 999; 67 for (int k = 0; k < N; ++k)//OpenCV not better than DIY 68 { 69 //1.GenerateData 70 Matx31d radian0 = radian0.randu(-3.14159265358979323846, 3.14159265358979323846); 71 Matx33d R; euler2matrix(radian0.val, R.val, true); 72 const double deg2rad = 3.14159265358979323846 * 0.0055555555555555556; 73 const double rad2deg = 180 * 0.3183098861837906715; 74 75 //2.CalcByOpenCV 76 Matx31d radian1 = cv::RQDecomp3x3(R, Matx33d(), Matx33d()) * deg2rad; 77 78 //3.CalcByDIY 79 Matx31d radian2; euler2matrix(R.val, radian2.val, false); 80 81 //4.AnalyzeError 82 double infRadian0Radian1 = norm(radian0, radian1, NORM_INF); 83 double infRadian1Radian2 = norm(radian1, radian2, NORM_INF); 84 85 //5.PrintError 86 cout << endl << "LoopCount: " << k << endl; 87 if (infRadian0Radian1 > 0 || infRadian1Radian2 > 0) 88 { 89 cout << endl << "5.1PrintError" << endl; 90 cout << endl << "infRadian0Radian1: " << infRadian0Radian1 << endl; 91 cout << endl << "infRadian1Radian2: " << infRadian1Radian2 << endl; 92 if (0) 93 { 94 cout << endl << "5.2PrintDiff" << endl; 95 cout << endl << "radian0-degree0:" << endl << radian0.t() << endl << radian0.t() * rad2deg << endl; 96 cout << endl << "radian1-degree1:" << endl << radian1.t() << endl << radian1.t() * rad2deg << endl; 97 cout << endl << "radian2-degree2:" << endl << radian2.t() << endl << radian2.t() * rad2deg << endl; 98 cout << endl << "5.3PrintOthers" << endl; 99 cout << endl << "R:" << endl << R << endl; 100 } 101 cout << endl << "Press any key to continue" << endl; std::getchar(); 102 } 103 } 104 return; 105 } 106 if (forward)//check with 3D Rotation Converter 107 { 108 double sinR = std::sin(e[0]); 109 double sinP = std::sin(e[1]); 110 double sinY = std::sin(e[2]); 111 double cosR = std::cos(e[0]); 112 double cosP = std::cos(e[1]); 113 double cosY = std::cos(e[2]); 114 115 //RPY indicates: first Yaw aroundZ, second Pitch aroundY, third Roll aroundX 116 R[0] = cosY * cosP; R[1] = cosY * sinP * sinR - sinY * cosR; R[2] = cosY * sinP * cosR + sinY * sinR; 117 R[3] = sinY * cosP; R[4] = sinY * sinP * sinR + cosY * cosR; R[5] = sinY * sinP * cosR - cosY * sinR; 118 R[6] = -sinP; R[7] = cosP * sinR; R[8] = cosP * cosR; 119 } 120 else 121 { 122 double vs1 = std::abs(R[6] - 1.); 123 double vs_1 = std::abs(R[6] + 1.); 124 if (vs1 > 1E-9 && vs_1 > 1E-9) 125 { 126 e[2] = std::atan2(R[3], R[0]); //Yaw aroundZ 127 e[1] = std::asin(-R[6]);//Pitch aroundY 128 e[0] = std::atan2(R[7], R[8]); //Roll aroundX 129 } 130 else if (vs_1 <= 1E-9) 131 { 132 e[2] = 0; //Yaw aroundZ 133 e[1] = 3.14159265358979323846 * 0.5;//Pitch aroundY 134 e[0] = e[2] + atan2(R[1], R[2]); //Roll aroundX 135 } 136 else 137 { 138 e[2] = 0; //Yaw aroundZ 139 e[1] = -3.14159265358979323846 * 0.5;//Pitch aroundY 140 e[0] = -e[2] + atan2(-R[1], -R[2]); //Roll aroundX 141 } 142 } 143 }; 144 static void quat2matrix(double q[4], double R[9], bool forward = true) 145 { 146 if (forward)//refer to qglviwer 147 { 148 double L1 = std::sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); 149 if (std::abs(L1 - 1) > 1E-9) { std::printf("Not uint quaternion: NormQ=%.9f ", L1); abort(); } 150 151 double xx = 2.0 * q[1] * q[1]; 152 double yy = 2.0 * q[2] * q[2]; 153 double zz = 2.0 * q[3] * q[3]; 154 155 double xy = 2.0 * q[1] * q[2]; 156 double xz = 2.0 * q[1] * q[3]; 157 double wx = 2.0 * q[1] * q[0]; 158 159 double yz = 2.0 * q[2] * q[3]; 160 double wy = 2.0 * q[2] * q[0]; 161 162 double wz = 2.0 * q[3] * q[0]; 163 164 R[0] = 1.0 - yy - zz; 165 R[4] = 1.0 - xx - zz; 166 R[8] = 1.0 - xx - yy; 167 168 R[1] = xy - wz; 169 R[3] = xy + wz; 170 171 R[2] = xz + wy; 172 R[6] = xz - wy; 173 174 R[5] = yz - wx; 175 R[7] = yz + wx; 176 } 177 else 178 { 179 double onePlusTrace = 1.0 + R[0] + R[4] + R[8];// Compute one plus the trace of the matrix 180 if (onePlusTrace > 1E-9) 181 { 182 double s = sqrt(onePlusTrace) * 2.0; 183 double is = 1 / s; 184 q[0] = 0.25 * s; 185 q[1] = (R[7] - R[5]) * is; 186 q[2] = (R[2] - R[6]) * is; 187 q[3] = (R[3] - R[1]) * is; 188 } 189 else 190 { 191 std::printf("1+trace(R)=%.9f is too small and (R11,R22,R33)=(%.9f,%.9f,%.9f) ", onePlusTrace, R[0], R[4], R[8]); 192 if ((R[0] > R[4]) && (R[0] > R[8]))//max(R00, R11, R22)=R00 193 { 194 double s = sqrt(1.0 + R[0] - R[4] - R[8]) * 2.0; 195 double is = 1 / s; 196 q[0] = (R[5] - R[7]) * is; 197 q[1] = 0.25 * s; 198 q[2] = (R[1] + R[3]) * is; 199 q[3] = (R[2] + R[6]) * is; 200 } 201 else if (R[4] > R[8])//max(R00, R11, R22)=R11 202 { 203 double s = sqrt(1.0 - R[0] + R[4] - R[8]) * 2.0; 204 double is = 1 / s; 205 q[0] = (R[2] - R[6]) * is; 206 q[1] = (R[1] + R[3]) * is; 207 q[2] = 0.25 * s; 208 q[3] = (R[5] + R[7]) * is; 209 } 210 else//max(R00, R11, R22)=R22 211 { 212 double s = sqrt(1.0 - R[0] - R[4] + R[8]) * 2.0; 213 double is = 1 / s; 214 q[0] = (R[1] - R[3]) * is; 215 q[1] = (R[2] + R[6]) * is; 216 q[2] = (R[5] + R[7]) * is; 217 q[3] = 0.25 * s; 218 } 219 } 220 double L1 = std::sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); 221 if (L1 < 1e-9) { std::printf("Wrong rotation matrix: NormQ=%.9f ", L1); abort(); } 222 else { L1 = 1 / L1; q[0] *= L1; q[1] *= L1; q[2] *= L1; q[3] *= L1; } 223 } 224 } 225 static void vec2quat(double r[3], double q[4], bool forward = true) 226 { 227 if (forward)//refer to qglviwer 228 { 229 double theta = std::sqrt(r[0] * r[0] + r[1] * r[1] + r[2] * r[2]); 230 if (std::abs(theta) < 1E-9) 231 { 232 q[0] = 1; q[1] = q[2] = q[3] = 0; 233 std::printf("Rotation approximates zero: Theta=%.9f ", theta); 234 }; 235 236 q[0] = std::cos(theta * 0.5); 237 double ss = std::sin(theta * 0.5) / theta; 238 q[1] = r[0] * ss; 239 q[2] = r[1] * ss; 240 q[3] = r[2] * ss; 241 } 242 else 243 { 244 double L1 = std::sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); 245 if (std::abs(L1 - 1) > 1E-9) { std::printf("Not uint quaternion: NormQ=%.9f ", L1); abort(); } 246 247 double theta = 2 * acos(q[0]); 248 if (theta > 3.14159265358979323846) theta = 2 * 3.14159265358979323846 - theta; 249 double thetaEx = theta / std::sin(theta * 0.5); 250 r[0] = q[1] * thetaEx; 251 r[1] = q[2] * thetaEx; 252 r[2] = q[3] * thetaEx; 253 } 254 } 255 static void vec2matrix(double r[3], double R[9], bool forward = true, int argc = 0, char** argv = 0) 256 { 257 if (argc > 0) 258 { 259 int N = 999; 260 for (int k = 0; k < N; ++k) //refer to the subsequent article for more details 261 { 262 //1.GenerateData 263 Matx31d r0 = r0.randu(-999, 999); 264 Matx33d R0; cv::Rodrigues(r0, R0); 265 266 //2.CalcByOpenCV 267 Matx33d R1; 268 Matx31d r1; 269 cv::Rodrigues(r0, R1); 270 cv::Rodrigues(R0, r1); 271 272 //3.CalcByDIY 273 Matx33d R2; 274 Matx31d r2; 275 vec2matrix(r0.val, R2.val, true); 276 vec2matrix(r2.val, R0.val, false); 277 278 //4.AnalyzeError 279 double infR1R2 = norm(R1, R2, NORM_INF); 280 double infr1r2 = norm(r1, r2, NORM_INF); 281 282 //5.PrintError 283 cout << endl << "LoopCount: " << k << endl; 284 if (infR1R2 > 1E-12 || infr1r2 > 1E-12) 285 { 286 cout << endl << "5.1PrintError" << endl; 287 cout << endl << "infR1R2: " << infR1R2 << endl; 288 cout << endl << "infr1r2: " << infr1r2 << endl; 289 if (0) 290 { 291 cout << endl << "5.2PrintDiff" << endl; 292 cout << endl << "R1: " << endl << R1 << endl; 293 cout << endl << "R2: " << endl << R2 << endl; 294 cout << endl; 295 cout << endl << "r1: " << endl << r1.t() << endl; 296 cout << endl << "r2: " << endl << r2.t() << endl; 297 cout << endl << "5.3PrintOthers" << endl; 298 } 299 cout << endl << "Press any key to continue" << endl; std::getchar(); 300 } 301 } 302 return; 303 } 304 305 if (forward) 306 { 307 double theta = std::sqrt(r[0] * r[0] + r[1] * r[1] + r[2] * r[2]); 308 if (theta < 1E-9) 309 { 310 R[0] = R[4] = R[8] = 1.0; 311 R[1] = R[2] = R[3] = R[5] = R[6] = R[7] = 0.0; 312 std::printf("Rotation approximates zero: Theta=%.9f ", theta); 313 return; 314 } 315 double cs = cos(theta); 316 double sn = sin(theta); 317 double itheta = 1. / theta; 318 double cs1 = 1 - cs; 319 double nx = r[0] * itheta; 320 double ny = r[1] * itheta; 321 double nz = r[2] * itheta; 322 323 double nxnx = nx * nx, nyny = ny * ny, nznz = nz * nz; 324 double nxny = nx * ny, nxnz = nx * nz, nynz = ny * nz; 325 double nxsn = nx * sn, nysn = ny * sn, nzsn = nz * sn; 326 327 R[0] = nxnx * cs1 + cs; 328 R[3] = nxny * cs1 + nzsn; 329 R[6] = nxnz * cs1 - nysn; 330 331 R[1] = nxny * cs1 - nzsn; 332 R[4] = nyny * cs1 + cs; 333 R[7] = nynz * cs1 + nxsn; 334 335 R[2] = nxnz * cs1 + nysn; 336 R[5] = nynz * cs1 - nxsn; 337 R[8] = nznz * cs1 + cs; 338 339 if (0) 340 { 341 Mat_<double> dRdu({ 9, 4 }, { 342 2 * nx * cs1, 0, 0, (nxnx - 1) * sn, 343 ny * cs1, nx * cs1, -sn, nxny * sn - nz * cs, 344 nz * cs1, sn, nx * cs1, nxnz * sn + ny * cs, 345 ny * cs1, nx * cs1, sn, nxny * sn + nz * cs, 346 0, 2 * ny * cs1, 0, (nyny - 1) * sn, 347 -sn, nz * cs1, ny * cs1, nynz * sn - nx * cs, 348 nz * cs1, -sn, nx * cs1, nxnz * sn - ny * cs, 349 sn, nz * cs1, ny * cs1, nynz * sn + nx * cs, 350 0, 0, 2 * nz * cs1, (nznz - 1) * sn }); 351 352 Mat_<double> dudv({ 4, 4 }, { 353 itheta, 0, 0, -nx * itheta, 354 0, itheta, 0, -ny * itheta, 355 0, 0, itheta, -nz * itheta, 356 0, 0, 0, 1 }); 357 358 Mat_<double> dvdr({ 4, 3 }, { 359 1, 0, 0, 360 0, 1, 0, 361 0, 0, 1, 362 nx, ny, nz }); 363 364 Mat_<double> Jacobian = dRdu * dudv * dvdr;//rows=9 cols=3 365 } 366 } 367 else 368 { 369 double sx = R[7] - R[5]; 370 double sy = R[2] - R[6]; 371 double sz = R[3] - R[1]; 372 double sn = sqrt(sx * sx + sy * sy + sz * sz) * 0.5; 373 double cs = (R[0] + R[4] + R[8] - 1) * 0.5; 374 double theta = acos(cs); 375 double ss = 2 * sn; 376 double iss = 1. / ss; 377 double tss = theta * iss; 378 r[0] = tss * sx; 379 r[1] = tss * sy; 380 r[2] = tss * sz; 381 382 if (0) 383 { 384 Mat_<double> drdu({ 3, 4 }, { 385 tss, 0, 0, (sn - theta * cs) * iss * iss * sx * 2, 386 0, tss, 0, (sn - theta * cs) * iss * iss * sy * 2, 387 0, 0, tss, (sn - theta * cs) * iss * iss * sz * 2 }); 388 389 Mat_<double> dudR({ 4, 9 }, { 390 0, 0, 0, 0, 0, -1, 0, 1, 0, 391 0, 0, 1, 0, 0, 0, -1, 0, 0, 392 0, -1, 0, 1, 0, 0, 0, 0, 0, 393 -iss, 0, 0, 0, -iss, 0, 0, 0, -iss }); 394 395 Mat_<double> Jacobian = drdu * dudR;//rows=3 cols=9 396 } 397 } 398 } 399 400 private: 401 const int nHorPoint3D = 100; 402 const int nVerPoint3D = 100; 403 const double varPoint3DXY = 10.; 404 const double minPoint3DZ = 1.; 405 const double maxPoint3DZ = 99.; 406 const double minCamZ = 101.; 407 const double maxCamZ = 150.; 408 const double varCamDegree = 10.; 409 Mat_<Vec3d> allPoint3D = Mat_<Vec3d>(nVerPoint3D * nHorPoint3D, 1); 410 Mat_<double> allPoint3DZ = Mat_<double>(nVerPoint3D * nHorPoint3D, 1); 411 Mat_<double> K; 412 Mat_<double> D; 413 const double deg2rad = 3.14159265358979323846 * 0.0055555555555555556; 414 const double rad2deg = 180 * 0.3183098861837906715; 415 416 public: 417 int camRows = 480; 418 int camCols = 640; 419 int camFovY = 90; 420 int camFovX = 90; 421 int camRand = 10;//append random[0,camRand] to camera intrinsics 422 int nCamDist = 5;//refer to opencv for value domain 423 int nMinMotion = 32; // no less than X motion views 424 int nMaxMotion = INT_MAX; // no more than X motion views 425 int nPoint2DThenExit = 32;//exit when less than X pixies 426 int rotMode = 1 + 2 + 4;//0=noRot 1=xAxis 2=yAxis 4=zAxis 427 bool noTrans = false;//translate or not while motion 428 bool world2D = false;//planar world or not 429 bool rndSeek = true;//use random seek or not 430 bool enableVerbose = false;//check motions one by one or not 431 vector<MotionView> motionViews;//World Information: RightX, FrontY, DownZ 432 MotionSim(bool run = true, bool world2D0 = false, bool noTrans0 = false, int rotMode0 = 7) { if (run) runMotion(world2D0, noTrans0, rotMode0); } 433 434 public: 435 void runMotion(bool world2D0 = false, bool noTrans0 = false, int rotMode0 = 7) 436 { 437 world2D = world2D0; 438 noTrans = noTrans0; 439 rotMode = rotMode0; 440 motionViews.clear(); 441 if (rndSeek) cv::setRNGSeed(clock()); 442 while (motionViews.size() < nMinMotion) 443 { 444 //1.GetAllPoint3D 445 if (world2D) allPoint3DZ = 0.; 446 else cv::randu(allPoint3DZ, -maxPoint3DZ, -minPoint3DZ);//DownZ 447 for (int i = 0, k = 0; i < nVerPoint3D; ++i) 448 for (int j = 0; j < nHorPoint3D; ++j, ++k) 449 allPoint3D(k) = Vec3d((j + cv::randu<double>()) * varPoint3DXY, (i + cv::randu<double>()) * varPoint3DXY, allPoint3DZ(i, j)); 450 451 //2.GetCamParams 452 double camFx = camCols / 2. / std::tan(camFovX / 2. * deg2rad) + cv::randu<double>() * camRand; 453 double camFy = camRows / 2. / std::tan(camFovY / 2. * deg2rad) + cv::randu<double>() * camRand; 454 double camCx = camCols / 2. + cv::randu<double>() * camRand; 455 double camCy = camRows / 2. + cv::randu<double>() * camRand; 456 K.create(3, 3); K << camFx, 0, camCx, 0, camFy, camCy, 0, 0, 1; 457 D.create(nCamDist, 1); cv::randu(D, -1.0, 1.0); 458 459 //3.GetAllMotionView 460 motionViews.clear(); 461 for (int64 k = 0; ; ++k) 462 { 463 //3.1 JoinCamParams 464 MotionView view; 465 view.K = K.clone(); 466 view.D = D.clone(); 467 468 //3.2 GetCamTrans 469 if (k == 0) view.t(0) = view.t(1) = 0; 470 else 471 { 472 view.t(0) = motionViews[k - 1].t(0) + cv::randu<double>() * varPoint3DXY; 473 view.t(1) = motionViews[k - 1].t(1) + cv::randu<double>() * varPoint3DXY; 474 } 475 view.t(2) = minCamZ + cv::randu<double>() * (maxCamZ - minCamZ); 476 view.t(2) = -view.t(2);//DownZ 477 if (noTrans && k != 0) { view.t(0) = motionViews[0].t(0); view.t(1) = motionViews[0].t(1); view.t(2) = motionViews[0].t(2); } 478 479 //3.3 GetCamRot: degree-->radian-->matrix-->vector&quaternion 480 view.degree = 0.; 481 if (rotMode & 1) view.degree(0) = cv::randu<double>() * varCamDegree; 482 if (rotMode & 2) view.degree(1) = cv::randu<double>() * varCamDegree; 483 if (rotMode & 4) view.degree(2) = cv::randu<double>() * varCamDegree; 484 view.radian = view.degree * deg2rad; 485 euler2matrix(view.radian.ptr<double>(), view.R.ptr<double>()); 486 cv::Rodrigues(view.R, view.r); 487 quat2matrix(view.q.ptr<double>(), view.R.ptr<double>(), false); 488 cv::hconcat(view.R, view.t, view.T); 489 cv::vconcat(view.r, view.t, view.rt); 490 491 //3.4 GetPoint3DAndPoint2D 492 Mat_<Vec2d> allPoint2D; 493 cv::projectPoints(allPoint3D, -view.r, -view.R.t() * view.t, view.K, view.D, allPoint2D); 494 for (int k = 0; k < allPoint2D.total(); ++k) 495 if (allPoint2D(k)[0] > 0 && allPoint2D(k)[0] < camCols && allPoint2D(k)[1] > 0 && allPoint2D(k)[1] < camRows) 496 { 497 view.point2D.push_back(allPoint2D(k)); 498 view.point3D.push_back(allPoint3D(k)); 499 view.point3DIds.push_back(k); 500 } 501 502 //3.5 PrintDetails 503 motionViews.push_back(view); 504 if (enableVerbose) 505 { 506 cout << endl << view.print(); 507 cout << fmt::format("view={} features={} ", k, view.point2D.rows); 508 double minV = 0, maxV = 0;//Distortion makes some minV next to maxV 509 int minId = 0, maxId = 0; 510 cv::minMaxIdx(allPoint2D.reshape(1, int(allPoint2D.total()) * allPoint2D.channels()), &minV, &maxV, &minId, &maxId); 511 cout << fmt::format("minInfo:({}, {})", minId, minV) << allPoint3D(minId / 2) << allPoint2D(minId / 2) << endl; 512 cout << fmt::format("maxInfo:({}, {})", maxId, maxV) << allPoint3D(maxId / 2) << allPoint2D(maxId / 2) << endl; 513 cout << "Press any key to continue" << endl; std::getchar(); 514 } 515 if (view.point2D.rows < nPoint2DThenExit || motionViews.size() > nMaxMotion) break; 516 } 517 } 518 } 519 void visMotion() 520 { 521 //1.CreateWidgets 522 Size2d validSize(nHorPoint3D * varPoint3DXY, nVerPoint3D * varPoint3DXY); 523 Mat_<cv::Affine3d> camPoses(int(motionViews.size()), 1); for (int k = 0; k < camPoses.rows; ++k) camPoses(k) = cv::Affine3d(motionViews[k].T); 524 viz::WText worldInfo(fmt::format("nMotionView: {} K: {} D: {}", motionViews.size(), cvarr2str(K), cvarr2str(D)), Point(10, 240), 10); 525 viz::WCoordinateSystem worldCSys(1000); 526 viz::WPlane worldGround(Point3d(validSize.width / 2, validSize.height / 2, 0), Vec3d(0, 0, 1), Vec3d(0, 1, 0), validSize); 527 viz::WCloud worldPoints(allPoint3D, Mat_<Vec3b>(allPoint3D.size(), Vec3b(0, 255, 0))); 528 viz::WTrajectory camTraj1(camPoses, viz::WTrajectory::FRAMES, 8); 529 viz::WTrajectorySpheres camTraj2(camPoses, 100, 2); 530 viz::WTrajectoryFrustums camTraj3(camPoses, Matx33d(K), 4., viz::Color::yellow()); 531 worldCSys.setRenderingProperty(viz::OPACITY, 0.1); 532 worldGround.setRenderingProperty(viz::OPACITY, 0.1); 533 camTraj2.setRenderingProperty(viz::OPACITY, 0.6); 534 535 //2.ShowWidgets 536 static viz::Viz3d viz3d(__FUNCTION__); 537 viz3d.showWidget("worldInfo", worldInfo); 538 viz3d.showWidget("worldCSys", worldCSys); 539 viz3d.showWidget("worldGround", worldGround); 540 viz3d.showWidget("worldPoints", worldPoints); 541 viz3d.showWidget("camTraj1", camTraj1); 542 viz3d.showWidget("camTraj2", camTraj2); 543 viz3d.showWidget("camTraj3", camTraj3); 544 545 //3.UpdateWidghts 546 static const vector<MotionView>& views = motionViews; 547 viz3d.registerKeyboardCallback([](const viz::KeyboardEvent& keyboarEvent, void* pVizBorad)->void 548 { 549 if (keyboarEvent.action != viz::KeyboardEvent::KEY_DOWN) return; 550 static int pos = 0; 551 if (keyboarEvent.code == ' ') 552 { 553 size_t num = views.size(); 554 size_t ind = pos % num; 555 double xmin3D = DBL_MAX, ymin3D = DBL_MAX, xmin2D = DBL_MAX, ymin2D = DBL_MAX; 556 double xmax3D = -DBL_MAX, ymax3D = -DBL_MAX, xmax2D = -DBL_MAX, ymax2D = -DBL_MAX; 557 for (size_t k = 0; k < views[ind].point3D.rows; ++k) 558 { 559 Vec3d pt3 = views[ind].point3D(int(k)); 560 Vec2d pt2 = views[ind].point2D(int(k)); 561 if (pt3[0] < xmin3D) xmin3D = pt3[0]; 562 if (pt3[0] > xmax3D) xmax3D = pt3[0]; 563 if (pt3[1] < ymin3D) ymin3D = pt3[1]; 564 if (pt3[1] > ymax3D) ymax3D = pt3[1]; 565 if (pt2[0] < xmin2D) xmin2D = pt2[0]; 566 if (pt2[0] > xmax2D) xmax2D = pt2[0]; 567 if (pt2[1] < ymin2D) ymin2D = pt2[1]; 568 if (pt2[1] > ymax2D) ymax2D = pt2[1]; 569 } 570 if (pos != 0) 571 { 572 for (int k = 0; k < views[ind == 0 ? num - 1 : ind - 1].point3D.rows; ++k) viz3d.removeWidget("active" + std::to_string(k)); 573 viz3d.removeWidget("viewInfo"); 574 viz3d.removeWidget("camSolid"); 575 } 576 for (int k = 0; k < views[ind].point3D.rows; ++k) viz3d.showWidget("active" + std::to_string(k), viz::WSphere(views[ind].point3D(k), 5, 10)); 577 viz3d.showWidget("viewInfo", viz::WText(fmt::format("CurrentMotion: {} ValidPoints: {} Min3DXY_Min2DXY: {}, {}, {}, {} Max3DXY_Max2DXY: {}, {}, {}, {} Rot_Trans_Euler: {} ", 578 ind, views[ind].point3D.rows, xmin3D, ymin3D, xmin2D, ymin2D, xmax3D, ymax3D, xmax2D, ymax2D, 579 cvarr2str(views[ind].r.t()) + cvarr2str(views[ind].t.t()) + cvarr2str(views[ind].degree.t())), Point(10, 10), 10)); 580 viz3d.showWidget("camSolid", viz::WCameraPosition(Matx33d(views[ind].K), 10, viz::Color::yellow()), cv::Affine3d(views[ind].T)); 581 ++pos; 582 } 583 }, 0); 584 viz3d.spin(); 585 } 586 }; 587 588 class OptimizeRt 589 { 590 public: 591 using MotionView = MotionSim::MotionView; 592 static void TestMe(int argc = 0, char** argv = 0) 593 { 594 int N = 99; 595 for (int k = 0; k < N; ++k) 596 { 597 //1.GenerateData 598 bool world2D = k % 2; 599 int rotMode = k % 7 + 1; 600 MotionSim motionSim(false); 601 motionSim.camFovX = 90; 602 motionSim.camFovY = 90; 603 motionSim.camRand = 10; 604 motionSim.nMinMotion = 16;//2 605 motionSim.nMaxMotion = 32;//4 606 motionSim.rndSeek = false; 607 motionSim.nCamDist = 5; 608 motionSim.runMotion(world2D, false, rotMode); 609 //motionSim.visMotion(); 610 int rndInd = int(motionSim.motionViews.size() * cv::randu<double>()); 611 Mat_<double> r0 = -motionSim.motionViews[rndInd].r; 612 Mat_<double> t0 = -motionSim.motionViews[rndInd].R.t() * motionSim.motionViews[rndInd].t; 613 const MotionView& motionView = motionSim.motionViews[rndInd]; 614 double errRatio = 0.9; 615 616 //2.CalcByCeres 617 Mat_<double> param1; cv::vconcat(r0, t0, param1); param1 *= errRatio;//use one group of param for LMSolver param format 618 ceres::Problem problem; 619 problem.AddResidualBlock(new CeresCostRt(motionView), NULL, param1.ptr<double>()); 620 ceres::Solver::Options options; 621 ceres::Solver::Summary summary; 622 ceres::Solve(options, &problem, &summary); 623 int nIter1 = (int)summary.iterations.size(); 624 625 //3.CalcByOpenCV 626 Mat_<double> param2; cv::vconcat(r0, t0, param2); param2 *= errRatio; 627 Ptr<cv::LMSolver::Callback> callback = makePtr<CvCallbackRt>(motionView); 628 Ptr<cv::LMSolver> lmSolver2 = cv::LMSolver::create(callback, 50); 629 int nIter2 = lmSolver2->run(param2); 630 631 //4.AnalyzeError 632 double infr0r0 = norm(r0, r0 * errRatio, NORM_INF); 633 double infr0r1 = norm(r0, param1.rowRange(0, 3), NORM_INF); 634 double infr0r2 = norm(r0, param2.rowRange(0, 3), NORM_INF); 635 double inft0t0 = norm(t0, t0 * errRatio, NORM_INF); 636 double inft0t1 = norm(t0, param1.rowRange(3, 6), NORM_INF); 637 double inft0t2 = norm(t0, param2.rowRange(3, 6), NORM_INF); 638 double infr1r2 = norm(param1.rowRange(0, 3), param2.rowRange(0, 3), NORM_INF); 639 double inft1t2 = norm(param1.rowRange(3, 6), param2.rowRange(3, 6), NORM_INF); 640 641 //5.PrintError 642 cout << fmt::format("LoopCount: {} CeresSolver.iters: {} CVLMSolver.iters: {} ", k, nIter1, nIter2); 643 if (infr0r1 > 1e-8 || infr0r2 > 1e-8 || inft0t1 > 1e-8 || inft0t2 > 1e-8) 644 { 645 cout << fmt::format("infr0r1: {:<15.9} {:<15.9} ", infr0r1, infr0r0); 646 cout << fmt::format("infr0r2: {:<15.9} {:<15.9} ", infr0r2, infr0r0); 647 cout << fmt::format("inft0t1: {:<15.9} {:<15.9} ", inft0t1, inft0t0); 648 cout << fmt::format("inft0t2: {:<15.9} {:<15.9} ", inft0t2, inft0t0); 649 cout << fmt::format("infr1r2: {:<15.9} ", infr1r2); 650 cout << fmt::format("inft1t2: {:<15.9} ", inft1t2); 651 cout << "Press any key to continue" << endl; std::getchar(); 652 } 653 } 654 } 655 656 public: 657 struct CeresCostRt : public ceres::CostFunction 658 { 659 const MotionView& motionView;//use K&D&point2D&point3D as groundtruth 660 CeresCostRt(const MotionView& motionView0) : motionView(motionView0) 661 { 662 this->set_num_residuals(motionView.point2D.rows * 2); 663 this->mutable_parameter_block_sizes()->push_back(6);//Also can use two groups of params: r and t. Refer to OptimizeKDRt. And two groups can contribute to less jacobians computation. 664 } 665 bool Evaluate(double const* const* params, double* residuals, double** jacobians) const 666 { 667 //1.ExtractInput 668 Vec3d r(params[0]); 669 Vec3d t(params[0] + 3); 670 Mat_<Vec2d> errPoint2D(motionView.point2D.rows, 1, (Vec2d*)(residuals)); 671 Mat_<double> dpdrt; if (jacobians && jacobians[0]) dpdrt = Mat_<double>(motionView.point2D.rows * 2, 6, jacobians[0]); 672 673 //2.CalcJacAndErr 674 Mat_<double> dpdKDT; 675 Mat_<Vec2d> point2DEva; 676 cv::projectPoints(motionView.point3D, r, t, motionView.K, motionView.D, point2DEva, dpdrt.empty() ? noArray() : dpdKDT); 677 errPoint2D = point2DEva - motionView.point2D; 678 if (dpdrt.empty() == false) dpdKDT.colRange(0, 6).copyTo(dpdrt); 679 return true; 680 } 681 }; 682 683 public: 684 struct CvCallbackRt : public cv::LMSolver::Callback 685 { 686 const MotionView& motionView;//use K&D&point2D&point3D as groundtruth 687 CvCallbackRt(const MotionView& motionView0) : motionView(motionView0) {} 688 bool compute(InputArray params, OutputArray residuals, OutputArray jacobians) const 689 { 690 //1.ExtractInput 691 Vec3d r(params.getMat().ptr<double>()); 692 Vec3d t(params.getMat().ptr<double>() + 3); 693 if (residuals.empty()) residuals.create(motionView.point2D.rows * 2, 1, CV_64F); 694 if (jacobians.needed() && jacobians.empty()) jacobians.create(motionView.point2D.rows * 2, params.rows(), CV_64F); 695 Mat_<Vec2d> errPoint2D = residuals.getMat(); 696 Mat_<double> dpdrt = jacobians.getMat(); 697 698 //2.CalcJacAndErr 699 Mat_<double> dpdKDT; 700 Mat_<Vec2d> point2DEva; 701 cv::projectPoints(motionView.point3D, r, t, motionView.K, motionView.D, point2DEva, dpdrt.empty() ? noArray() : dpdKDT); 702 errPoint2D = point2DEva - motionView.point2D; 703 if (dpdrt.empty() == false) dpdKDT.colRange(0, 6).copyTo(dpdrt); 704 return true; 705 } 706 }; 707 }; 708 709 class OptimizeKDRt 710 { 711 public: 712 using MotionView = MotionSim::MotionView; 713 static void TestMe(int argc = 0, char** argv = 0) 714 { 715 int N = 99; 716 for (int k = 0; k < N; ++k) 717 { 718 //1.GenerateData 719 bool world2D = k % 2; 720 int rotMode = k % 7 + 1; 721 MotionSim motionSim(false); 722 motionSim.camFovX = 90; 723 motionSim.camFovY = 90; 724 motionSim.camRand = 10; 725 motionSim.nMinMotion = 16;//2 726 motionSim.nMaxMotion = 32;//4 727 motionSim.rndSeek = false; 728 motionSim.nCamDist = 5; 729 motionSim.runMotion(world2D, false, rotMode); 730 //motionSim.visMotion(); 731 Mat_<double> rs0; for (size_t k = 0; k < motionSim.motionViews.size(); ++k) rs0.push_back(-motionSim.motionViews[k].r); 732 Mat_<double> ts0; for (size_t k = 0; k < motionSim.motionViews.size(); ++k) ts0.push_back(-motionSim.motionViews[k].R.t() * motionSim.motionViews[k].t); 733 Mat_<double> K0({ 4, 1 }, { motionSim.motionViews[0].K(0, 0), motionSim.motionViews[0].K(1, 1), motionSim.motionViews[0].K(0, 2), motionSim.motionViews[0].K(1, 2) }); 734 Mat_<double> D0 = motionSim.motionViews[0].D.clone(); 735 double errRatio = 0.9; 736 double errRatioTrans = 0.99; 737 738 //2.CalcByCeres 739 Mat_<double> params1, rs1, ts1;//use one group of param for LMSolver param format 740 for (int k = 0; k < rs0.rows; k += 3) { params1.push_back(rs0.rowRange(k, k + 3) * errRatio); params1.push_back(ts0.rowRange(k, k + 3) * errRatioTrans); } 741 params1.push_back(K0 * errRatio); 742 params1.push_back(D0 * errRatio); 743 ceres::Problem problem; 744 for (int k = 0; k < motionSim.motionViews.size(); ++k) 745 problem.AddResidualBlock(new CeresCostKDRt(motionSim.motionViews[k]), NULL, params1.ptr<double>(k * 6), params1.ptr<double>(k * 6 + 3), 746 params1.ptr<double>(params1.rows - 4 - D0.rows), params1.ptr<double>(params1.rows - D0.rows)); 747 ceres::Solver::Options options; 748 ceres::Solver::Summary summary; 749 ceres::Solve(options, &problem, &summary); 750 int nIter1 = (int)summary.iterations.size(); 751 for (int k = 0; k < params1.rows - 4 - D0.rows; k += 6) { rs1.push_back(params1.rowRange(k, k + 3)); ts1.push_back(params1.rowRange(k + 3, k + 6)); } 752 Mat_<double> K1 = params1.rowRange(params1.rows - 4 - D0.rows, params1.rows - D0.rows).clone(); 753 Mat_<double> D1 = params1.rowRange(params1.rows - D0.rows, params1.rows).clone(); 754 755 //3.CalcByOpenCV 756 Mat_<double> params2, rs2, ts2; 757 for (int k = 0; k < rs0.rows; k += 3) { params2.push_back(rs0.rowRange(k, k + 3) * errRatio); params2.push_back(ts0.rowRange(k, k + 3) * errRatioTrans); } 758 params2.push_back(K0 * errRatio); 759 params2.push_back(D0 * errRatio); 760 Ptr<cv::LMSolver::Callback> callback = makePtr<CvCallbackKDRt>(motionSim.motionViews); 761 Ptr<cv::LMSolver> lmSolver2 = cv::LMSolver::create(callback, 50); 762 int nIter2 = lmSolver2->run(params2); 763 for (int k = 0; k < params2.rows - 4 - D0.rows; k += 6) { rs2.push_back(params2.rowRange(k, k + 3)); ts2.push_back(params2.rowRange(k + 3, k + 6)); } 764 Mat_<double> K2 = params2.rowRange(params2.rows - 4 - D0.rows, params2.rows - D0.rows).clone(); 765 Mat_<double> D2 = params2.rowRange(params2.rows - D0.rows, params2.rows).clone(); 766 767 //4.AnalyzeError 768 double infrs0rs0 = norm(rs0, rs0 * errRatio, NORM_INF); 769 double infrs0rs1 = norm(rs0, rs1, NORM_INF); 770 double infrs0rs2 = norm(rs0, rs2, NORM_INF); 771 double infts0ts0 = norm(ts0, ts0 * errRatioTrans, NORM_INF); 772 double infts0ts1 = norm(ts0, ts1, NORM_INF); 773 double infts0ts2 = norm(ts0, ts2, NORM_INF); 774 double infK0K0 = norm(K0, K0 * errRatio, NORM_INF); 775 double infK0K1 = norm(K0, K1, NORM_INF); 776 double infK0K2 = norm(K0, K2, NORM_INF); 777 double infD0D0 = norm(D0, D0 * errRatio, NORM_INF); 778 double infD0D1 = norm(D0, D1, NORM_INF); 779 double infD0D2 = norm(D0, D2, NORM_INF); 780 double infrs1rs2 = norm(rs1, rs2, NORM_INF); 781 double infts1ts2 = norm(ts1, ts2, NORM_INF); 782 double infK1K2 = norm(K1, K2, NORM_INF); 783 double infD1D2 = norm(D1, D2, NORM_INF); 784 785 //5.PrintError 786 cout << fmt::format("LoopCount: {} CeresSolver.iters: {} CVLMSolver.iters: {} ", k, nIter1, nIter2); 787 if (infrs0rs1 > 1e-8 || infrs0rs2 > 1e-8 || infts0ts1 > 1e-8 || infts0ts2 > 1e-8 || infK0K1 > 1e-8 || infK0K2 > 1e-8 || infD0D1 > 1e-8 || infD0D2 > 1e-8) 788 { 789 cout << fmt::format("infrs0rs1: {:<15.9} {:<15.9} ", infrs0rs1, infrs0rs0); 790 cout << fmt::format("infrs0rs2: {:<15.9} {:<15.9} ", infrs0rs2, infrs0rs0); 791 cout << fmt::format("infts0ts1: {:<15.9} {:<15.9} ", infts0ts1, infts0ts0); 792 cout << fmt::format("infts0ts2: {:<15.9} {:<15.9} ", infts0ts2, infts0ts0); 793 cout << fmt::format("infK0K1 : {:<15.9} {:<15.9} ", infK0K1, infK0K0); 794 cout << fmt::format("infK0K2 : {:<15.9} {:<15.9} ", infK0K2, infK0K0); 795 cout << fmt::format("infD0D1 : {:<15.9} {:<15.9} ", infD0D1, infD0D0); 796 cout << fmt::format("infD0D2 : {:<15.9} {:<15.9} ", infD0D2, infD0D0); 797 cout << fmt::format("infrs1rs2: {:<15.9} ", infrs1rs2); 798 cout << fmt::format("infts1ts2: {:<15.9} ", infts1ts2); 799 cout << fmt::format("infK1D2 : {:<15.9} ", infK1K2); 800 cout << fmt::format("infD1D2 : {:<15.9} ", infD1D2); 801 cout << "Press any key to continue" << endl; std::getchar(); 802 } 803 } 804 } 805 806 public: 807 struct CeresCostKDRt : public ceres::CostFunction 808 { 809 const MotionView& motionView;//use K&D&point2D&point3D as groundtruth 810 CeresCostKDRt(const MotionView& motionView0) : motionView(motionView0) 811 { 812 this->set_num_residuals(motionView.point2D.rows * 2); 813 this->mutable_parameter_block_sizes()->push_back(3); 814 this->mutable_parameter_block_sizes()->push_back(3); 815 this->mutable_parameter_block_sizes()->push_back(4); 816 this->mutable_parameter_block_sizes()->push_back(motionView.D.rows); 817 } 818 bool Evaluate(double const* const* params, double* residuals, double** jacobians) const 819 { 820 //1.ExtractInput 821 Vec3d r(params[0]); 822 Vec3d t(params[1]); 823 Matx33d K(params[2][0], 0, params[2][2], 0, params[2][1], params[2][3], 0, 0, 1); 824 Mat_<double> D(motionView.D.rows, 1); for (int k = 0; k < D.rows; ++k) D(k) = params[3][k]; 825 Mat_<Vec2d> errPoint2D(motionView.point2D.rows, 1, (Vec2d*)(residuals)); 826 Mat_<double> dpdr; if (jacobians && jacobians[0]) dpdr = Mat_<double>(motionView.point2D.rows * 2, 3, jacobians[0]); 827 Mat_<double> dpdt; if (jacobians && jacobians[1]) dpdt = Mat_<double>(motionView.point2D.rows * 2, 3, jacobians[1]); 828 Mat_<double> dpdK; if (jacobians && jacobians[2]) dpdK = Mat_<double>(motionView.point2D.rows * 2, 4, jacobians[2]); 829 Mat_<double> dpdD; if (jacobians && jacobians[3]) dpdD = Mat_<double>(motionView.point2D.rows * 2, motionView.D.rows, jacobians[3]); 830 831 //2.CalcJacAndErr 832 Mat_<double> dpdKDT; 833 Mat_<Vec2d> point2DEva; 834 cv::projectPoints(motionView.point3D, r, t, K, D, point2DEva, dpdr.empty() && dpdt.empty() && dpdK.empty() && dpdD.empty() ? noArray() : dpdKDT); 835 errPoint2D = point2DEva - motionView.point2D; 836 if (dpdr.empty() == false) dpdKDT.colRange(0, 3).copyTo(dpdr); 837 if (dpdt.empty() == false) dpdKDT.colRange(3, 6).copyTo(dpdt); 838 if (dpdK.empty() == false) dpdKDT.colRange(6, 10).copyTo(dpdK); 839 if (dpdD.empty() == false) dpdKDT.colRange(10, 10 + D.rows).copyTo(dpdD); 840 return true; 841 } 842 }; 843 844 public: 845 struct CvCallbackKDRt : public cv::LMSolver::Callback 846 { 847 int nResidual = 0; 848 const vector<MotionView>& motionViews;//use K&D&point2D&point3D as groundtruth 849 CvCallbackKDRt(const vector<MotionView>& motionViews0) : motionViews(motionViews0) { for (size_t i = 0; i < motionViews.size(); ++i) nResidual += motionViews[i].point2D.rows * 2; } 850 bool compute(InputArray params, OutputArray residuals, OutputArray jacobians) const 851 { 852 //1.ExtractInputGlobal 853 Mat_<double> cvParams = params.getMat(); 854 double* rsdata = params.getMat().ptr<double>(); 855 double* tsdata = params.getMat().ptr<double>() + 3; 856 double* KData = params.getMat().ptr<double>() + motionViews.size() * 2 * 3; 857 double* DData = params.getMat().ptr<double>() + motionViews.size() * 2 * 3 + 4; 858 if (residuals.empty()) residuals.create(nResidual, 1, CV_64F); 859 if (jacobians.needed() && jacobians.empty()) jacobians.create(nResidual, params.rows(), CV_64F); 860 Mat_<Vec2d> errPoint2Ds = residuals.getMat(); 861 Mat_<double> dpdKDTs = jacobians.getMat(); 862 863 //2.CalcJacAndErrGlobal 864 for (int k = 0, row1 = 0, row2 = 0, nView = int(motionViews.size()); k < nView; ++k, row1 = row2) 865 { 866 const MotionView& motionView = motionViews[k]; 867 //2.1 ExtractInput 868 Vec3d r(rsdata + k * 6); 869 Vec3d t(tsdata + k * 6); 870 Matx33d K(KData[0], 0, KData[2], 0, KData[1], KData[3], 1); 871 Mat_<double> D(motionView.D.rows, 1, DData); 872 Mat_<Vec2d> errPoint2D = errPoint2Ds.rowRange(row1, row2 = row1 + motionView.point2D.rows); 873 Mat_<double> dpdr; if (dpdKDTs.empty() == false) dpdr = dpdKDTs.rowRange(row1 * 2, row2 * 2).colRange(k * 6, k * 6 + 3); 874 Mat_<double> dpdt; if (dpdKDTs.empty() == false) dpdt = dpdKDTs.rowRange(row1 * 2, row2 * 2).colRange(k * 6 + 3, k * 6 + 6); 875 Mat_<double> dpdK; if (dpdKDTs.empty() == false) dpdK = dpdKDTs.rowRange(row1 * 2, row2 * 2).colRange(nView * 2 * 3, nView * 2 * 3 + 4); 876 Mat_<double> dpdD; if (dpdKDTs.empty() == false) dpdD = dpdKDTs.rowRange(row1 * 2, row2 * 2).colRange(nView * 2 * 3 + 4, dpdKDTs.cols); 877 878 //2.2 CalcJacAndErr 879 Mat_<double> dpdKDT; 880 Mat_<Vec2d> point2DEva; 881 cv::projectPoints(motionView.point3D, r, t, K, D, point2DEva, dpdKDTs.empty() ? noArray() : dpdKDT); 882 errPoint2D = point2DEva - motionView.point2D; 883 if (dpdr.empty() == false) dpdKDT.colRange(0, 3).copyTo(dpdr); 884 if (dpdt.empty() == false) dpdKDT.colRange(3, 6).copyTo(dpdt); 885 if (dpdK.empty() == false) dpdKDT.colRange(6, 10).copyTo(dpdK); 886 if (dpdD.empty() == false) dpdKDT.colRange(10, 10 + D.rows).copyTo(dpdD); 887 if (0)//for DEBUG 888 { 889 cout << norm(point2DEva - motionView.point2D, errPoint2Ds.rowRange(row1, row2 = row1 + motionView.point2D.rows), NORM_INF) << " "; 890 if (dpdr.empty() == false) cout << norm(dpdKDT.colRange(0, 3), dpdKDTs.rowRange(row1 * 2, row2 * 2).colRange(k * 6, k * 6 + 3), NORM_INF) << " "; 891 if (dpdt.empty() == false) cout << norm(dpdKDT.colRange(3, 6), dpdKDTs.rowRange(row1 * 2, row2 * 2).colRange(k * 6 + 3, k * 6 + 6), NORM_INF) << " "; 892 if (dpdK.empty() == false) cout << norm(dpdKDT.colRange(6, 10), dpdKDTs.rowRange(row1 * 2, row2 * 2).colRange(nView * 2 * 3, nView * 2 * 3 + 4), NORM_INF) << " "; 893 if (dpdK.empty() == false) cout << norm(dpdKDT.colRange(10, 10 + D.rows), dpdKDTs.rowRange(row1 * 2, row2 * 2).colRange(nView * 2 * 3 + 4, dpdKDTs.cols), NORM_INF) << endl; 894 } 895 } 896 return true; 897 } 898 }; 899 }; 900 901 int main(int argc, char** argv) { OptimizeKDRt::TestMe(argc, argv); return 0; } 902 int main1(int argc, char** argv) { OptimizeRt::TestMe(argc, argv); return 0; } 903 int main2(int argc, char** argv) { OptimizeKDRt::TestMe(argc, argv); return 0; }