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