1.本文要点说明
2.实验测试代码
依赖于OpenCV、Ceres和Spdlog，封装在类InitCamera：
(1)initRTCM、TestInitRTCM
(2)initKBCM、TestInitKBCM
(3)initEUCM、TestInitMUCM
1 #include <ceres/Motion2D.h> 2 3 class InitCamera 4 { 5 public: 6 static void initRTCM(vector<vector<Point3f>>& point3D, vector<vector<Point2f>>& point2D, Size imaSize, Mat_<double>& camK, Mat_<Vec3d>& camRs, Mat_<Vec3d>& camTs, int method = 1) 7 { 8 //0.FormatInput 9 camRs.resize(0); camTs.resize(0); 10 if (camK.rows != 3 || camK.cols != 3) camK.create(3, 3); 11 camK(0, 2) = (imaSize.width - 1) * 0.5; 12 camK(1, 2) = (imaSize.height - 1) * 0.5; 13 camK(2, 2) = 1; camK(0, 1) = camK(1, 0) = camK(2, 0) = camK(2, 1) = 0; 14 15 //1.CalcInstrinsic 16 const double cx = camK(0, 2); 17 const double cy = camK(1, 2); 18 Mat_<double> A; 19 vector<Mat_<double>> Hs; 20 if (method <= 2)//const: skew=zero pp=center 21 { 22 //1.CreateEquations 23 for (int k = 0; k < point3D.size(); ++k) 24 { 25 //1.1 FindHomos 26 Mat_<double> H = findHomography(point3D[k], point2D[k]); 27 Hs.push_back(H); 28 29 //1.2 CalcCoeffs 30 double I = H(0, 0) - H(2, 0) * cx; 31 double J = H(0, 1) - H(2, 1) * cx; 32 double M = H(1, 0) - H(2, 0) * cy; 33 double N = H(1, 1) - H(2, 1) * cy; 34 double E = H(2, 0); 35 double F = H(2, 1); 36 37 //1.3 GetEquation 38 Mat_<double> equation1({ 1, 3 }, { I * J, M * N, E * F }); 39 Mat_<double> equation2({ 1, 3 }, { I * I - J * J, M * M - N * N, E * E - F * F }); 40 if (method == 1) //same as opencv 41 { 42 double inorm1 = 1. / sqrt((I * I + M * M + E * E) * (J * J + N * N + F * F)); 43 double inorm2 = 1. / sqrt(((I - J) * (I - J) + (M - N) * (M - N) + (E - F) * (E - F)) * ((I + J) * (I + J) + (M + N) * (M + N) + (E + F) * (E + F))); 44 equation1 *= inorm1; 45 equation2 *= inorm2; 46 } 47 A.push_back(equation1); 48 A.push_back(equation2); 49 } 50 51 //2.SolveEquations 52 Mat_<double> w, u, vt; 53 cv::SVDecomp(A, w, u, vt, SVD::FULL_UV); 54 55 //3.ConvertToSolution 56 camK(0, 0) = sqrt(vt(vt.rows - 1, 2) / vt(vt.rows - 1, 0)); 57 camK(1, 1) = sqrt(vt(vt.rows - 1, 2) / vt(vt.rows - 1, 1)); 58 } 59 else if (method == 3)//const: skew=zero pp=center 60 { 61 //1.CreateEquations 62 for (int k = 0; k < point3D.size(); ++k) 63 { 64 //1.1 FindHomo 65 Mat_<double> H = findHomography(point3D[k], point2D[k]); 66 Hs.push_back(H); 67 68 //1.2 CalcCoeffs 69 auto CalcCoeffs = [](Mat_<double>& H, int u, int v, double cx, double cy)->Mat_<double> 70 { 71 return Mat_<double>({ 1, 3 }, { 72 H(0, u) * H(0, v) - cx * (H(0, u) * H(2, v) + H(2, u) * H(0, v)), 73 H(1, u) * H(1, v) - cy * (H(1, u) * H(2, v) + H(2, u) * H(1, v)), 74 H(2, u) * H(2, v) }); 75 }; 76 77 //1.3 GetEquation 78 A.push_back(CalcCoeffs(H, 0, 1, cx, cy)); 79 A.push_back((CalcCoeffs(H, 0, 0, cx, cy) - CalcCoeffs(H, 1, 1, cx, cy))); 80 } 81 82 //2.SolveEquations 83 Mat_<double> w, u, vt; 84 cv::SVDecomp(A, w, u, vt); 85 86 //3.ConvertToSolution 87 double B11 = vt(vt.rows - 1, 0); 88 double B22 = vt(vt.rows - 1, 1); 89 double B33 = vt(vt.rows - 1, 2); 90 double gamma = B33 - cx * cx * B11 - cy * cy * B22; 91 camK(0, 0) = sqrt(gamma / B11); 92 camK(1, 1) = sqrt(gamma / B22); 93 } 94 else if (method == 4)//const: skew=0 95 { 96 //1.CreateEquations 97 for (int k = 0; k < point3D.size(); ++k) 98 { 99 //1.1 FindHomo 100 Mat_<double> H = findHomography(point3D[k], point2D[k]); 101 Hs.push_back(H); 102 103 //1.2 CalcCoeffs 104 auto CalcCoeffs = [](Mat_<double>& H, int u, int v)->Mat_<double> 105 { 106 return Mat_<double>({ 1, 5 }, { 107 H(0, u) * H(0, v), 108 H(0, u) * H(2, v) + H(2, u) * H(0, v), 109 H(1, u) * H(1, v), 110 H(1, u) * H(2, v) + H(2, u) * H(1, v), 111 H(2, u) * H(2, v) }); 112 }; 113 114 //1.3 GetEquation 115 A.push_back(CalcCoeffs(H, 0, 1)); 116 A.push_back((CalcCoeffs(H, 0, 0) - CalcCoeffs(H, 1, 1))); 117 } 118 119 //2.SolveEquations 120 Mat_<double> w, u, vt; 121 cv::SVDecomp(A, w, u, vt); 122 123 //3.ConvertToSolution 124 double B11 = vt(vt.rows - 1, 0); 125 double B13 = vt(vt.rows - 1, 1); 126 double B22 = vt(vt.rows - 1, 2); 127 double B23 = vt(vt.rows - 1, 3); 128 double B33 = vt(vt.rows - 1, 4); 129 camK(0, 2) = -B13 / B11; 130 camK(1, 2) = -B23 / B22; 131 double gamma = B33 + camK(0, 2) * B13 + camK(1, 2) * B23; 132 camK(0, 0) = sqrt(gamma / B11); 133 camK(1, 1) = sqrt(gamma / B22); 134 } 135 136 //2.CalcExtrinsic 137 for (int k = 0; k < point3D.size(); ++k) 138 { 139 //2.1 FindHomos 140 Mat_<double> H = Hs[k]; 141 142 //2.2 GetScaledR 143 double ilambda = 1. / norm(camK.inv() * H.col(0), NORM_L2); 144 Mat_<double> r1 = ilambda * camK.inv() * H.col(0); 145 Mat_<double> r2 = ilambda * camK.inv() * H.col(1); 146 Mat_<double> r3 = r1.cross(r2); 147 Mat_<double> R({ 3, 3 }, { r1(0), r2(0), r3(0), r1(1), r2(1), r3(1), r1(2), r2(2), r3(2) }); 148 149 //2.3 GetUnitRt 150 Mat_<double> u, vt; cv::SVDecomp(R, Matx31d(), u, vt); 151 Mat_<double> r; cv::Rodrigues(u * vt, r); 152 Mat_<double> t = ilambda * camK.inv() * H.col(2); 153 camRs.push_back(Vec3d(r)); 154 camTs.push_back(Vec3d(t)); 155 } 156 } 157 158 static void initKBCM() {} 159 160 static double initMUCM(vector<vector<Point3f>>& point3D, vector<vector<Point2f>>& point2D, Size imaSize, Size chboardHV, Mat_<double>& camK, Mat_<Vec3d>& camRs, Mat_<Vec3d>& camTs) 161 { 162 //0.FormatInput 163 if (camK.rows != 3 || camK.cols != 3) camK.create(3, 3); 164 if (camRs.total() != point3D.size()) camRs.create(int(point3D.size()), 1); 165 if (camTs.total() != point3D.size()) camTs.create(int(point3D.size()), 1); 166 camK(0, 2) = imaSize.width / 2.0; 167 camK(1, 2) = imaSize.height / 2.0; 168 camK(2, 2) = 1; camK(0, 1) = camK(1, 0) = camK(2, 0) = camK(2, 1) = 0; 169 170 //1.MainTask 171 double& ff = camK(0, 0); 172 const double cx = camK(0, 2); 173 const double cy = camK(1, 2); 174 double minReprErr = DBL_MAX; 175 vector<Vec3d> camRs0(point3D.size()); 176 vector<Vec3d> camTs0(point3D.size()); 177 for (int ind = 0; ind < point3D.size(); ++ind) 178 for (int i = 0; i < chboardHV.height; ++i) 179 { 180 //1.GetEquations 181 Mat_<double> P(chboardHV.width, 4); 182 for (int j = 0; j < chboardHV.width; ++j) 183 { 184 Point2f pt = point2D[ind][i * chboardHV.width + j]; 185 double u = pt.x - cx; 186 double v = pt.y - cy; 187 P(j, 0) = u; 188 P(j, 1) = v; 189 P(j, 2) = 0.5; 190 P(j, 3) = -0.5 * (u * u + v * v); 191 } 192 193 //2.SolveEquations 194 Mat_<double> C; 195 SVD::solveZ(P, C); 196 double t = C(0) * C(0) + C(1) * C(1) + C(2) * C(3); 197 if (t < 0.0) continue; 198 199 //3.LineIma cannot be radial 200 double d = std::sqrt(1.0 / t); 201 double nx = C(0) * d; 202 double ny = C(1) * d; 203 if (std::hypot(nx, ny) > 0.95) continue; 204 double ffk = std::sqrt(C(2) / C(3)); 205 206 //4.CalcAllRt 207 double reprErr = 0; 208 double nPoint = 0; 209 for (int k = 0; k < point3D.size(); ++k) 210 { 211 //4.1 CalcEachRt(CanCallInitRt) 212 vector<Point3f> point3DEx; 213 vector<Point2f> point2DEx; 214 for (int ii = 0; ii < point2D[k].size(); ++ii) 215 { 216 double x = (point2D[k][ii].x - cx) / ffk; 217 double y = (point2D[k][ii].y - cy) / ffk; 218 double z = (1 - x * x - y * y) / 2;//because of a=b=1 D=0 219 if (z < 0.01) continue; // { spdlog::warn("Skip point3DRay[{}] ({}, {}, {}) which is close to or more than 180: z<0.01", ii, x, y, z); continue; } 220 point3DEx.push_back(point3D[k][ii]); 221 point2DEx.push_back(Point2f(float(x / z), float(y / z))); 222 } 223 if (point3DEx.size() < 8) { spdlog::warn("View{}---Line{}---View{}--ValidPoints{}", ind, i, k, point3DEx.size()); continue; } 224 cv::solvePnP(point3DEx, point2DEx, Mat_<double>::eye(3, 3), noArray(), camRs0[k], camTs0[k]); 225 226 //4.2 CalcReprErr(CanCallProjectPoints) 227 Matx33d R; cv::Rodrigues(camRs0[k], R); 228 for (int ii = 0; ii < point3D[k].size(); ++ii) 229 { 230 double x = R.val[0] * point3D[k][ii].x + R.val[1] * point3D[k][ii].y + R.val[2] * point3D[k][ii].z + camTs0[k][0]; 231 double y = R.val[3] * point3D[k][ii].x + R.val[4] * point3D[k][ii].y + R.val[5] * point3D[k][ii].z + camTs0[k][1]; 232 double z = R.val[6] * point3D[k][ii].x + R.val[7] * point3D[k][ii].y + R.val[8] * point3D[k][ii].z + camTs0[k][2]; 233 double d = sqrt(x * x + y * y + z * z);//because of a=b=1 D=0 234 double iz = 1. / (d + z); //if denominator==0 235 double xn = x * iz; 236 double yn = y * iz; 237 reprErr += cv::norm(Vec2d(ffk * xn + cx - point2D[k][ii].x, ffk * yn + cy - point2D[k][ii].y)); 238 } 239 nPoint += point3D[k].size(); 240 } 241 if ((reprErr /= nPoint) < minReprErr) //1.4185722111399552 242 { 243 minReprErr = reprErr; 244 ff = ffk; 245 for (int k = 0; k < point3D.size(); ++k) 246 { 247 camRs(k) = camRs0[k]; 248 camTs(k) = camTs0[k]; 249 } 250 } 251 } 252 253 //2.MakeDecision 254 camK(1, 1) = camK(0, 0); 255 return minReprErr; 256 } 257 258 public: 259 static void TestInitRTCM(int argc, char** argv) 260 { 261 for (int k = 0; k < 999; ++k) 262 { 263 //0.GenerateData 264 const int camInd = 0; 265 Motion2D::BoardMotion boardMotion; 266 boardMotion.initMotion(5, Motion2D::VisionCM::RTCM, true, false); 267 boardMotion.createRawMotion(2, camInd); 268 boardMotion.createDstMotion(1, camInd); 269 Size imaSize(boardMotion.imaCols, boardMotion.imaRows); 270 Mat_<double> camK0({ 3, 3 }, { boardMotion.cams[camInd].K[0], 0, boardMotion.cams[camInd].K[2], 0, boardMotion.cams[camInd].K[1], boardMotion.cams[camInd].K[3], 0, 0, 1 }); 271 vector<Motion2D::SolidPose> camPoses0 = boardMotion.dstObs[camInd].poses; 272 Mat_<Vec3d> camRs0; for (int i = 0; i < camPoses0.size(); ++i) camRs0.push_back(Vec3d(camPoses0[i].mdata)); 273 Mat_<Vec3d> camTs0; for (int i = 0; i < camPoses0.size(); ++i) camTs0.push_back(Vec3d(camPoses0[i].tdata)); 274 275 //1.CalcByDIY1 276 Mat_<double> camK1; 277 Mat_<Vec3d> camRs1; 278 Mat_<Vec3d> camTs1; 279 initRTCM(boardMotion.dstObs[camInd].point3D, boardMotion.dstObs[camInd].point2D, imaSize, camK1, camRs1, camTs1, 1); 280 281 //2.CalcByDIY2 282 Mat_<double> camK2; 283 Mat_<Vec3d> camRs2; 284 Mat_<Vec3d> camTs2; 285 initRTCM(boardMotion.dstObs[camInd].point3D, boardMotion.dstObs[camInd].point2D, imaSize, camK2, camRs2, camTs2, 2); 286 287 //3.CalcByDIY3 288 Mat_<double> camK3; 289 Mat_<Vec3d> camRs3; 290 Mat_<Vec3d> camTs3; 291 initRTCM(boardMotion.dstObs[camInd].point3D, boardMotion.dstObs[camInd].point2D, imaSize, camK3, camRs3, camTs3, 3); 292 293 //4.CalcByDIY4 294 Mat_<double> camK4; 295 Mat_<Vec3d> camRs4; 296 Mat_<Vec3d> camTs4; 297 initRTCM(boardMotion.dstObs[camInd].point3D, boardMotion.dstObs[camInd].point2D, imaSize, camK4, camRs4, camTs4, 4); 298 299 //5.CalcByOpenCV 300 Mat_<double> camK9 = initCameraMatrix2D(boardMotion.dstObs[camInd].point3D, boardMotion.dstObs[camInd].point2D, imaSize, 0); 301 302 //6.AnalyzeError 303 double infcamK0camK1 = cv::norm(camK0, camK1, NORM_INF), infRs0Rs1 = cv::norm(camRs0, camRs1, NORM_INF), infTs0Ts1 = cv::norm(camTs0, camTs1, NORM_INF); 304 double infcamK0camK2 = cv::norm(camK0, camK2, NORM_INF), infRs0Rs2 = cv::norm(camRs0, camRs2, NORM_INF), infTs0Ts2 = cv::norm(camTs0, camTs2, NORM_INF); 305 double infcamK0camK3 = cv::norm(camK0, camK3, NORM_INF), infRs0Rs3 = cv::norm(camRs0, camRs3, NORM_INF), infTs0Ts3 = cv::norm(camTs0, camTs3, NORM_INF); 306 double infcamK0camK4 = cv::norm(camK0, camK4, NORM_INF), infRs0Rs4 = cv::norm(camRs0, camRs4, NORM_INF), infTs0Ts4 = cv::norm(camTs0, camTs4, NORM_INF); 307 double infcamK0camK9 = cv::norm(camK0, camK9, NORM_INF); 308 double infcamK1camK9 = cv::norm(camK1, camK9, NORM_INF); 309 310 //7.PrintError 311 cout << endl << "LoopCount: " << k << endl; 312 if (infcamK1camK9 > 1E-9) 313 { 314 cout << endl << fmt::format("infcamK0camK1: {} infcamRs0Rs1_camTs0Ts1: {:.6f} {:.6f}", infcamK0camK1, infRs0Rs1, infTs0Ts1) << endl; 315 cout << endl << fmt::format("infcamK0camK2: {} infcamRs0Rs2_camTs0Ts2: {:.6f} {:.6f}", infcamK0camK2, infRs0Rs2, infTs0Ts2) << endl; 316 cout << endl << fmt::format("infcamK0camK3: {} infcamRs0Rs3_camTs0Ts3: {:.6f} {:.6f}", infcamK0camK3, infRs0Rs3, infTs0Ts3) << endl; 317 cout << endl << fmt::format("infcamK0camK4: {} infcamRs0Rs4_camTs0Ts4: {:.6f} {:.6f}", infcamK0camK4, infRs0Rs4, infTs0Ts4) << endl; 318 cout << endl << "infcamK0camK9: " << infcamK0camK9 << endl; 319 cout << endl << "infcamK1camK9: " << infcamK1camK9 << endl; 320 if (1) 321 { 322 cout << endl << "camK1: " << camK1.reshape(1, 1) << endl; 323 cout << endl << "camK2: " << camK2.reshape(1, 1) << endl; 324 cout << endl << "camK3: " << camK3.reshape(1, 1) << endl; 325 cout << endl << "camK4: " << camK4.reshape(1, 1) << endl; 326 cout << endl << "camK0: " << camK0.reshape(1, 1) << endl; 327 cout << endl << "camK9: " << camK9.reshape(1, 1) << endl; 328 } 329 cout << endl << "Press any key to continue" << endl; getchar(); 330 } 331 } 332 } 333 334 static void TestInitKBCM(int argc, char** argv) {} 335 336 static void TestInitMUCM(int argc, char** argv) 337 { 338 for (int k = 0; k < 999; ++k) 339 { 340 //0.GenerateData 341 const int camInd = 0; 342 Motion2D::BoardMotion boardMotion; 343 boardMotion.initMotion(4, Motion2D::VisionCM::MUCM, false, false); 344 boardMotion.createRawMotion(2, camInd); 345 boardMotion.createDstMotion(1, camInd); 346 Size imaSize(boardMotion.imaCols, boardMotion.imaRows); 347 Mat_<double> camK0({ 3, 3 }, { boardMotion.cams[camInd].K[0], 0, boardMotion.cams[camInd].K[2], 0, boardMotion.cams[camInd].K[1], boardMotion.cams[camInd].K[3], 0, 0, 1 }); 348 Mat_<double> camD0(boardMotion.cams[camInd].nDist, 1, boardMotion.cams[camInd].D); 349 Mat_<double> camAB0(boardMotion.cams[camInd].nModel, 1, boardMotion.cams[camInd].ab); 350 Mat_<Vec3d> camRs0; for (int i = 0; i < boardMotion.dstObs[camInd].poses.size(); ++i) camRs0.push_back(Vec3d(boardMotion.dstObs[camInd].poses[i].vdata)); 351 Mat_<Vec3d> camTs0; for (int i = 0; i < boardMotion.dstObs[camInd].poses.size(); ++i) camTs0.push_back(Vec3d(boardMotion.dstObs[camInd].poses[i].tdata)); 352 Size chboardHV(boardMotion.nHorCorner, boardMotion.nVerCorner); 353 354 //1.CalcByDIY 355 Mat_<double> camK1; 356 Mat_<Vec3d> camRs1; 357 Mat_<Vec3d> camTs1; 358 initMUCM(boardMotion.dstObs[camInd].point3D, boardMotion.dstObs[camInd].point2D, imaSize, chboardHV, camK1, camRs1, camTs1); 359 360 //2.CalcByCamodoOrBesalt 361 Mat_<double> camK2(3, 3); 362 Mat_<Vec3d> camRs2; 363 Mat_<Vec3d> camTs2; 364 vector<double> caliParams; 365 //camodocal::CameraCalibration calibration = camodocal::CameraCalibration(camodocal::Camera::MEI, "XUCM", imaSize, chboardHV, -1); 366 //calibration.m_camera->estimateIntrinsics(chboardHV, boardMotion.dstObs[camInd].point3D, boardMotion.dstObs[camInd].point2D); 367 //calibration.m_camera->writeParameters(caliParams); 368 //camK2 << caliParams[5], 0, caliParams[7], 0, caliParams[6], caliParams[8], 0, 0, 1; 369 370 //3.AnalyzeError 371 double infcamK0camK1 = cv::norm(camK0, camK1, NORM_INF), infRs0Rs1 = cv::norm(camRs0, camRs1, NORM_INF), infTs0Ts1 = cv::norm(camTs0, camTs1, NORM_INF); 372 //double infcamK0camK2 = cv::norm(camK0, camK2, NORM_INF); 373 374 //4.PrintError 375 cout << endl << "LoopCount: " << k << endl; 376 if (infcamK0camK1 > 1E-9) 377 { 378 cout << endl << fmt::format("infcamK0camK1: {} infcamRs0Rs1_camTs0Ts1: {:.6f} {:.6f}", infcamK0camK1, infRs0Rs1, infTs0Ts1) << endl; 379 if (1) 380 { 381 cout << endl << "camK0: " << camK0.reshape(1, 1) << endl; 382 cout << endl << "camK1: " << camK1.reshape(1, 1) << endl; 383 } 384 cout << endl << "Press any key to continue" << endl; getchar(); 385 } 386 } 387 } 388 }; 389 390 int main(int argc, char** argv) { InitCamera::TestInitRTCM(argc, argv); return 0; } 391 int main1(int argc, char** argv) { InitCamera::TestInitKBCM(argc, argv); return 0; } 392 int main2(int argc, char** argv) { InitCamera::TestInitMUCM(argc, argv); return 0; }
