zoukankan      html  css  js  c++  java
  • CV学习日志:CV开发之平面仿真器

    1.本文要点说明

             基于之前介绍的视觉成像器,对平面世界进行成像仿真,对世界系进行位姿变换以用于模型解算。

                  (0)关于基准参数(不能重设):物理尺度10cm基准,畸变范围±0.2。

                  (1)关于坐标系统(不能重设):相机系采用Z轴向前的右手系,世界系亦同。

                  (2)关于世界目标(不能重设):创建m*n=11*10目标,Z坐标=0,Y坐标=10*I(I=0~n),X坐标=10*J(J=0~m),令世界短长边分别为MinLen=min(10*m-10, 10*n-10)和MaxLen=max(10*m-10, 10*n-10)。

                  (3)关于世界位姿(不能重设):创建a*b*c=2*5*9位姿,Z坐标=z1和z2,XY坐标=±s1和±s2及0,RPY=除两个零X零Y位姿外分别绕X轴和Y轴旋转旋转±e(e=15,30或10,20)。

                                1)RTCM&&FOV100&&Size1280*800: z1=1.2*MinLen, z2=1.6*MinLen, z1=0.2*MinLen, z2=0.4*MinLen

                                2)KBCM&&FOV160&&Size1200*1200: z1=0.8*MinLen, z2=1.2*MinLen, z1=0.4*MinLen, z2=0.6*MinLen      cv::fisheye::calibrate在世界越往边界平移及畸变越大越容易发生易常???

                                3)RTUCM&&FOV240&&Size1600*1600: z1=*MinLen, z2=*MinLen, z1=*MinLen, z2=*MinLen

                  (4)关于相机位姿(提供重设):创建d=9个相机,Z坐标=randu(-MinLen, MinLen),Y坐标=randu(-MinLen, MinLen),X坐标=I*MinLen(I=0~9), RPY=randu(-15, 15)。

                  (5)关于相机内参(提供重设):基于给定的FOV随机生成,所有相机成像模型一样。

                  (6)关于随机化设置:可设置(调用cv::setRNGSeed(clock())实现)是否使用随机种子(默认不使用),若需要每次启动都得到不同的仿真数据,则设置为使用。

                  (7)关于可视化操作:启动可视化后,按空格可逐次可视化每次观察,当到达最后一次观察后则返回第一次观察,按所提示的功能键可调整世界位置,用于内参修改时调试世界位置,以获取足够的可用的视图数。

    2.实验测试代码

             依赖于OpenCV、Ceres和Spdlog,封装在类Motion2D:

                  (1)MotionBases:SolidPose、VisionCM、CamSolid、CamObservation

                  (2)BoardMotion:initMotion、resetBoardPose、resetCamKDAB、resetCamPose、createRawMotion、refineRawMotion、createDstMotion

                  (3)VisMotion:VisRTCM、VisKBCM、VisRTEUCM(仅四畸变的UCM)

                  (4)TestCalib:CalibMono、CalibBino

             关于BoardMotion:

                  (1)initMotion:设置成像模型、所有相机内参、所有相机位姿(与世界不同每个相机固定不动)、世界的所有位姿(与相机不同世界仅一个且对每个相机而言该世界都按相同的路径运动)及世界目标。

                  (2)resetBoardPose:对每个成像模型而言,世界位姿都是经调试后得到的,本不提供更改世界位姿的接口,但为visMotion能进行调试可行的世界位姿,提供了此接口。

                  (3)resetCamKDAB:重设指定相机内参。

                  (4)resetCamPose:设置指定相机的位姿。

                  (5)createRawMotion:用指定相机观察世界的运动。

                  (6)refineRawMotion:决策指定相机中的无效观察。

                  (7)createDstMotion:提取指定相机的有效观察。

             测试结果:可视化全正常、OpenCV透视标定通过、OpenCV等距标定不通过、OpenCV全向标定不算通过、CamOdoCal标定全通过。

             遗留问题:全向模型尚未实现基于输入的视场角计算内参、全向模型仅实现第二模型参数为1的情况、双球模型所有功能都尚未实现。

      1 #include <ceres/ModelCamera.h>
      2 #include <opencv2/viz.hpp>
      3 
      4 class Motion2D
      5 {
      6 public://Utilities
      7     static string cvarr2str(InputArray v)
      8     {
      9         Ptr<Formatted> fmtd = cv::format(v, Formatter::FMT_DEFAULT);
     10         string dstm; fmtd->reset();
     11         for (const char* str = fmtd->next(); str; str = fmtd->next()) dstm += string(str);
     12         return dstm;
     13     }
     14 
     15 public://MotionBases
     16     using SolidPose = ModelRotation::SolidPose;
     17     using VisionCM = ModelCamera::VisionCM;
     18     using CamSolid = ModelCamera::CamSolid;
     19     using CamObservation = ModelCamera::CamObservation;
     20 
     21 public://BoardMotion
     22     class BoardMotion
     23     {
     24     public://Cameras
     25         static const int ncamera = 9;
     26         CamSolid cams[ncamera];//support multiple cameras
     27         SolidPose camPoses[ncamera]; //all poses for each camera
     28         CamObservation rawObs[ncamera];//all views for each camera
     29         CamObservation dstObs[ncamera];//workable views for each camera
     30         int imaRows;
     31         int imaCols;
     32         const float maxDist = 0.2f;
     33         const double pi = 3.1415926535897932384626433832795;
     34         const double deg2rad = pi / 180;
     35         const double rad2deg = 180 / pi;
     36 
     37     public://BoardProps
     38         vector<Point3f> corner3D;
     39         const int nVerCorner = 10;
     40         const int nHorCorner = 11;
     41         const float cmSquareSide = 10;
     42         const float cmBoardWidth = (nHorCorner - 1) * cmSquareSide;
     43         const float cmBoardHeight = (nVerCorner - 1) * cmSquareSide;
     44         const float cmBoardMinSide = min(cmBoardWidth, cmBoardHeight);
     45         const float cmBoardMaxSide = max(cmBoardWidth, cmBoardHeight);
     46 
     47     public://BoardPoses
     48         static const int nmodel = 4;
     49         static const int ntrans = 10;
     50         static const int nrotat = 9;
     51         static const int nboard = ntrans * nrotat;
     52         SolidPose boardPoses[nmodel][nboard];
     53         Vec<double, nmodel> vecZ1 = Vec<double, nmodel>(cmBoardMinSide * 1.2, cmBoardMinSide * 0.8, cmBoardMinSide * 0.4, 0);
     54         Vec<double, nmodel> vecZ2 = Vec<double, nmodel>(cmBoardMinSide * 1.6, cmBoardMinSide * 1.2, cmBoardMinSide * 0.8, 0);
     55         Vec<double, nmodel> vecS1 = Vec<double, nmodel>(cmBoardMinSide * 0.2, cmBoardMinSide * 0.4, cmBoardMinSide * 0.6, 0);
     56         Vec<double, nmodel> vecS2 = Vec<double, nmodel>(cmBoardMinSide * 0.4, cmBoardMinSide * 0.6, cmBoardMinSide * 1.0, 0);
     57         const Vec<int, nmodel> vecRows = Vec<int, nmodel>(800, 1200, 1600, 0);
     58         const Vec<int, nmodel> vecCols = Vec<int, nmodel>(1280, 1200, 1600, 0);
     59         const Vec<int, nmodel> vecFOV = Vec<int, nmodel>(100, 160, 240, 0);
     60         const Vec<int, nmodel> euler1 = Vec<int, nmodel>(15, 15, 10, 0);
     61         const Vec<int, nmodel> euler2 = Vec<int, nmodel>(30, 30, 20, 0);
     62 
     63     public://Process
     64         void initMotion(short nDist0, short camModel0, short simPin0, bool rndSeed = false)
     65         {
     66             //1.CamModel
     67             for (int k = 0; k < ncamera; ++k)
     68             {
     69                 cams[k].nDist = nDist0;
     70                 cams[k].simPin = simPin0;
     71                 cams[k].camModel = camModel0;
     72                 if (cams[k].camModel == VisionCM::RTCM) { cams[k].nModel = 0; imaRows = vecRows[0]; imaCols = vecCols[0]; }
     73                 if (cams[k].camModel == VisionCM::KBCM) { cams[k].nModel = 0; imaRows = vecRows[1]; imaCols = vecCols[1]; }
     74                 if (cams[k].camModel == VisionCM::RTEUCM) { cams[k].nModel = 2; imaRows = vecRows[2]; imaCols = vecCols[2]; }
     75                 if (cams[k].camModel == VisionCM::RTDSCM) { cams[k].nModel = 2; imaRows = vecRows[3]; imaCols = vecCols[3]; }
     76             }
     77 
     78             //2.CamParams
     79             if (rndSeed) cv::setRNGSeed(clock());
     80             for (int k = 0; k < ncamera; ++k) resetCamKDAB(k);
     81 
     82             //3.CameraPoses
     83             for (int k = 0; k < ncamera; ++k)
     84             {
     85                 if (k != 0) camPoses[k].setPos(Matx31d::randu(-cmSquareSide, cmSquareSide).val); camPoses[k].tdata[0] = k * cmSquareSide;
     86                 if (k != 0) camPoses[k].setRot(Matx31d::randu(-15 * deg2rad, 15 * deg2rad).val, 3);
     87             }
     88 
     89             //4.BoardData
     90             for (int i = 0; i < nVerCorner; ++i)
     91                 for (int j = 0; j < nHorCorner; ++j)
     92                     corner3D.push_back(Point3f(j * cmSquareSide - cmBoardWidth / 2, i * cmSquareSide - cmBoardHeight / 2, 0));
     93 
     94             //5.BoardPoses
     95             resetBoardPose();
     96             //for (int k = 0; k < ncamera; ++k) cout << endl << fmt::format("##########Camera{} information##########
    {}{}", k, cams[k].print(), camPoses[k].print());
     97             //for (int i = 0; i < nmodel; ++i) for (int j = 0; j < nboard; ++j) cout << endl << fmt::format("##########CamModel{}-BoardPose{}##########
    {}", j, i, boardPoses[i][j].print());
     98         }
     99         void resetBoardPose(/*Only for visMotion to test approapriate board poses*/)
    100         {
    101             for (int whichModel = 0; whichModel < nmodel; ++whichModel)
    102             {
    103                 double z1 = vecZ1[whichModel], z2 = vecZ2[whichModel];
    104                 double s1 = vecS1[whichModel], s2 = vecS2[whichModel];
    105                 double e1 = euler1[whichModel], e2 = euler2[whichModel];
    106                 Vec3d boardTVec[ntrans] =
    107                 {
    108                     Vec3d(0, 0, z1), Vec3d(s1, 0, z1), Vec3d(-s1, 0, z1), Vec3d(0, s1, z1), Vec3d(0, -s1, z1),
    109                     Vec3d(0, 0, z2), Vec3d(s2, 0, z2), Vec3d(-s2, 0, z2), Vec3d(0, s2, z2), Vec3d(0, -s2, z2)
    110                 };
    111                 Vec3d boardRVec[nrotat] =
    112                 {
    113                     Vec3d(0, 0, 0),
    114                     Vec3d(e1 * deg2rad, 0, 0), Vec3d(e2 * deg2rad, 0, 0), Vec3d(-e1 * deg2rad, 0, 0), Vec3d(-e2 * deg2rad, 0, 0),
    115                     Vec3d(0, e1 * deg2rad, 0), Vec3d(0, e2 * deg2rad, 0), Vec3d(0, -e1 * deg2rad, 0),  Vec3d(0, -e2 * deg2rad, 0)
    116                 };
    117                 for (int i = 0, k = 0; i < ntrans; ++i)
    118                     for (int j = 0; j < nrotat; ++j, ++k)
    119                     {
    120                         boardPoses[whichModel][k].setPos(boardTVec[i].val);
    121                         boardPoses[whichModel][k].setRot(boardRVec[j].val, 3);
    122                     }
    123             }
    124         }
    125 
    126         void resetCamKDAB(int camInd = 0)
    127         {
    128             //1.ModelParams
    129             if (cams[camInd].camModel == VisionCM::RTCM || cams[camInd].camModel == VisionCM::KBCM) { cams[camInd].ab[0] = 0; cams[camInd].ab[1] = 1; }
    130             else if (cams[camInd].camModel == VisionCM::RTEUCM)
    131             {
    132                 cams[camInd].ab[1] = 1.;///Vec2d::randu(-9.9, 9.99)(0);
    133                 cams[camInd].ab[0] = sqrt(cams[camInd].ab[1] / pow(tan(((vecFOV[cams[camInd].camModel] < 200 ? 200 : vecFOV[cams[camInd].camModel] > 300 ? 330 : vecFOV[cams[camInd].camModel] + 30) * 0.5 - 90) * deg2rad), 2) + 1);
    134             }
    135             else if (cams[camInd].camModel == VisionCM::RTEUCM)
    136             {
    137                 cams[camInd].ab[1] = Vec2d::randu(-9.9, 9.99)(0);
    138                 cams[camInd].ab[0] = -1. / cos((vecFOV[cams[camInd].camModel] < 200 ? 200 : vecFOV[cams[camInd].camModel] > 300 ? 330 : vecFOV[cams[camInd].camModel] + 30) * 0.5 * deg2rad);
    139             }
    140             if (cams[camInd].simPin == 1) cams[camInd].ab[0] = cams[camInd].ab[0] / (1 + cams[camInd].ab[0]);
    141 
    142             //2.CameraParams
    143             cams[camInd].K[2] = imaCols * (0.5 + Vec2d::randu(-0.08, 0.08)(0));
    144             cams[camInd].K[3] = imaRows * (0.5 + Vec2d::randu(-0.08, 0.08)(0));
    145             if (cams[camInd].camModel == VisionCM::RTCM)
    146             {
    147                 double halfFOV = vecFOV[0] / 2 * deg2rad;
    148                 cams[camInd].K[0] = cams[camInd].K[1] = (imaCols + imaRows) / tan(halfFOV) / 4;
    149                 cams[camInd].K[0] *= (1 + Vec2d::randu(-0.02, 0.02)(0));
    150                 cams[camInd].K[1] *= (1 + Vec2d::randu(-0.02, 0.02)(0));
    151             }
    152             else if (cams[camInd].camModel == VisionCM::KBCM)
    153             {
    154                 double halfFOV = vecFOV[1] / 2 * deg2rad;
    155                 cams[camInd].K[0] = cams[camInd].K[1] = (imaCols + imaRows) / halfFOV / 4;
    156                 cams[camInd].K[0] *= (1 + Vec2d::randu(-0.01, 0.04)(0));
    157                 cams[camInd].K[1] *= (1 + Vec2d::randu(-0.01, 0.04)(0));
    158             }
    159             else if (cams[camInd].camModel == VisionCM::RTEUCM && cams[camInd].simPin == false)
    160             {
    161                 double halfFOV = vecFOV[2] / 2 * deg2rad;
    162                 if (0)
    163                 {
    164                     double A = cams[camInd].simPin ? cams[camInd].ab[0] / (1 - cams[camInd].ab[0]) : cams[camInd].ab[0], A2 = A * A, A3 = A * A2, A4 = A2 * A2;
    165                     double b = cams[camInd].ab[1], b2 = b * b;
    166                     double e = tan(abs(halfFOV - pi / 2)), e2 = e * e;
    167 
    168                     vector<double> coeffs = { e2 * b - e2 * b * A2 - b2 * A2, 2 * e * b * A, e2 * A2 + b * A2 - b * A4 - e2 * A4, 2 * e * A3, A4 };
    169                     vector<double> coeffs1 = { A4, 2 * e * A3, e2 * A2 + b * A2 - b * A4 - e2 * A4, 2 * e * b * A, e2 * b - e2 * b * A2 - b2 * A2 };
    170                     Mat_<double> Fs; cv::solvePoly(coeffs, Fs);
    171                     cout << endl << Fs << endl; getchar();
    172                 }
    173                 cams[camInd].K[0] = cams[camInd].K[1] = (cams[camInd].K[2] + cams[camInd].K[3]) * 0.66;
    174                 cams[camInd].K[0] *= (1 + Vec2d::randu(-0.01, 0.01)(0));
    175                 cams[camInd].K[1] *= (1 + Vec2d::randu(-0.01, 0.01)(0));
    176             }
    177             else if (cams[camInd].camModel == VisionCM::RTDSCM && cams[camInd].simPin == false) {}
    178 
    179             //3.DistortionParams
    180             memset(cams[camInd].D, 0, sizeof(cams[camInd].D)); cv::randu(Mat_<double>(cams[camInd].nDist, 1, cams[camInd].D), -maxDist, maxDist);
    181             stable_sort(cams[camInd].D, cams[camInd].D + cams[camInd].nDist, [](double a, double b)->bool { return abs(a) > abs(b); });
    182         }
    183         void resetCamPose(double* rvec, double* tvec, int camInd = 0)
    184         {
    185             camPoses[camInd].setPos(tvec);
    186             camPoses[camInd].setRot(rvec, 3);
    187         }
    188 
    189         void createRawMotion(int step = 1, int camInd = 0)
    190         {
    191             rawObs[camInd].clear();
    192             for (int k = 0; k < nboard; k += step)
    193             {
    194                 //2.1 GetPose
    195                 SolidPose viewPose = camPoses[camInd].inv().mul(boardPoses[cams[camInd].camModel][k]);
    196 
    197                 //2.2 ProjectPoints
    198                 vector<Point2f> corner2D;
    199                 if (cams[camInd].camModel == VisionCM::RTCM)
    200                     projectPoints(corner3D, Vec3d(viewPose.vdata), Vec3d(viewPose.tdata), Matx33d(cams[camInd].K[0], 0, cams[camInd].K[2], 0, cams[camInd].K[1], cams[camInd].K[3], 0, 0, 1), Mat_<double>(cams[camInd].nDist, 1, cams[camInd].D), corner2D);
    201                 else if (cams[camInd].camModel == VisionCM::KBCM)
    202                     fisheye::projectPoints(corner3D, corner2D, Vec3d(viewPose.vdata), Vec3d(viewPose.tdata), Matx33d(cams[camInd].K[0], 0, cams[camInd].K[2], 0, cams[camInd].K[1], cams[camInd].K[3], 0, 0, 1), Mat_<double>(cams[camInd].nDist, 1, cams[camInd].D));
    203                 else if (cams[camInd].camModel == VisionCM::RTEUCM)
    204                     omnidir::projectPoints(corner3D, corner2D, Vec3d(viewPose.vdata), Vec3d(viewPose.tdata), Matx33d(cams[camInd].K[0], 0, cams[camInd].K[2], 0, cams[camInd].K[1], cams[camInd].K[3], 0, 0, 1), cams[camInd].ab[0], Mat_<double>(cams[camInd].nDist, 1, cams[camInd].D));
    205                 else if (cams[camInd].camModel == VisionCM::RTEUCM) {}
    206                 //2.3 CheckValid
    207                 bool ok = true;
    208                 for (int k = 0; k < corner2D.size(); ++k)
    209                     if (corner2D[k].x < 10 || corner2D[k].x > imaCols - 10 || corner2D[k].y < 10 || corner2D[k].y > imaRows - 10) ok = false;
    210                 //if (!ok) spdlog::warn("Invalid view and tvec{} rvec{}: {} {} {} {} {} {}", i, j, tvec[0], tvec[1], tvec[2], rvec[0], rvec[1], rvec[2]);
    211 
    212                 //2.4 AddGroup
    213                 rawObs[camInd].poses.push_back(viewPose);
    214                 rawObs[camInd].point3D.push_back(corner3D);
    215                 rawObs[camInd].point2D.push_back(corner2D);
    216                 rawObs[camInd].oksView.push_back(ok);
    217             }
    218         }
    219         void refineRawMotion(vector<bool> oksView, int camInd = 0)
    220         {
    221             if (oksView.size() != rawObs[camInd].oksView.size()) { spdlog::critical("No same views: {}!={}", oksView.size(), rawObs[camInd].oksView.size()); assert(0); }
    222             for (int k = 0; k < rawObs[camInd].oksView.size(); ++k) rawObs[camInd].oksView[k] = rawObs[camInd].oksView[k] && oksView[k];
    223         }
    224         void createDstMotion(int step = 1, int camInd = 0)
    225         {
    226             dstObs[camInd].clear();
    227             for (int k = 0; k < rawObs[camInd].oksView.size(); k += step)
    228             {
    229                 if (!rawObs[camInd].oksView[k]) continue;
    230                 dstObs[camInd].poses.push_back(rawObs[camInd].poses[k]);
    231                 dstObs[camInd].point3D.push_back(rawObs[camInd].point3D[k]);
    232                 dstObs[camInd].point2D.push_back(rawObs[camInd].point2D[k]);
    233                 dstObs[camInd].oksView.push_back(true);
    234             }
    235             Vec6d idealFOV = cams[camInd].idealFOV(imaRows, imaCols);
    236             spdlog::info("Cam{:}: ValidViews={:}/{:}  ImaSize=({}, {})  idealFOV=({:.1f}, {:.1f}, {:.1f}, {:.1f}, {:.1f}, {:.1f}, {:.1f})", camInd, dstObs[camInd].point3D.size(), rawObs[camInd].point3D.size(), imaCols, imaRows,
    237                 idealFOV[0], idealFOV[1], idealFOV[2], idealFOV[3], idealFOV[4], idealFOV[5], cams[camInd].limitFOV());
    238         }
    239 
    240     public://Visualize
    241         void visMotion(int camInd0 = 0)
    242         {
    243             //1.CreateWidgets
    244             static int camInd; camInd = camInd0;
    245             viz::WCoordinateSystem worldSys(cmBoardMaxSide / 2);//basicSolid
    246             viz::WPlane worldGrd(Point3d(camPoses[0].tdata[0], camPoses[0].tdata[1], camPoses[0].tdata[2]), Vec3d(0, 1, 0), Vec3d(0, 0, 1), Size2d(200, 200));//basicSolid
    247             viz::WText worldTip(fmt::format("FunKey: w-s-d-a-8-5-6-4
    CamInd: {}
    {}{}", camInd, cams[camInd].print(imaRows, imaCols), camPoses[camInd].print()), Point(10, 480), 10);
    248             vector<cv::Affine3d> camAffs; for (int k = 0; k < ncamera; ++k) camAffs.push_back(cv::Affine3d(Matx33d(camPoses[k].mdata), Vec3d(camPoses[k].tdata)));
    249             viz::WTrajectory camTraj1(camAffs, viz::WTrajectory::FRAMES, cmSquareSide * 0.6);
    250             viz::WTrajectorySpheres camTraj2(camAffs, 100, cmSquareSide * 0.1);
    251             viz::WTrajectoryFrustums camTraj3(camAffs, Matx33d(cmBoardMinSide, 0, cmBoardMinSide * 0.85, 0, cmBoardMinSide, cmBoardMinSide * 0.85, 0, 0, 1), cmSquareSide * 0.4, viz::Color::yellow());
    252             static vector<cv::Affine3d> boardAffs; for (int k = 0; k < nboard; ++k) boardAffs.push_back(cv::Affine3d(Matx33d(boardPoses[cams[camInd].camModel][k].mdata), Vec3d(boardPoses[cams[camInd].camModel][k].tdata)));
    253             static viz::WTrajectorySpheres boardTraj(boardAffs, 1000, cmSquareSide * 0.1);
    254 
    255             //2.ShowWidgets
    256             worldSys.setRenderingProperty(viz::OPACITY, 0.1);
    257             worldGrd.setRenderingProperty(viz::OPACITY, 0.1);
    258             camTraj1.setRenderingProperty(viz::OPACITY, 0.5);
    259             camTraj2.setRenderingProperty(viz::OPACITY, 0.5);
    260             camTraj3.setRenderingProperty(viz::OPACITY, 0.5);
    261             boardTraj.setRenderingProperty(viz::OPACITY, 0.5);
    262             static viz::Viz3d viz3d(__FUNCTION__);
    263             viz3d.showWidget("worldSys", worldSys);
    264             viz3d.showWidget("worldGrd", worldGrd);
    265             viz3d.showWidget("worldTip", worldTip);
    266             viz3d.showWidget("camTraj1", camTraj1);
    267             viz3d.showWidget("camTraj2", camTraj2);
    268             viz3d.showWidget("camTraj3", camTraj3);
    269             viz3d.showWidget("boardTraj", boardTraj);
    270 
    271             //3.UpdateWidghts
    272             static BoardMotion& thisMotion = *this;
    273             viz3d.registerKeyboardCallback([](const viz::KeyboardEvent& keyboarEvent, void* pVizBorad)->void
    274                 {
    275                     if (keyboarEvent.action != viz::KeyboardEvent::KEY_DOWN) return;
    276 
    277                     //3.1 UpdateViewTraj
    278                     if (keyboarEvent.code == 'w') thisMotion.vecZ1[thisMotion.cams[camInd].camModel] += thisMotion.cmBoardMinSide * 0.1;
    279                     else if (keyboarEvent.code == 's') thisMotion.vecZ1[thisMotion.cams[camInd].camModel] -= thisMotion.cmBoardMinSide * 0.1;
    280                     else if (keyboarEvent.code == 'd') thisMotion.vecS1[thisMotion.cams[camInd].camModel] += thisMotion.cmBoardMinSide * 0.1;
    281                     else if (keyboarEvent.code == 'a') thisMotion.vecS1[thisMotion.cams[camInd].camModel] -= thisMotion.cmBoardMinSide * 0.1;
    282                     else if (keyboarEvent.code == '8') thisMotion.vecZ2[thisMotion.cams[camInd].camModel] += thisMotion.cmBoardMinSide * 0.1;
    283                     else if (keyboarEvent.code == '5') thisMotion.vecZ2[thisMotion.cams[camInd].camModel] -= thisMotion.cmBoardMinSide * 0.1;
    284                     else if (keyboarEvent.code == '6') thisMotion.vecS2[thisMotion.cams[camInd].camModel] += thisMotion.cmBoardMinSide * 0.1;
    285                     else if (keyboarEvent.code == '4') thisMotion.vecS2[thisMotion.cams[camInd].camModel] -= thisMotion.cmBoardMinSide * 0.1;
    286                     if (keyboarEvent.code == 'w' || keyboarEvent.code == 's' || keyboarEvent.code == 'd' || keyboarEvent.code == 'a' ||
    287                         keyboarEvent.code == '8' || keyboarEvent.code == '5' || keyboarEvent.code == '6' || keyboarEvent.code == '4')
    288                     {
    289                         thisMotion.resetBoardPose();
    290                         thisMotion.createRawMotion(1, camInd);
    291                         thisMotion.createDstMotion(1, camInd);
    292                         boardAffs.clear();
    293                         for (int k = 0; k < thisMotion.nboard; ++k) boardAffs.push_back(cv::Affine3d(Matx33d(thisMotion.boardPoses[thisMotion.cams[camInd].camModel][k].mdata), Vec3d(thisMotion.boardPoses[thisMotion.cams[camInd].camModel][k].tdata)));
    294                         boardTraj = viz::WTrajectorySpheres(boardAffs, 1000, 1);
    295                         boardTraj.setRenderingProperty(viz::OPACITY, 0.5);
    296                         viz3d.showWidget("boardTraj", boardTraj);//auto cover the widget with same id
    297                     }
    298 
    299                     //3.3 UpdateEachView
    300                     static int viewPos = 0;//absolute index
    301                     if (keyboarEvent.code == ' ')//Press space for next view
    302                     {
    303                         //1.GetNewView
    304                         int viewCnt = int(thisMotion.rawObs[camInd].point3D.size());
    305                         int viewInd = viewPos % viewCnt;
    306 
    307                         //2.FindMinMax
    308                         Point3f min3D(FLT_MAX, FLT_MAX, FLT_MAX), max3D(-FLT_MAX, -FLT_MAX, -FLT_MAX);
    309                         Point2f min2D(FLT_MAX, FLT_MAX), max2D(-FLT_MAX, -FLT_MAX);
    310                         for (int k = 0; k < thisMotion.rawObs[camInd].point3D[k].size(); ++k)//Find
    311                         {
    312                             Point3f pt3 = thisMotion.rawObs[camInd].point3D[viewInd][k];
    313                             Point2f pt2 = thisMotion.rawObs[camInd].point2D[viewInd][k];
    314                             if (pt3.x < min3D.x) min3D.x = pt3.x;
    315                             if (pt3.x > max3D.x) max3D.x = pt3.x;
    316                             if (pt3.y < min3D.y) min3D.y = pt3.y;
    317                             if (pt3.y > max3D.y) max3D.y = pt3.y;
    318                             if (pt2.x < min2D.x) min2D.x = pt2.x;
    319                             if (pt2.x > max2D.x) max2D.x = pt2.x;
    320                             if (pt2.y < min2D.y) min2D.y = pt2.y;
    321                             if (pt2.y > max2D.y) max2D.y = pt2.y;
    322                         }
    323 
    324                         //3.AddWidget
    325                         if (viewPos != 0)//exclude first start
    326                             for (int k = 0; k < thisMotion.rawObs[camInd].point3D[viewInd == 0 ? viewCnt - 1 : viewInd - 1].size(); ++k) viz3d.removeWidget("corner" + std::to_string(k));//if current view has no same number of corners as last view
    327                         viz3d.showWidget("viewSys", viz::WCoordinateSystem(thisMotion.cmBoardMinSide / 2), boardAffs[viewInd]);
    328                         for (int k = 0; k < thisMotion.rawObs[camInd].point3D[viewInd].size(); ++k) viz3d.showWidget(fmt::format("corner{}", k), viz::WSphere(thisMotion.rawObs[camInd].point3D[viewInd][k], 1, 10), boardAffs[viewInd]);//show all corners of current view
    329                         Mat_<uchar> ima2D(thisMotion.imaRows, thisMotion.imaCols, uchar(255));
    330                         for (int k = 0; k < thisMotion.rawObs[camInd].point2D[viewInd].size(); ++k)
    331                         {
    332                             Point2f pt = thisMotion.rawObs[camInd].point2D[viewInd][k];
    333                             if (pt.x < 10 || pt.x > thisMotion.imaCols - 10 || pt.y < 10 || pt.y > thisMotion.imaRows - 10) continue;
    334                             cv::circle(ima2D, Point(int(pt.x), int(pt.y)), 2, Scalar::all(0), 4, 8, 0);
    335                         }
    336                         viz3d.showWidget("ima2D", viz::WImage3D(ima2D, Size2d(thisMotion.cmBoardWidth, thisMotion.cmBoardHeight)));
    337                         viz3d.getWidget("ima2D").setRenderingProperty(viz::OPACITY, 0.8);
    338 
    339                         //4.ShowText
    340                         string viewTip;
    341                         viewTip += fmt::format("ViewCount: {}
    ", thisMotion.rawObs[camInd].point3D.size());
    342                         viewTip += fmt::format("ValidCount: {}
    ", thisMotion.dstObs[camInd].point3D.size());
    343                         viewTip += fmt::format("CurrentView: {}
    ", viewInd);
    344                         viewTip += fmt::format("CurrentValid: {}
    ", thisMotion.rawObs[camInd].oksView[viewInd]);
    345                         viewTip += fmt::format("Z1-Z2-S1-S2: {:.1f} {:.1f} {:.1f} {:.1f}
    ",
    346                             thisMotion.vecZ1[thisMotion.cams[camInd].camModel] / thisMotion.cmBoardMinSide,
    347                             thisMotion.vecZ2[thisMotion.cams[camInd].camModel] / thisMotion.cmBoardMinSide,
    348                             thisMotion.vecS1[thisMotion.cams[camInd].camModel] / thisMotion.cmBoardMinSide,
    349                             thisMotion.vecS2[thisMotion.cams[camInd].camModel] / thisMotion.cmBoardMinSide);
    350                         viewTip += fmt::format("MinMaxPoint3D: {:.1f} {:.1f} {:.1f} {:.1f}
    ", min3D.x, min3D.y, max3D.x, max3D.y);
    351                         viewTip += fmt::format("MinMaxPoint2D: {:.1f} {:.1f} {:.1f} {:.1f}
    ", min2D.x, min2D.y, max2D.x, max2D.y);
    352                         viewTip += fmt::format("CurrentBoardPose:
    {}", thisMotion.boardPoses[thisMotion.cams[camInd].camModel][viewInd].print());
    353                         viewTip += fmt::format("CurrentRelativePose:
    {}", thisMotion.rawObs[camInd].poses[viewInd].print());
    354                         viz3d.showWidget("viewTip", viz::WText(viewTip, Point(10, 10), 10));
    355                         ++viewPos;
    356                     }
    357                 }, 0);
    358             viz3d.spin();
    359         }
    360     };
    361 
    362 public://VisMotion
    363     static void VisRTCM(int argc, char** argv) { BoardMotion boardMotion; boardMotion.initMotion(5, VisionCM::RTCM, true, false); boardMotion.createRawMotion(1, 2); boardMotion.createDstMotion(1, 2); boardMotion.visMotion(2); }
    364     static void VisKBCM(int argc, char** argv) { BoardMotion boardMotion; boardMotion.initMotion(4, VisionCM::KBCM, true, false); boardMotion.createRawMotion(1, 4); boardMotion.createDstMotion(1, 4); boardMotion.visMotion(4); }
    365     static void VisRTEUCM(int argc, char** argv) { BoardMotion boardMotion; boardMotion.initMotion(4, VisionCM::RTEUCM, false, false); boardMotion.createRawMotion(1, 6); boardMotion.createDstMotion(1, 6); boardMotion.visMotion(6); }
    366 
    367 public://TestCalib
    368     static void CalibMono(int argc, char** argv)
    369     {
    370         for (int k = 0; k < 99; ++k)
    371         {
    372             //0.GenerateData
    373             int whichModel = k % 3;
    374             const int camInd = 0;
    375             BoardMotion boardMotion;
    376             if (whichModel == 0) boardMotion.initMotion(5, VisionCM::RTCM, true, false);
    377             else if (whichModel == 1) boardMotion.initMotion(4, VisionCM::KBCM, true, false);
    378             else if (whichModel == 2) boardMotion.initMotion(4, VisionCM::RTEUCM, false, false);
    379             boardMotion.createRawMotion(2, camInd);
    380             boardMotion.createDstMotion(1, camInd);
    381             Size imaSize(boardMotion.imaCols, boardMotion.imaRows);
    382             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 });
    383             Mat_<double> camD0(boardMotion.cams[camInd].nDist, 1, boardMotion.cams[camInd].D);
    384             Mat_<double> camAB0(boardMotion.cams[camInd].nModel, 1, boardMotion.cams[camInd].ab);
    385             Mat_<Vec3d> camRs0; for (int i = 0; i < boardMotion.dstObs[camInd].poses.size(); ++i) camRs0.push_back(Vec3d(boardMotion.dstObs[camInd].poses[i].vdata));
    386             Mat_<Vec3d> camTs0; for (int i = 0; i < boardMotion.dstObs[camInd].poses.size(); ++i) camRs0.push_back(Vec3d(boardMotion.dstObs[camInd].poses[i].tdata));
    387 
    388             //1.Calibration
    389             Mat_<double> camK1;
    390             Mat_<double> camD1;
    391             Mat_<double> camAB1;
    392             Mat_<Vec3d> camRs;
    393             Mat_<Vec3d> camTs;
    394             Mat_<double> errCamKD;
    395             Mat_<double> errCamRT;
    396             Mat_<double> errReprs;
    397             Mat_<int> validIds;
    398             double errRepr = 0;
    399             if (whichModel == 0) errRepr =
    400                 calibrateCamera(boardMotion.dstObs[camInd].point3D, boardMotion.dstObs[camInd].point2D, imaSize, camK1, camD1, camRs, camTs, errCamKD, errCamRT, errReprs, 0, TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 60, 1e-6));
    401             if (whichModel == 1) errRepr =
    402                 fisheye::calibrate(boardMotion.dstObs[camInd].point3D, boardMotion.dstObs[camInd].point2D, imaSize, camK1, camD1, camRs, camTs, fisheye::CALIB_RECOMPUTE_EXTRINSIC, TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 80, 1e-6));
    403             if (whichModel == 2) errRepr =
    404                 omnidir::calibrate(boardMotion.dstObs[camInd].point3D, boardMotion.dstObs[camInd].point2D, imaSize, camK1, camAB1, camD1, camRs, camTs, omnidir::CALIB_FIX_SKEW, TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, 1e-6), validIds);
    405 
    406             //2.AnalyzeError
    407             double infcamK0camK1 = cv::norm(camK0, camK1, NORM_INF);
    408             double infcamD0camD1 = cv::norm(camD0.reshape(1, 1), camD1.reshape(1, 1), NORM_INF);
    409             double infcamAB0camAB1 = 0; if (!camAB0.empty() && !camAB1.empty()) infcamAB0camAB1 = cv::norm(camAB0.row(0), camAB1, NORM_INF);
    410 
    411             //3.PrintError
    412             spdlog::info("LOOP{} {} calibrate done: {}", k, whichModel == 0 ? "RTCM" : whichModel == 1 ? "KBCM" : "RTEUCM", errRepr);
    413             if (infcamK0camK1 > 1E-6 || infcamD0camD1 > 1E-6 || infcamAB0camAB1 > 1E-6)
    414             {
    415                 cout << "infcamK0camK1: " << infcamK0camK1 << endl;
    416                 cout << "infcamD0camD1: " << infcamD0camD1 << endl;
    417                 cout << "infcamAB0camAB1: " << infcamAB0camAB1 << endl;
    418                 if (1)
    419                 {
    420                     cout << endl << "camK0: " << camK0.reshape(1, 1) << endl;
    421                     cout << endl << "camD0: " << camD0.reshape(1, 1) << endl;
    422                     if (!camAB0.empty()) cout << endl << "camAB0: " << camAB0.reshape(1, 1) << endl;
    423                     cout << endl << "camK1: " << camK1.reshape(1, 1) << endl;
    424                     cout << endl << "camD1: " << camD1.reshape(1, 1) << endl;
    425                     if (!camAB1.empty()) cout << endl << "camAB1: " << camAB1.reshape(1, 1) << endl;
    426                 }
    427                 cout << endl << "Press any key to continue" << endl; getchar();
    428             }
    429         }
    430     }
    431     static void CalibBino(int argc, char** argv)
    432     {
    433         for (int k = 0; k < 99; ++k)
    434         {
    435             //0.GenerateData
    436             int whichModel = k % 3;
    437             const int camInd1 = 1;
    438             const int camInd2 = 3;
    439             BoardMotion boardMotion;
    440             if (whichModel == 0) boardMotion.initMotion(5, VisionCM::RTCM, true, false);
    441             else if (whichModel == 1) boardMotion.initMotion(4, VisionCM::KBCM, true, false);
    442             else if (whichModel == 2) boardMotion.initMotion(4, VisionCM::RTEUCM, false, false);
    443             boardMotion.createRawMotion(2, camInd1);
    444             boardMotion.createRawMotion(2, camInd2);
    445             boardMotion.refineRawMotion(boardMotion.rawObs[camInd1].oksView, camInd2);
    446             boardMotion.refineRawMotion(boardMotion.rawObs[camInd2].oksView, camInd1);
    447             boardMotion.createDstMotion(1, camInd1);
    448             boardMotion.createDstMotion(1, camInd2);
    449             Size imaSize(boardMotion.imaCols, boardMotion.imaRows);
    450             Mat_<double> cam1K0({ 3, 3 }, { boardMotion.cams[camInd1].K[0], 0, boardMotion.cams[camInd1].K[2], 0, boardMotion.cams[camInd1].K[1], boardMotion.cams[camInd1].K[3], 0, 0, 1 });
    451             Mat_<double> cam2K0({ 3, 3 }, { boardMotion.cams[camInd2].K[0], 0, boardMotion.cams[camInd2].K[2], 0, boardMotion.cams[camInd2].K[1], boardMotion.cams[camInd2].K[3], 0, 0, 1 });
    452             Mat_<double> cam1D0(boardMotion.cams[camInd1].nDist, 1, boardMotion.cams[camInd1].D);
    453             Mat_<double> cam2D0(boardMotion.cams[camInd2].nDist, 1, boardMotion.cams[camInd2].D);
    454             Mat_<double> cam1AB0(boardMotion.cams[camInd1].nModel, 1, boardMotion.cams[camInd1].ab);
    455             Mat_<double> cam2AB0(boardMotion.cams[camInd2].nModel, 1, boardMotion.cams[camInd2].ab);
    456             Mat_<Vec3d> cam1Rs0; for (int i = 0; i < boardMotion.dstObs[camInd1].poses.size(); ++i) cam1Rs0.push_back(Vec3d(boardMotion.dstObs[camInd1].poses[i].vdata));
    457             Mat_<Vec3d> cam1Ts0; for (int i = 0; i < boardMotion.dstObs[camInd1].poses.size(); ++i) cam1Ts0.push_back(Vec3d(boardMotion.dstObs[camInd1].poses[i].tdata));
    458             Mat_<Vec3d> cam2Rs0; for (int i = 0; i < boardMotion.dstObs[camInd2].poses.size(); ++i) cam2Rs0.push_back(Vec3d(boardMotion.dstObs[camInd2].poses[i].vdata));
    459             Mat_<Vec3d> cam2Ts0; for (int i = 0; i < boardMotion.dstObs[camInd2].poses.size(); ++i) cam2Ts0.push_back(Vec3d(boardMotion.dstObs[camInd2].poses[i].tdata));
    460             Mat_<double> cam21R0 = Mat_<double>(3, 3, boardMotion.camPoses[camInd2].inv().mul(boardMotion.camPoses[camInd1]).mdata).clone();
    461             Mat_<double> cam21T0 = Mat_<double>(3, 1, boardMotion.camPoses[camInd2].inv().mul(boardMotion.camPoses[camInd1]).tdata).clone();
    462 
    463             //1.Calibration
    464             Mat_<double> cam1K1, cam2K1;
    465             Mat_<double> cam1D1, cam2D1;
    466             Mat_<double> cam1AB1, cam2AB1;
    467             Mat_<Vec3d> cam1Rs1, cam2Rs1;
    468             Mat_<Vec3d> cam1Ts1, cam1Ts2;
    469             Mat_<double> errCamKD1, errCamKD2;
    470             Mat_<double> errCamRT1, errCamRT2;
    471             Mat_<double> errReprs1, errReprs2;
    472             Mat_<double> cam21R1, cam21T1, cam21E1, cam21F1;
    473             Mat_<int> validIds;
    474             double errRepr = 0;
    475             if (whichModel == 0) errRepr = stereoCalibrate(boardMotion.rawObs[camInd1].point3D, boardMotion.rawObs[camInd1].point2D, boardMotion.rawObs[camInd2].point2D, cam1K1, cam1D1, cam2K1, cam2D1,
    476                 imaSize, cam21R1, cam21T1, cam21E1, cam21F1, 0, TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 60, 1e-6));
    477             if (whichModel == 1) errRepr = fisheye::stereoCalibrate(boardMotion.rawObs[camInd1].point3D, boardMotion.rawObs[camInd1].point2D, boardMotion.rawObs[camInd2].point2D, cam1K1, cam1D1, cam2K1, cam2D1,
    478                 imaSize, cam21R1, cam21T1, fisheye::CALIB_RECOMPUTE_EXTRINSIC, TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 80, 1e-6));
    479             if (whichModel == 2) errRepr = omnidir::stereoCalibrate(boardMotion.rawObs[camInd1].point3D, boardMotion.rawObs[camInd1].point2D, boardMotion.rawObs[camInd2].point2D, imaSize, imaSize,
    480                 cam1K1, cam1AB1, cam1D1, cam2K1, cam2AB1, cam2D1, cam21R1, cam21T1, cam1Rs1, cam1Ts1, omnidir::CALIB_FIX_SKEW, TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, 1e-6), validIds);
    481             if (cam21R1.total() == 3) cv::Rodrigues(cam21R1, cam21R1);
    482 
    483             //2.AnalyzeError
    484             double infcam1K0cam1K1 = cv::norm(cam1K0, cam1K1, NORM_INF);
    485             double infcam1D0cam1D1 = cv::norm(cam1D0.reshape(1, 1), cam1D1.reshape(1, 1), NORM_INF);
    486             double infcam2K0cam2K1 = cv::norm(cam2K0, cam2K1, NORM_INF);
    487             double infcam2D0cam2D1 = cv::norm(cam2D0.reshape(1, 1), cam2D1.reshape(1, 1), NORM_INF);
    488             double infcam1AB0cam1AB1 = 0; if (!cam1AB0.empty() && !cam1AB1.empty()) infcam1AB0cam1AB1 = cv::norm(cam1AB0.row(0), cam1AB1, NORM_INF);
    489             double infcam2AB0cam2AB1 = 0; if (!cam2AB0.empty() && !cam2AB1.empty()) infcam2AB0cam2AB1 = cv::norm(cam2AB0.row(0), cam2AB1, NORM_INF);
    490             double infcam21R0cam21R1 = cv::norm(cam21R0, cam21R1, NORM_INF);
    491             double infcam21T0cam21T1 = cv::norm(cam21T0, cam21T1, NORM_INF);
    492 
    493             //3.PrintError
    494             spdlog::info("LOOP{} {} calibrate done: {}", k, whichModel == 0 ? "RTCM" : whichModel == 1 ? "KBCM" : "RTEUCM", errRepr);
    495             if (infcam1K0cam1K1 > 1E-6 || infcam1D0cam1D1 > 1E-6 ||
    496                 infcam2K0cam2K1 > 1E-6 || infcam2D0cam2D1 > 1E-6 ||
    497                 infcam21R0cam21R1 > 1E-6 || infcam21T0cam21T1 > 1E-6)
    498             {
    499                 cout << "infcam1K0cam1K1: " << infcam1K0cam1K1 << endl;
    500                 cout << "infcam1D0cam1D1: " << infcam1D0cam1D1 << endl;
    501                 cout << "infcam2K0cam2K1: " << infcam2K0cam2K1 << endl;
    502                 cout << "infcam2D0cam2D1: " << infcam2D0cam2D1 << endl;
    503                 cout << "infcam1AB0cam1AB1: " << infcam1AB0cam1AB1 << endl;
    504                 cout << "infcam2AB0cam2AB1: " << infcam2AB0cam2AB1 << endl;
    505                 cout << "infcam21R0cam21R1: " << infcam21R0cam21R1 << endl;
    506                 cout << "infcam21T0cam21T1: " << infcam21T0cam21T1 << endl;
    507                 if (0)
    508                 {
    509                     cout << endl << "cam1K0: " << cam1K0.reshape(1, 1) << endl;
    510                     cout << endl << "cam1D0: " << cam1D0.reshape(1, 1) << endl;
    511                     if (!cam1AB0.empty()) cout << endl << "cam1AB0: " << cam1AB0.reshape(1, 1) << endl;
    512                     cout << endl << "cam1K1: " << cam1K1.reshape(1, 1) << endl;
    513                     cout << endl << "cam1D1: " << cam1D1.reshape(1, 1) << endl;
    514                     if (!cam1AB1.empty()) cout << endl << "cam1AB1: " << cam1AB1.reshape(1, 1) << endl;
    515                     cout << endl << "cam2K0: " << cam2K0.reshape(1, 1) << endl;
    516                     cout << endl << "cam2D0: " << cam2D0.reshape(1, 1) << endl;
    517                     if (!cam2AB0.empty()) cout << endl << "cam2AB0: " << cam2AB0.reshape(1, 1) << endl;
    518                     cout << endl << "cam2K1: " << cam2K1.reshape(1, 1) << endl;
    519                     cout << endl << "cam2D1: " << cam2D1.reshape(1, 1) << endl;
    520                     if (!cam2AB1.empty()) cout << endl << "cam2AB1: " << cam2AB1.reshape(1, 1) << endl;
    521                     cout << endl << "cam21R0: " << cam21R0.reshape(1, 1) << endl;
    522                     cout << endl << "cam21T0: " << cam21R0.reshape(1, 1) << endl;
    523                     cout << endl << "cam21R1: " << cam21R1.reshape(1, 1) << endl;
    524                     cout << endl << "cam21T1: " << cam21T1.reshape(1, 1) << endl;
    525                 }
    526                 cout << endl << "Press any key to continue" << endl; getchar();
    527             }
    528         }
    529     }
    530 };
    531 
    532 #if 0
    533 int main(int argc, char** argv) { Motion2D::CalibMono(argc, argv); return 0; }
    534 int main1(int argc, char** argv) { Motion2D::VisRTCM(argc, argv); return 0; }
    535 int main2(int argc, char** argv) { Motion2D::VisKBCM(argc, argv); return 0; }
    536 int main3(int argc, char** argv) { Motion2D::VisRTEUCM(argc, argv); return 0; }
    537 int main4(int argc, char** argv) { Motion2D::CalibMono(argc, argv); return 0; }
    538 int main5(int argc, char** argv) { Motion2D::CalibBino(argc, argv); return 0; }
    539 #endif
    540 
    541 #if 0
    542 static void TestMonoMotion2D()
    543 {
    544     for (int k = 0; k < 99; ++k)
    545     {
    546         //0.GenerateData
    547         int whichModel = k % 2;
    548         const int camInd = 0;
    549         Motion2D::BoardMotion boardMotion;
    550         if (whichModel == 0) boardMotion.initMotion(4, Motion2D::VisionCM::KBCM, true, false);
    551         if (whichModel == 1) boardMotion.initMotion(4, Motion2D::VisionCM::RTEUCM, false, false);
    552         boardMotion.createRawMotion(2, camInd);
    553         boardMotion.createDstMotion(1, camInd);
    554         Size imaSize(boardMotion.imaCols, boardMotion.imaRows);
    555         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 });
    556         Mat_<double> camD0(boardMotion.cams[camInd].nDist, 1, boardMotion.cams[camInd].D);
    557         Mat_<double> camAB0(boardMotion.cams[camInd].nModel, 1, boardMotion.cams[camInd].ab);
    558         Mat_<Vec3d> camRs0; for (int i = 0; i < boardMotion.dstObs[camInd].poses.size(); ++i) camRs0.push_back(Vec3d(boardMotion.dstObs[camInd].poses[i].vdata));
    559         Mat_<Vec3d> camTs0; for (int i = 0; i < boardMotion.dstObs[camInd].poses.size(); ++i) camRs0.push_back(Vec3d(boardMotion.dstObs[camInd].poses[i].tdata));
    560         Size boardSize(boardMotion.nHorCorner, boardMotion.nVerCorner);
    561         float cmSquareSide = boardMotion.cmSquareSide;
    562 
    563         //1.Calibration
    564         camodocal::Camera::ModelType modelType = whichModel == 0 ? camodocal::Camera::KANNALA_BRANDT : camodocal::Camera::MEI;
    565         camodocal::CameraCalibration calibration(modelType, "CamMotion2D", imaSize, boardSize, cmSquareSide); calibration.setVerbose(true);
    566         calibration.setVerbose(true);
    567         calibration.imagePoints() = boardMotion.dstObs[camInd].point2D;
    568         calibration.scenePoints() = boardMotion.dstObs[camInd].point3D;
    569         calibration.calibrate();
    570         Mat_<double> camK1(3, 3);
    571         Mat_<double> camD1(4, 1);
    572         Mat_<double> camAB1(1, 1);
    573         if (modelType == camodocal::Camera::KANNALA_BRANDT)
    574         {
    575             camodocal::EquidistantCameraPtr kbCam = std::dynamic_pointer_cast<camodocal::EquidistantCamera>(calibration.m_camera);
    576             camK1 << kbCam->mParameters.m_mu, 0, kbCam->mParameters.m_u0, 0, kbCam->mParameters.m_mv, kbCam->mParameters.m_v0, 0, 0, 1;
    577             camD1 << kbCam->mParameters.m_k2, kbCam->mParameters.m_k3, kbCam->mParameters.m_k4, kbCam->mParameters.m_k5;
    578         }
    579         if (modelType == camodocal::Camera::MEI)
    580         {
    581             camodocal::CataCameraPtr meiCam = std::dynamic_pointer_cast<camodocal::CataCamera>(calibration.m_camera);
    582             camK1 << meiCam->mParameters.m_gamma1, 0, meiCam->mParameters.m_u0, 0, meiCam->mParameters.m_gamma2, meiCam->mParameters.m_v0, 0, 0, 1;
    583             camD1 << meiCam->mParameters.m_k1, meiCam->mParameters.m_k2, meiCam->mParameters.m_p1, meiCam->mParameters.m_p2;
    584             camAB1 << meiCam->mParameters.m_xi, 1;
    585         }
    586 
    587         //2.AnalyzeError
    588         double infcamK0camK1 = cv::norm(camK0, camK1, NORM_INF);
    589         double infcamD0camD1 = cv::norm(camD0.reshape(1, 1), camD1.reshape(1, 1), NORM_INF);
    590         double infcamAB0camAB1 = 0; if (!camAB0.empty() && !camAB1.empty()) infcamAB0camAB1 = cv::norm(camAB0.row(0), camAB1, NORM_INF);
    591 
    592         //3.PrintError
    593         spdlog::info("LOOP{} {} calibrate done", k, whichModel == 0 ? "RTCM" : whichModel == 1 ? "KBCM" : "RTEUCM");
    594         if (infcamK0camK1 > 1E-6 || infcamD0camD1 > 1E-6 || infcamAB0camAB1 > 1E-6)
    595         {
    596             cout << "infcamK0camK1: " << infcamK0camK1 << endl;
    597             cout << "infcamD0camD1: " << infcamD0camD1 << endl;
    598             cout << "infcamAB0camAB1: " << infcamAB0camAB1 << endl;
    599             if (1)
    600             {
    601                 cout << endl << "camK0: " << camK0.reshape(1, 1) << endl;
    602                 cout << endl << "camD0: " << camD0.reshape(1, 1) << endl;
    603                 if (!camAB0.empty()) cout << endl << "camAB0: " << camAB0.reshape(1, 1) << endl;
    604                 cout << endl << "camK1: " << camK1.reshape(1, 1) << endl;
    605                 cout << endl << "camD1: " << camD1.reshape(1, 1) << endl;
    606                 if (!camAB1.empty()) cout << endl << "camAB1: " << camAB1.reshape(1, 1) << endl;
    607             }
    608             cout << endl << "Press any key to continue" << endl; getchar();
    609         }
    610     }
    611 }
    612 #endif
    View Code
  • 相关阅读:
    (原)ubuntu16在torch中使用caffe训练好的模型
    (原)Ubuntu16中卸载并重新安装google的Protocol Buffers
    (原)lua提示cannot load incompatible bytecode
    (原)ubuntu上安装nvidia及torch的nccl
    Ubuntu修改grub菜单引导选项和等待时间
    Servelet 简介
    JAVA JUC 线程池
    JAVA JUC synchronized 锁的理解
    JAVA JUC 读写锁
    JAVA JUC 线程按顺序执行
  • 原文地址:https://www.cnblogs.com/dzyBK/p/13934461.html
Copyright © 2011-2022 走看看