导航

CV学习日志:CV开发之立体仿真器

Posted on 2020-01-31 23:51  dzyBK  阅读(318)  评论(0编辑  收藏  举报

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: {}\n", cvarr2str(r.t()));
 41             str += fmt::format("t: {}\n", cvarr2str(t.t()));
 42             str += fmt::format("q: {}\n", cvarr2str(q.t()));
 43             str += fmt::format("rt: {}\n", cvarr2str(rt.t()));
 44             str += fmt::format("radian: {}\n", cvarr2str(radian.t()));
 45             str += fmt::format("degree: {}\n", cvarr2str(degree.t()));
 46             str += fmt::format("R: {}\n", cvarr2str(R));
 47             str += fmt::format("T: {}\n", cvarr2str(T));
 48             str += fmt::format("K: {}\n", cvarr2str(K));
 49             str += fmt::format("D: {}\n", 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\n", 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)\n", 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\n", 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\n", 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\n", 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\n", 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={}\n", 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: {}\nK: {}\nD: {}", 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: {}\nValidPoints: {}\nMin3DXY_Min2DXY: {}, {}, {}, {}\nMax3DXY_Max2DXY: {}, {}, {}, {}\nRot_Trans_Euler: {}\n",
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; }
View Code