1.本文要点说明
基于之前介绍的视觉成像器，对空间世界进行成像仿真，对相机系进行位姿变换以用于位姿解算。
(0)关于固定参数：物理尺度10cm基准，建议畸变范围±0.2。
(1)关于坐标系统：世界系采用Z轴向下的右手系，相机系亦同。
(2)关于世界目标：创建m*n=100*100目标，Z坐标=0或randu(-1,-99)，Y坐标=10*I(I=0~n)+randu(0,9)，X坐标=10*J(J=0~m)+randu(0,9)，令世界边长L=(10*m+10*n)/2。
(3)关于相机位姿：创建max=256或num目标，Z坐标=randu(-101,-150)，Y坐标=LastY+randu(0,9)，X坐标=LastX+randu(0,9)，num为成像目标数少于min=64时值，RPY=同时绕XYZ轴旋转旋转e=randu(-5, 5)。
(4)关于随机化设置：可设置(调用cv::setRNGSeed(clock())实现)是否使用随机种子(默认不使用)，若需要每次启动都得到不同的仿真数据，则设置为使用。
(5)关于可视化操作：启动可视化后，按空格可逐次可视化每次观察，当到达最后一次观察后则返回第一次观察。
2.实验测试代码
依赖于OpenCV、Ceres和Spdlog，封装在类Motion3D：
(1)cvarr2str+euler2matrix
(2)MotionView
(3)runMotion+visMotion+TestMe
1 #include <opencv2/opencv.hpp> 2 #include <opencv2/viz.hpp> 3 #include <spdlog/spdlog.h> 4 using namespace std; 5 using namespace cv; 6 7 class MotionSim 8 { 9 public: 10 static void TestMe(int argc, char** argv) 11 { 12 MotionSim motionSim(false); 13 motionSim.camFovX = 45; 14 motionSim.camFovY = 30; 15 motionSim.camRand = 10; 16 motionSim.enableVerbose = false; 17 motionSim.runMotion(false, false, 7); 18 motionSim.visMotion(); 19 } 20 21 public: 22 struct MotionView 23 { 24 Mat_<double> r = Mat_<double>(3, 1); 25 Mat_<double> t = Mat_<double>(3, 1); 26 Mat_<double> q = Mat_<double>(4, 1); 27 Mat_<double> rt = Mat_<double>(6, 1); 28 Mat_<double> radian = Mat_<double>(3, 1); 29 Mat_<double> degree = Mat_<double>(3, 1); 30 Mat_<double> R = Mat_<double>(3, 3); 31 Mat_<double> T = Mat_<double>(3, 4); 32 Mat_<double> K; 33 Mat_<double> D; 34 Mat_<Vec3d> point3D; 35 Mat_<Vec2d> point2D; 36 Mat_<int> point3DIds; 37 string print(string savePath = "") 38 { 39 string str; 40 str += fmt::format("r: {} ", cvarr2str(r.t())); 41 str += fmt::format("t: {} ", cvarr2str(t.t())); 42 str += fmt::format("q: {} ", cvarr2str(q.t())); 43 str += fmt::format("rt: {} ", cvarr2str(rt.t())); 44 str += fmt::format("radian: {} ", cvarr2str(radian.t())); 45 str += fmt::format("degree: {} ", cvarr2str(degree.t())); 46 str += fmt::format("R: {} ", cvarr2str(R)); 47 str += fmt::format("T: {} ", cvarr2str(T)); 48 str += fmt::format("K: {} ", cvarr2str(K)); 49 str += fmt::format("D: {} ", cvarr2str(D.t())); 50 if (savePath.empty() == false) { FILE* out = fopen(savePath.c_str(), "w"); fprintf(out, str.c_str()); fclose(out); } 51 return str; 52 } 53 }; 54 static string cvarr2str(InputArray v) 55 { 56 Ptr<Formatted> fmtd = cv::format(v, Formatter::FMT_DEFAULT); 57 string dst; fmtd->reset(); 58 for (const char* str = fmtd->next(); str; str = fmtd->next()) dst += string(str); 59 return dst; 60 } 61 static void euler2matrix(double e[3], double R[9], bool forward = true, int argc = 0, char** argv = 0) 62 { 63 if (argc > 0) 64 { 65 int N = 999; 66 for (int k = 0; k < N; ++k)//OpenCV not better than DIY 67 { 68 //1.GenerateData 69 Matx31d radian0 = radian0.randu(-3.14159265358979323846, 3.14159265358979323846); 70 Matx33d R; euler2matrix(radian0.val, R.val, true); 71 const double deg2rad = 3.14159265358979323846 * 0.0055555555555555556; 72 const double rad2deg = 180 * 0.3183098861837906715; 73 74 //2.CalcByOpenCV 75 Matx31d radian1 = cv::RQDecomp3x3(R, Matx33d(), Matx33d()) * deg2rad; 76 77 //3.CalcByDIY 78 Matx31d radian2; euler2matrix(R.val, radian2.val, false); 79 80 //4.AnalyzeError 81 double infRadian0Radian1 = norm(radian0, radian1, NORM_INF); 82 double infRadian1Radian2 = norm(radian1, radian2, NORM_INF); 83 84 //5.PrintError 85 cout << endl << "LoopCount: " << k << endl; 86 if (infRadian0Radian1 > 0 || infRadian1Radian2 > 0) 87 { 88 cout << endl << "5.1PrintError" << endl; 89 cout << endl << "infRadian0Radian1: " << infRadian0Radian1 << endl; 90 cout << endl << "infRadian1Radian2: " << infRadian1Radian2 << endl; 91 if (0) 92 { 93 cout << endl << "5.2PrintDiff" << endl; 94 cout << endl << "radian0-degree0:" << endl << radian0.t() << endl << radian0.t() * rad2deg << endl; 95 cout << endl << "radian1-degree1:" << endl << radian1.t() << endl << radian1.t() * rad2deg << endl; 96 cout << endl << "radian2-degree2:" << endl << radian2.t() << endl << radian2.t() * rad2deg << endl; 97 cout << endl << "5.3PrintOthers" << endl; 98 cout << endl << "R:" << endl << R << endl; 99 } 100 cout << endl << "Press any key to continue" << endl; std::getchar(); 101 } 102 } 103 return; 104 } 105 if (forward)//check with 3D Rotation Converter 106 { 107 double sinR = std::sin(e[0]); 108 double sinP = std::sin(e[1]); 109 double sinY = std::sin(e[2]); 110 double cosR = std::cos(e[0]); 111 double cosP = std::cos(e[1]); 112 double cosY = std::cos(e[2]); 113 114 //RPY indicates: first Yaw aroundZ, second Pitch aroundY, third Roll aroundX 115 R[0] = cosY * cosP; R[1] = cosY * sinP * sinR - sinY * cosR; R[2] = cosY * sinP * cosR + sinY * sinR; 116 R[3] = sinY * cosP; R[4] = sinY * sinP * sinR + cosY * cosR; R[5] = sinY * sinP * cosR - cosY * sinR; 117 R[6] = -sinP; R[7] = cosP * sinR; R[8] = cosP * cosR; 118 } 119 else 120 { 121 double vs1 = std::abs(R[6] - 1.); 122 double vs_1 = std::abs(R[6] + 1.); 123 if (vs1 > 1E-9 && vs_1 > 1E-9) 124 { 125 e[2] = std::atan2(R[3], R[0]); //Yaw aroundZ 126 e[1] = std::asin(-R[6]);//Pitch aroundY 127 e[0] = std::atan2(R[7], R[8]); //Roll aroundX 128 } 129 else if (vs_1 <= 1E-9) 130 { 131 e[2] = 0; //Yaw aroundZ 132 e[1] = 3.14159265358979323846 * 0.5;//Pitch aroundY 133 e[0] = e[2] + atan2(R[1], R[2]); //Roll aroundX 134 } 135 else 136 { 137 e[2] = 0; //Yaw aroundZ 138 e[1] = -3.14159265358979323846 * 0.5;//Pitch aroundY 139 e[0] = -e[2] + atan2(-R[1], -R[2]); //Roll aroundX 140 } 141 } 142 }; 143 static void quat2matrix(double q[4], double R[9], bool forward = true) 144 { 145 if (forward)//refer to qglviwer 146 { 147 double L1 = std::sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); 148 if (std::abs(L1 - 1) > 1E-9) { std::printf("Not uint quaternion: NormQ=%.9f ", L1); abort(); } 149 150 double xx = 2.0 * q[1] * q[1]; 151 double yy = 2.0 * q[2] * q[2]; 152 double zz = 2.0 * q[3] * q[3]; 153 154 double xy = 2.0 * q[1] * q[2]; 155 double xz = 2.0 * q[1] * q[3]; 156 double wx = 2.0 * q[1] * q[0]; 157 158 double yz = 2.0 * q[2] * q[3]; 159 double wy = 2.0 * q[2] * q[0]; 160 161 double wz = 2.0 * q[3] * q[0]; 162 163 R[0] = 1.0 - yy - zz; 164 R[4] = 1.0 - xx - zz; 165 R[8] = 1.0 - xx - yy; 166 167 R[1] = xy - wz; 168 R[3] = xy + wz; 169 170 R[2] = xz + wy; 171 R[6] = xz - wy; 172 173 R[5] = yz - wx; 174 R[7] = yz + wx; 175 } 176 else 177 { 178 double onePlusTrace = 1.0 + R[0] + R[4] + R[8];// Compute one plus the trace of the matrix 179 if (onePlusTrace > 1E-9) 180 { 181 double s = sqrt(onePlusTrace) * 2.0; 182 double is = 1 / s; 183 q[0] = 0.25 * s; 184 q[1] = (R[7] - R[5]) * is; 185 q[2] = (R[2] - R[6]) * is; 186 q[3] = (R[3] - R[1]) * is; 187 } 188 else 189 { 190 std::printf("1+trace(R)=%.9f is too small and (R11,R22,R33)=(%.9f,%.9f,%.9f) ", onePlusTrace, R[0], R[4], R[8]); 191 if ((R[0] > R[4]) && (R[0] > R[8]))//max(R00, R11, R22)=R00 192 { 193 double s = sqrt(1.0 + R[0] - R[4] - R[8]) * 2.0; 194 double is = 1 / s; 195 q[0] = (R[5] - R[7]) * is; 196 q[1] = 0.25 * s; 197 q[2] = (R[1] + R[3]) * is; 198 q[3] = (R[2] + R[6]) * is; 199 } 200 else if (R[4] > R[8])//max(R00, R11, R22)=R11 201 { 202 double s = sqrt(1.0 - R[0] + R[4] - R[8]) * 2.0; 203 double is = 1 / s; 204 q[0] = (R[2] - R[6]) * is; 205 q[1] = (R[1] + R[3]) * is; 206 q[2] = 0.25 * s; 207 q[3] = (R[5] + R[7]) * is; 208 } 209 else//max(R00, R11, R22)=R22 210 { 211 double s = sqrt(1.0 - R[0] - R[4] + R[8]) * 2.0; 212 double is = 1 / s; 213 q[0] = (R[1] - R[3]) * is; 214 q[1] = (R[2] + R[6]) * is; 215 q[2] = (R[5] + R[7]) * is; 216 q[3] = 0.25 * s; 217 } 218 } 219 double L1 = std::sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); 220 if (L1 < 1e-9) { std::printf("Wrong rotation matrix: NormQ=%.9f ", L1); abort(); } 221 else { L1 = 1 / L1; q[0] *= L1; q[1] *= L1; q[2] *= L1; q[3] *= L1; } 222 } 223 } 224 static void vec2quat(double r[3], double q[4], bool forward = true) 225 { 226 if (forward)//refer to qglviwer 227 { 228 double theta = std::sqrt(r[0] * r[0] + r[1] * r[1] + r[2] * r[2]); 229 if (std::abs(theta) < 1E-9) 230 { 231 q[0] = 1; q[1] = q[2] = q[3] = 0; 232 std::printf("Rotation approximates zero: Theta=%.9f ", theta); 233 }; 234 235 q[0] = std::cos(theta * 0.5); 236 double ss = std::sin(theta * 0.5) / theta; 237 q[1] = r[0] * ss; 238 q[2] = r[1] * ss; 239 q[3] = r[2] * ss; 240 } 241 else 242 { 243 double L1 = std::sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); 244 if (std::abs(L1 - 1) > 1E-9) { std::printf("Not uint quaternion: NormQ=%.9f ", L1); abort(); } 245 246 double theta = 2 * acos(q[0]); 247 if (theta > 3.14159265358979323846) theta = 2 * 3.14159265358979323846 - theta; 248 double thetaEx = theta / std::sin(theta * 0.5); 249 r[0] = q[1] * thetaEx; 250 r[1] = q[2] * thetaEx; 251 r[2] = q[3] * thetaEx; 252 } 253 } 254 static void vec2matrix(double r[3], double R[9], bool forward = true, int argc = 0, char** argv = 0) 255 { 256 if (argc > 0) 257 { 258 int N = 999; 259 for (int k = 0; k < N; ++k) //refer to the subsequent article for more details 260 { 261 //1.GenerateData 262 Matx31d r0 = r0.randu(-999, 999); 263 Matx33d R0; cv::Rodrigues(r0, R0); 264 265 //2.CalcByOpenCV 266 Matx33d R1; 267 Matx31d r1; 268 cv::Rodrigues(r0, R1); 269 cv::Rodrigues(R0, r1); 270 271 //3.CalcByDIY 272 Matx33d R2; 273 Matx31d r2; 274 vec2matrix(r0.val, R2.val, true); 275 vec2matrix(r2.val, R0.val, false); 276 277 //4.AnalyzeError 278 double infR1R2 = norm(R1, R2, NORM_INF); 279 double infr1r2 = norm(r1, r2, NORM_INF); 280 281 //5.PrintError 282 cout << endl << "LoopCount: " << k << endl; 283 if (infR1R2 > 1E-12 || infr1r2 > 1E-12) 284 { 285 cout << endl << "5.1PrintError" << endl; 286 cout << endl << "infR1R2: " << infR1R2 << endl; 287 cout << endl << "infr1r2: " << infr1r2 << endl; 288 if (0) 289 { 290 cout << endl << "5.2PrintDiff" << endl; 291 cout << endl << "R1: " << endl << R1 << endl; 292 cout << endl << "R2: " << endl << R2 << endl; 293 cout << endl; 294 cout << endl << "r1: " << endl << r1.t() << endl; 295 cout << endl << "r2: " << endl << r2.t() << endl; 296 cout << endl << "5.3PrintOthers" << endl; 297 } 298 cout << endl << "Press any key to continue" << endl; std::getchar(); 299 } 300 } 301 return; 302 } 303 304 if (forward) 305 { 306 double theta = std::sqrt(r[0] * r[0] + r[1] * r[1] + r[2] * r[2]); 307 if (theta < 1E-9) 308 { 309 R[0] = R[4] = R[8] = 1.0; 310 R[1] = R[2] = R[3] = R[5] = R[6] = R[7] = 0.0; 311 std::printf("Rotation approximates zero: Theta=%.9f ", theta); 312 return; 313 } 314 double cs = cos(theta); 315 double sn = sin(theta); 316 double itheta = 1. / theta; 317 double cs1 = 1 - cs; 318 double nx = r[0] * itheta; 319 double ny = r[1] * itheta; 320 double nz = r[2] * itheta; 321 322 double nxnx = nx * nx, nyny = ny * ny, nznz = nz * nz; 323 double nxny = nx * ny, nxnz = nx * nz, nynz = ny * nz; 324 double nxsn = nx * sn, nysn = ny * sn, nzsn = nz * sn; 325 326 R[0] = nxnx * cs1 + cs; 327 R[3] = nxny * cs1 + nzsn; 328 R[6] = nxnz * cs1 - nysn; 329 330 R[1] = nxny * cs1 - nzsn; 331 R[4] = nyny * cs1 + cs; 332 R[7] = nynz * cs1 + nxsn; 333 334 R[2] = nxnz * cs1 + nysn; 335 R[5] = nynz * cs1 - nxsn; 336 R[8] = nznz * cs1 + cs; 337 338 if (0) 339 { 340 Mat_<double> dRdu({ 9, 4 }, { 341 2 * nx * cs1, 0, 0, (nxnx - 1) * sn, 342 ny * cs1, nx * cs1, -sn, nxny * sn - nz * cs, 343 nz * cs1, sn, nx * cs1, nxnz * sn + ny * cs, 344 ny * cs1, nx * cs1, sn, nxny * sn + nz * cs, 345 0, 2 * ny * cs1, 0, (nyny - 1) * sn, 346 -sn, nz * cs1, ny * cs1, nynz * sn - nx * cs, 347 nz * cs1, -sn, nx * cs1, nxnz * sn - ny * cs, 348 sn, nz * cs1, ny * cs1, nynz * sn + nx * cs, 349 0, 0, 2 * nz * cs1, (nznz - 1) * sn }); 350 351 Mat_<double> dudv({ 4, 4 }, { 352 itheta, 0, 0, -nx * itheta, 353 0, itheta, 0, -ny * itheta, 354 0, 0, itheta, -nz * itheta, 355 0, 0, 0, 1 }); 356 357 Mat_<double> dvdr({ 4, 3 }, { 358 1, 0, 0, 359 0, 1, 0, 360 0, 0, 1, 361 nx, ny, nz }); 362 363 Mat_<double> Jacobian = dRdu * dudv * dvdr;//rows=9 cols=3 364 } 365 } 366 else 367 { 368 double sx = R[7] - R[5]; 369 double sy = R[2] - R[6]; 370 double sz = R[3] - R[1]; 371 double sn = sqrt(sx * sx + sy * sy + sz * sz) * 0.5; 372 double cs = (R[0] + R[4] + R[8] - 1) * 0.5; 373 double theta = acos(cs); 374 double ss = 2 * sn; 375 double iss = 1. / ss; 376 double tss = theta * iss; 377 r[0] = tss * sx; 378 r[1] = tss * sy; 379 r[2] = tss * sz; 380 381 if (0) 382 { 383 Mat_<double> drdu({ 3, 4 }, { 384 tss, 0, 0, (sn - theta * cs) * iss * iss * sx * 2, 385 0, tss, 0, (sn - theta * cs) * iss * iss * sy * 2, 386 0, 0, tss, (sn - theta * cs) * iss * iss * sz * 2 }); 387 388 Mat_<double> dudR({ 4, 9 }, { 389 0, 0, 0, 0, 0, -1, 0, 1, 0, 390 0, 0, 1, 0, 0, 0, -1, 0, 0, 391 0, -1, 0, 1, 0, 0, 0, 0, 0, 392 -iss, 0, 0, 0, -iss, 0, 0, 0, -iss }); 393 394 Mat_<double> Jacobian = drdu * dudR;//rows=3 cols=9 395 } 396 } 397 } 398 399 private: 400 const int nHorPoint3D = 100; 401 const int nVerPoint3D = 100; 402 const double varPoint3DXY = 10.; 403 const double minPoint3DZ = 1.; 404 const double maxPoint3DZ = 99.; 405 const double minCamZ = 101.; 406 const double maxCamZ = 150.; 407 const double varCamDegree = 10.; 408 Mat_<Vec3d> allPoint3D = Mat_<Vec3d>(nVerPoint3D * nHorPoint3D, 1); 409 Mat_<double> allPoint3DZ = Mat_<double>(nVerPoint3D * nHorPoint3D, 1); 410 Mat_<double> K; 411 Mat_<double> D; 412 const double deg2rad = 3.14159265358979323846 * 0.0055555555555555556; 413 const double rad2deg = 180 * 0.3183098861837906715; 414 415 public: 416 int camRows = 480; 417 int camCols = 640; 418 int camFovY = 90; 419 int camFovX = 90; 420 int camRand = 10;//append random[0,camRand] to camera intrinsics 421 int nCamDist = 5;//refer to opencv for value domain 422 int nMinMotion = 32; // no less than X motion views 423 int nMaxMotion = INT_MAX; // no more than X motion views 424 int nPoint2DThenExit = 32;//exit when less than X pixies 425 int rotMode = 1 + 2 + 4;//0=noRot 1=xAxis 2=yAxis 4=zAxis 426 bool noTrans = false;//translate or not while motion 427 bool world2D = false;//planar world or not 428 bool rndSeek = true;//use random seek or not 429 bool enableVerbose = false;//check motions one by one or not 430 vector<MotionView> motionViews;//World Information: RightX, FrontY, DownZ 431 MotionSim(bool run = true, bool world2D0 = false, bool noTrans0 = false, int rotMode0 = 7) { if (run) runMotion(world2D0, noTrans0, rotMode0); } 432 433 public: 434 void runMotion(bool world2D0 = false, bool noTrans0 = false, int rotMode0 = 7) 435 { 436 world2D = world2D0; 437 noTrans = noTrans0; 438 rotMode = rotMode0; 439 motionViews.clear(); 440 if (rndSeek) cv::setRNGSeed(clock()); 441 while (motionViews.size() < nMinMotion) 442 { 443 //1.GetAllPoint3D 444 if (world2D) allPoint3DZ = 0.; 445 else cv::randu(allPoint3DZ, -maxPoint3DZ, -minPoint3DZ);//DownZ 446 for (int i = 0, k = 0; i < nVerPoint3D; ++i) 447 for (int j = 0; j < nHorPoint3D; ++j, ++k) 448 allPoint3D(k) = Vec3d((j + cv::randu<double>()) * varPoint3DXY, (i + cv::randu<double>()) * varPoint3DXY, allPoint3DZ(i, j)); 449 450 //2.GetCamParams 451 double camFx = camCols / 2. / std::tan(camFovX / 2. * deg2rad) + cv::randu<double>() * camRand; 452 double camFy = camRows / 2. / std::tan(camFovY / 2. * deg2rad) + cv::randu<double>() * camRand; 453 double camCx = camCols / 2. + cv::randu<double>() * camRand; 454 double camCy = camRows / 2. + cv::randu<double>() * camRand; 455 K.create(3, 3); K << camFx, 0, camCx, 0, camFy, camCy, 0, 0, 1; 456 D.create(nCamDist, 1); cv::randu(D, -1.0, 1.0); 457 458 //3.GetAllMotionView 459 motionViews.clear(); 460 for (int64 k = 0; ; ++k) 461 { 462 //3.1 JoinCamParams 463 MotionView view; 464 view.K = K.clone(); 465 view.D = D.clone(); 466 467 //3.2 GetCamTrans 468 if (k == 0) view.t(0) = view.t(1) = 0; 469 else 470 { 471 view.t(0) = motionViews[k - 1].t(0) + cv::randu<double>() * varPoint3DXY; 472 view.t(1) = motionViews[k - 1].t(1) + cv::randu<double>() * varPoint3DXY; 473 } 474 view.t(2) = minCamZ + cv::randu<double>() * (maxCamZ - minCamZ); 475 view.t(2) = -view.t(2);//DownZ 476 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); } 477 478 //3.3 GetCamRot: degree-->radian-->matrix-->vector&quaternion 479 view.degree = 0.; 480 if (rotMode & 1) view.degree(0) = cv::randu<double>() * varCamDegree; 481 if (rotMode & 2) view.degree(1) = cv::randu<double>() * varCamDegree; 482 if (rotMode & 4) view.degree(2) = cv::randu<double>() * varCamDegree; 483 view.radian = view.degree * deg2rad; 484 euler2matrix(view.radian.ptr<double>(), view.R.ptr<double>()); 485 cv::Rodrigues(view.R, view.r); 486 quat2matrix(view.q.ptr<double>(), view.R.ptr<double>(), false); 487 cv::hconcat(view.R, view.t, view.T); 488 cv::vconcat(view.r, view.t, view.rt); 489 490 //3.4 GetPoint3DAndPoint2D 491 Mat_<Vec2d> allPoint2D; 492 cv::projectPoints(allPoint3D, -view.r, -view.R.t() * view.t, view.K, view.D, allPoint2D); 493 for (int k = 0; k < allPoint2D.total(); ++k) 494 if (allPoint2D(k)[0] > 0 && allPoint2D(k)[0] < camCols && allPoint2D(k)[1] > 0 && allPoint2D(k)[1] < camRows) 495 { 496 view.point2D.push_back(allPoint2D(k)); 497 view.point3D.push_back(allPoint3D(k)); 498 view.point3DIds.push_back(k); 499 } 500 501 //3.5 PrintDetails 502 motionViews.push_back(view); 503 if (enableVerbose) 504 { 505 cout << endl << view.print(); 506 cout << fmt::format("view={} features={} ", k, view.point2D.rows); 507 double minV = 0, maxV = 0;//Distortion makes some minV next to maxV 508 int minId = 0, maxId = 0; 509 cv::minMaxIdx(allPoint2D.reshape(1, int(allPoint2D.total()) * allPoint2D.channels()), &minV, &maxV, &minId, &maxId); 510 cout << fmt::format("minInfo:({}, {})", minId, minV) << allPoint3D(minId / 2) << allPoint2D(minId / 2) << endl; 511 cout << fmt::format("maxInfo:({}, {})", maxId, maxV) << allPoint3D(maxId / 2) << allPoint2D(maxId / 2) << endl; 512 cout << "Press any key to continue" << endl; std::getchar(); 513 } 514 if (view.point2D.rows < nPoint2DThenExit || motionViews.size() > nMaxMotion) break; 515 } 516 } 517 } 518 void visMotion() 519 { 520 //1.CreateWidgets 521 Size2d validSize(nHorPoint3D * varPoint3DXY, nVerPoint3D * varPoint3DXY); 522 Mat_<cv::Affine3d> camPoses(int(motionViews.size()), 1); for (int k = 0; k < camPoses.rows; ++k) camPoses(k) = cv::Affine3d(motionViews[k].T); 523 viz::WText worldInfo(fmt::format("nMotionView: {} K: {} D: {}", motionViews.size(), cvarr2str(K), cvarr2str(D)), Point(10, 240), 10); 524 viz::WCoordinateSystem worldCSys(1000); 525 viz::WPlane worldGround(Point3d(validSize.width / 2, validSize.height / 2, 0), Vec3d(0, 0, 1), Vec3d(0, 1, 0), validSize); 526 viz::WCloud worldPoints(allPoint3D, Mat_<Vec3b>(allPoint3D.size(), Vec3b(0, 255, 0))); 527 viz::WTrajectory camTraj1(camPoses, viz::WTrajectory::FRAMES, 8); 528 viz::WTrajectorySpheres camTraj2(camPoses, 100, 2); 529 viz::WTrajectoryFrustums camTraj3(camPoses, Matx33d(K), 4., viz::Color::yellow()); 530 worldCSys.setRenderingProperty(viz::OPACITY, 0.1); 531 worldGround.setRenderingProperty(viz::OPACITY, 0.1); 532 camTraj2.setRenderingProperty(viz::OPACITY, 0.6); 533 534 //2.ShowWidgets 535 static viz::Viz3d viz3d(__FUNCTION__); 536 viz3d.showWidget("worldInfo", worldInfo); 537 viz3d.showWidget("worldCSys", worldCSys); 538 viz3d.showWidget("worldGround", worldGround); 539 viz3d.showWidget("worldPoints", worldPoints); 540 viz3d.showWidget("camTraj1", camTraj1); 541 viz3d.showWidget("camTraj2", camTraj2); 542 viz3d.showWidget("camTraj3", camTraj3); 543 544 //3.UpdateWidghts 545 static const vector<MotionView>& views = motionViews; 546 viz3d.registerKeyboardCallback([](const viz::KeyboardEvent& keyboarEvent, void* pVizBorad)->void 547 { 548 if (keyboarEvent.action != viz::KeyboardEvent::KEY_DOWN) return; 549 static int pos = 0; 550 if (keyboarEvent.code == ' ') 551 { 552 size_t num = views.size(); 553 size_t ind = pos % num; 554 double xmin3D = DBL_MAX, ymin3D = DBL_MAX, xmin2D = DBL_MAX, ymin2D = DBL_MAX; 555 double xmax3D = -DBL_MAX, ymax3D = -DBL_MAX, xmax2D = -DBL_MAX, ymax2D = -DBL_MAX; 556 for (size_t k = 0; k < views[ind].point3D.rows; ++k) 557 { 558 Vec3d pt3 = views[ind].point3D(int(k)); 559 Vec2d pt2 = views[ind].point2D(int(k)); 560 if (pt3[0] < xmin3D) xmin3D = pt3[0]; 561 if (pt3[0] > xmax3D) xmax3D = pt3[0]; 562 if (pt3[1] < ymin3D) ymin3D = pt3[1]; 563 if (pt3[1] > ymax3D) ymax3D = pt3[1]; 564 if (pt2[0] < xmin2D) xmin2D = pt2[0]; 565 if (pt2[0] > xmax2D) xmax2D = pt2[0]; 566 if (pt2[1] < ymin2D) ymin2D = pt2[1]; 567 if (pt2[1] > ymax2D) ymax2D = pt2[1]; 568 } 569 if (pos != 0) 570 { 571 for (int k = 0; k < views[ind == 0 ? num - 1 : ind - 1].point3D.rows; ++k) viz3d.removeWidget("active" + std::to_string(k)); 572 viz3d.removeWidget("viewInfo"); 573 viz3d.removeWidget("camSolid"); 574 } 575 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)); 576 viz3d.showWidget("viewInfo", viz::WText(fmt::format("CurrentMotion: {} ValidPoints: {} Min3DXY_Min2DXY: {}, {}, {}, {} Max3DXY_Max2DXY: {}, {}, {}, {} Rot_Trans_Euler: {} ", 577 ind, views[ind].point3D.rows, xmin3D, ymin3D, xmin2D, ymin2D, xmax3D, ymax3D, xmax2D, ymax2D, 578 cvarr2str(views[ind].r.t()) + cvarr2str(views[ind].t.t()) + cvarr2str(views[ind].degree.t())), Point(10, 10), 10)); 579 viz3d.showWidget("camSolid", viz::WCameraPosition(Matx33d(views[ind].K), 10, viz::Color::yellow()), cv::Affine3d(views[ind].T)); 580 ++pos; 581 } 582 }, 0); 583 viz3d.spin(); 584 } 585 }; 586 587 int main(int argc, char** argv) { MotionSim::TestMe(argc, argv); return 0; }
