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)MUCM&&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：
(0)Tool2D：splitStr、globPaths、pathProps、serialPath、renamePath、comparePath、copyPath
(1)MotionBases：SolidPose、VisionCM、CamSolid、CamObservation
(2)BoardMotion：initMotion、resetBoardPose、resetCamKDAB、resetCamPose、createRawMotion、refineRawMotion、createDstMotion
(3)VisMotion：VisRTCM、VisKBCM、VisMUCM
(4)TestCalib：CalibMono、CalibBino
关于BoardMotion：
(1)initMotion：设置成像模型、所有相机内参、所有相机位姿(与世界不同每个相机固定不动)、世界的所有位姿(与相机不同世界仅一个且对每个相机而言该世界都按相同的路径运动)及世界目标。
(2)resetBoardPose：对每个成像模型而言，世界位姿都是经调试后得到的，本不提供更改世界位姿的接口，但为visMotion能进行调试可行的世界位姿，提供了此接口。
(3)resetCamKDAB：重设指定相机内参。
(4)resetCamPose：设置指定相机的位姿。
(5)createRawMotion：用指定相机观察世界的运动。
(6)refineRawMotion：决策指定相机中的无效观察。
(7)createDstMotion：提取指定相机的有效观察。
仿真来源：RTCM&MUCM来源OpenCV，KBCM&EUCM&DSCM来源basalt。
测试结果：可视化全正常、OpenCV透视标定通过、OpenCV等距标定不通过、OpenCV全向标定不算通过、CamOdoCal标定全通过。
遗留问题：全向模型尚未实现基于输入的视场角计算内参、全向模型仅实现第二模型参数为1的情况、双球模型所有功能都尚未实现。
1 #ifndef __Motion2D_h__ 2 #define __Motion2D_h__ 3 4 #include <cscv/ModelCamera.h> 5 #include <opencv2/core/utils/filesystem.hpp> 6 #include <opencv2/viz.hpp> 7 8 #ifndef TOOL2D 9 #define TOOL2D 10 struct Tool2D 11 { 12 static string cvarr2str(InputArray v) 13 { 14 Ptr<Formatted> fmtd = cv::format(v, Formatter::FMT_DEFAULT); 15 string dstm; fmtd->reset(); 16 for (const char* str = fmtd->next(); str; str = fmtd->next()) dstm += string(str); 17 return dstm; 18 } 19 static vector<string> splitStr(string str, char sep) 20 { 21 size_t ind0 = -1, ind = 0; 22 vector<string> rets; 23 while (1) 24 { 25 ++ind0; 26 ind = str.find_first_of(sep, ind0); 27 if (ind == string::npos) { if (ind0 != str.size()) rets.push_back(str.substr(ind0, str.size() - ind0)); break; } 28 else if (ind != ind0) { rets.push_back(str.substr(ind0, ind - ind0)); ind0 = ind; } 29 } 30 return rets; 31 } 32 static vector<string> globPaths(string dir, string pattern = "*", int type = 0/*0=file 1=dir 2=all*/, bool recursive = false) 33 { 34 //0.FormatInput 35 if (utils::fs::exists(dir) == false) return vector<string>(); 36 37 //1.RecurPaths 38 vector<string> srcPaths; 39 utils::fs::glob(dir, pattern, srcPaths, recursive, true); 40 for (size_t k = 0, pos = 0; k < srcPaths.size(); ++k) 41 for (size_t ind = 0; ; ++ind) 42 { 43 ind = srcPaths[k].find('\\', ind); 44 if (ind != string::npos) srcPaths[k].replace(ind, 1, "/"); 45 else break; 46 } 47 48 //2.ClassifyPaths 49 vector<string> dirPaths, filePaths; 50 for (size_t k = 0; k < srcPaths.size(); ++k) utils::fs::isDirectory(srcPaths[k]) ? dirPaths.push_back(srcPaths[k]) : filePaths.push_back(srcPaths[k]); 51 52 //3.SortPaths 53 if (dirPaths.size() > 1) stable_sort(dirPaths.begin(), dirPaths.end(), less<string>()); 54 if (dirPaths.size() > 1) stable_sort(dirPaths.begin(), dirPaths.end(), [](string str1, string str2)->bool {return str1.length() < str2.length(); }); 55 if (filePaths.size() > 1) stable_sort(filePaths.begin(), filePaths.end(), less<string>()); 56 if (filePaths.size() > 1) stable_sort(filePaths.begin(), filePaths.end(), [](string str1, string str2)->bool {return str1.length() < str2.length(); }); 57 58 //4.CollectPaths 59 if (type == 0) return filePaths; 60 if (type == 1) return dirPaths; 61 if (type == 2) for (size_t k = 0; k < dirPaths.size(); ++k) filePaths.push_back(dirPaths[k]); 62 return filePaths; 63 } 64 static vector<string> pathProps(string path/*0=dir 1=basename 2=extname 3=dir+basename 4=filename*/) 65 { 66 vector<string> props(5); 67 int ind1 = int(path.find_last_of('/')); 68 int ind2 = int(path.find_last_of('.')); 69 props[0] = path.substr(0, ind1); 70 props[1] = path.substr(ind1 + 1, ind2 - ind1 - 1); 71 props[2] = path.substr(ind2 + 1); 72 props[3] = path.substr(0, ind2); 73 props[4] = path.substr(ind1 + 1); 74 return props; 75 } 76 static string serialPath(string dir, string basename, string extname = ""/*empty for directory*/, int64 firstId = -1/*negtive for maximum*/) 77 { 78 if (firstId >= 0) 79 { 80 string fullPath; 81 do 82 { 83 fullPath = fmt::format("{}/{}{}{}", dir, basename, firstId, (extname.empty() ? extname : "." + extname)); 84 if (utils::fs::exists(fullPath) == false) break; 85 } while (++firstId); 86 return fullPath; 87 } 88 else 89 { 90 vector<string> paths = globPaths(dir, basename + "*", extname.empty() ? 1 : 0, false); 91 if (paths.empty()) return fmt::format("{}/{}{}{}", dir, basename, 0, (extname.empty() ? extname : "." + extname)); 92 if (extname.empty()) 93 { 94 int64 ind = paths[paths.size() - 1].find(basename) + basename.length(); 95 int64 id = atoll(paths[paths.size() - 1].substr(ind).c_str()) + 1; 96 return fmt::format("{}/{}{}", dir, basename, id); 97 } 98 else 99 { 100 int64 ind1 = paths[paths.size() - 1].find(basename) + basename.length(); 101 int64 ind2 = paths[paths.size() - 1].find_last_of("."); 102 int64 id = atoll(paths[paths.size() - 1].substr(ind1, ind2 - ind1).c_str()) + 1; 103 return fmt::format("{}/{}{}.{}", dir, basename, id, extname); 104 } 105 } 106 } 107 static string renamePath(string path, string value, int mode = 0/*0filename_1extname_2basename_3suffix_4prefix*/, bool doMove = true) 108 { 109 string path1; 110 int ind1 = int(path.find_last_of('/')); 111 int ind2 = int(path.find_last_of('.')); 112 if (mode == 0) path1 = fmt::format("{}/{}", path.substr(0, ind1), value); 113 else if (mode == 1) path1 = fmt::format("{}.{}", path.substr(0, ind2), value); 114 else if (mode == 2) path1 = fmt::format("{}/{}.{}", path.substr(0, ind1), value, path.substr(ind2 + 1)); 115 else if (mode == 3) path1 = fmt::format("{}{}.{}", path.substr(0, ind2), value, path.substr(ind2 + 1)); 116 else if (mode == 4) path1 = fmt::format("{}/{}{}", path.substr(0, ind1), value, path.substr(ind1 + 1)); 117 if (doMove) rename(path.c_str(), path1.c_str()); 118 } 119 static bool compareFile(string path1, string path2) 120 { 121 FILE* file1 = fopen(path1.c_str(), "rb"); if (file1 == 0) return false; 122 FILE* file2 = fopen(path2.c_str(), "rb"); if (file2 == 0) { fclose(file1); return false; } 123 if (fseek(file1, 0, SEEK_END) != 0) { fclose(file1); fclose(file2); return false; } 124 if (fseek(file2, 0, SEEK_END) != 0) { fclose(file1); fclose(file2); return false; } 125 long size1 = ftell(file1), size2 = ftell(file2); 126 if (size1 != size2) { fclose(file1); fclose(file2); return false; } 127 vector<char> data1(size1, 1), data2(size2, 2); 128 if (fseek(file1, 0, SEEK_SET) != 0) { fclose(file1); fclose(file2); return false; } 129 if (fseek(file2, 0, SEEK_SET) != 0) { fclose(file1); fclose(file2); return false; } 130 if (fread(data1.data(), 1, size1, file1) != size1) { fclose(file1); fclose(file2); return false; } 131 if (fread(data2.data(), 1, size2, file2) != size2) { fclose(file1); fclose(file2); return false; } 132 fclose(file1); fclose(file2); 133 return (data1 == data2); 134 } 135 static bool copyFile(string srcPath, string dstPath) 136 { 137 FILE* srcFile = fopen(srcPath.c_str(), "rb"); if (srcFile == 0) return false; 138 if (fseek(srcFile, 0, SEEK_END) != 0) { fclose(srcFile); return false; } 139 long size = ftell(srcFile); 140 if (fseek(srcFile, 0, SEEK_SET) != 0) { fclose(srcFile); return false; } 141 void* data = malloc(size); 142 if (fread(data, 1, size, srcFile) != size) { fclose(srcFile); return false; } 143 fclose(srcFile); 144 145 FILE* dstFile = fopen(dstPath.c_str(), "wb"); if (dstFile == 0) return false; 146 if (fwrite(data, 1, size, dstFile) != size) { fclose(dstFile); return false; } 147 fclose(dstFile); 148 free(data); 149 } 150 }; 151 #endif 152 153 class Motion2D 154 { 155 public://MotionBases 156 using SolidPose = ModelRotation::SolidPose; 157 using VisionCM = ModelCamera::VisionCM; 158 using CamSolid = ModelCamera::CamSolid; 159 using CamObservation = ModelCamera::CamObservation; 160 161 public://BoardMotion 162 class BoardMotion 163 { 164 public://Cameras 165 static const int ncamera = 9; 166 CamSolid cams[ncamera];//support multiple cameras 167 SolidPose camPoses[ncamera]; //all poses for each camera 168 CamObservation rawObs[ncamera];//all views for each camera 169 CamObservation dstObs[ncamera];//workable views for each camera 170 int imaRows; 171 int imaCols; 172 const float maxDist = 0.2f; 173 174 public://BoardProps 175 vector<Point3f> corner3D; 176 const int nVerCorner = 10; 177 const int nHorCorner = 11; 178 const float cmSquareSide = 10; 179 const float cmBoardWidth = (nHorCorner - 1) * cmSquareSide; 180 const float cmBoardHeight = (nVerCorner - 1) * cmSquareSide; 181 const float cmBoardMinSide = min(cmBoardWidth, cmBoardHeight); 182 const float cmBoardMaxSide = max(cmBoardWidth, cmBoardHeight); 183 184 public://BoardPoses 185 static const int nmodel = 5; 186 static const int ntrans = 10; 187 static const int nrotat = 9; 188 static const int nboard = ntrans * nrotat; 189 SolidPose boardPoses[nmodel][nboard]; 190 Vec<double, nmodel> vecZ1 = Vec<double, nmodel>(cmBoardMinSide * 1.2, cmBoardMinSide * 0.8, cmBoardMinSide * 0.4, 0, 0); 191 Vec<double, nmodel> vecZ2 = Vec<double, nmodel>(cmBoardMinSide * 1.6, cmBoardMinSide * 1.2, cmBoardMinSide * 0.8, 0, 0); 192 Vec<double, nmodel> vecS1 = Vec<double, nmodel>(cmBoardMinSide * 0.2, cmBoardMinSide * 0.4, cmBoardMinSide * 0.6, 0, 0); 193 Vec<double, nmodel> vecS2 = Vec<double, nmodel>(cmBoardMinSide * 0.4, cmBoardMinSide * 0.6, cmBoardMinSide * 1.0, 0, 0); 194 const Vec<int, nmodel> vecRows = Vec<int, nmodel>(800, 1200, 1600, 0, 0); 195 const Vec<int, nmodel> vecCols = Vec<int, nmodel>(1280, 1200, 1600, 0, 0); 196 const Vec<int, nmodel> vecFOV = Vec<int, nmodel>(100, 160, 240, 0, 0); 197 const Vec<int, nmodel> euler1 = Vec<int, nmodel>(15, 15, 10, 0, 0); 198 const Vec<int, nmodel> euler2 = Vec<int, nmodel>(30, 30, 20, 0, 0); 199 200 public://Process 201 void initMotion(short nDist0, short camModel0, short simPin0, bool rndSeed = false) 202 { 203 //1.CamModel 204 for (int k = 0; k < ncamera; ++k) 205 { 206 cams[k].nDist = nDist0; 207 cams[k].simPin = simPin0; 208 cams[k].camModel = camModel0; 209 if (cams[k].camModel == VisionCM::RTCM) { cams[k].nModel = 0; imaRows = vecRows[0]; imaCols = vecCols[0]; } 210 if (cams[k].camModel == VisionCM::KBCM) { cams[k].nModel = 0; imaRows = vecRows[1]; imaCols = vecCols[1]; } 211 if (cams[k].camModel == VisionCM::MUCM) { cams[k].nModel = 1; imaRows = vecRows[2]; imaCols = vecCols[2]; } 212 if (cams[k].camModel == VisionCM::EUCM) { cams[k].nModel = 2; imaRows = vecRows[2]; imaCols = vecCols[2]; } 213 if (cams[k].camModel == VisionCM::DSCM) { cams[k].nModel = 2; imaRows = vecRows[3]; imaCols = vecCols[3]; } 214 } 215 216 //2.CamParams 217 if (rndSeed) cv::setRNGSeed(clock()); 218 for (int k = 0; k < ncamera; ++k) resetCamKDAB(k); 219 220 //3.CameraPoses 221 for (int k = 0; k < ncamera; ++k) 222 { 223 if (k != 0) camPoses[k].setPos(Matx31d::randu(-cmSquareSide, cmSquareSide).val); camPoses[k].tdata[0] = k * cmSquareSide; 224 if (k != 0) camPoses[k].setRot(Matx31d::randu(-15 * deg2rad, 15 * deg2rad).val, 3); 225 } 226 227 //4.BoardData 228 for (int i = 0; i < nVerCorner; ++i) 229 for (int j = 0; j < nHorCorner; ++j) 230 corner3D.push_back(Point3f(j * cmSquareSide - cmBoardWidth / 2, i * cmSquareSide - cmBoardHeight / 2, 0)); 231 232 //5.BoardPoses 233 resetBoardPose(); 234 //for (int k = 0; k < ncamera; ++k) cout << endl << fmt::format("##########Camera{} information##########\n{}{}", k, cams[k].print(), camPoses[k].print()); 235 //for (int i = 0; i < nmodel; ++i) for (int j = 0; j < nboard; ++j) cout << endl << fmt::format("##########CamModel{}-BoardPose{}##########\n{}", j, i, boardPoses[i][j].print()); 236 } 237 void resetBoardPose(/*Only for visMotion to test approapriate board poses*/) 238 { 239 for (int whichModel = 0; whichModel < nmodel; ++whichModel) 240 { 241 double z1 = vecZ1[whichModel], z2 = vecZ2[whichModel]; 242 double s1 = vecS1[whichModel], s2 = vecS2[whichModel]; 243 double e1 = euler1[whichModel], e2 = euler2[whichModel]; 244 Vec3d boardTVec[ntrans] = 245 { 246 Vec3d(0, 0, z1), Vec3d(s1, 0, z1), Vec3d(-s1, 0, z1), Vec3d(0, s1, z1), Vec3d(0, -s1, z1), 247 Vec3d(0, 0, z2), Vec3d(s2, 0, z2), Vec3d(-s2, 0, z2), Vec3d(0, s2, z2), Vec3d(0, -s2, z2) 248 }; 249 Vec3d boardRVec[nrotat] = 250 { 251 Vec3d(0, 0, 0), 252 Vec3d(e1 * deg2rad, 0, 0), Vec3d(e2 * deg2rad, 0, 0), Vec3d(-e1 * deg2rad, 0, 0), Vec3d(-e2 * deg2rad, 0, 0), 253 Vec3d(0, e1 * deg2rad, 0), Vec3d(0, e2 * deg2rad, 0), Vec3d(0, -e1 * deg2rad, 0), Vec3d(0, -e2 * deg2rad, 0) 254 }; 255 for (int i = 0, k = 0; i < ntrans; ++i) 256 for (int j = 0; j < nrotat; ++j, ++k) 257 { 258 boardPoses[whichModel][k].setPos(boardTVec[i].val); 259 boardPoses[whichModel][k].setRot(boardRVec[j].val, 3); 260 } 261 } 262 } 263 264 void resetCamKDAB(int camInd = 0) 265 { 266 //1.ModelParams 267 if (cams[camInd].camModel == VisionCM::RTCM || cams[camInd].camModel == VisionCM::KBCM) { cams[camInd].ab[0] = 0; cams[camInd].ab[1] = 1; } 268 else if (cams[camInd].camModel == VisionCM::MUCM || cams[camInd].camModel == VisionCM::EUCM) 269 { 270 cams[camInd].ab[1] = cams[camInd].camModel == VisionCM::MUCM ? 1. : Vec2d::randu(0, 9.99)(0); 271 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); 272 } 273 else if (cams[camInd].camModel == VisionCM::DSCM) 274 { 275 cams[camInd].ab[1] = Vec2d::randu(-9.99, 9.99)(0); 276 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); 277 } 278 if (cams[camInd].simPin == 1) cams[camInd].ab[0] = cams[camInd].ab[0] / (1 + cams[camInd].ab[0]); 279 280 //2.CameraParams 281 cams[camInd].K[2] = imaCols * (0.5 + Vec2d::randu(-0.08, 0.08)(0)); 282 cams[camInd].K[3] = imaRows * (0.5 + Vec2d::randu(-0.08, 0.08)(0)); 283 if (cams[camInd].camModel == VisionCM::RTCM) 284 { 285 double halfFOV = vecFOV[0] / 2 * deg2rad; 286 cams[camInd].K[0] = cams[camInd].K[1] = (imaCols + imaRows) / tan(halfFOV) / 4; 287 cams[camInd].K[0] *= (1 + Vec2d::randu(-0.02, 0.02)(0)); 288 cams[camInd].K[1] *= (1 + Vec2d::randu(-0.02, 0.02)(0)); 289 } 290 else if (cams[camInd].camModel == VisionCM::KBCM) 291 { 292 double halfFOV = vecFOV[1] / 2 * deg2rad; 293 cams[camInd].K[0] = cams[camInd].K[1] = (imaCols + imaRows) / halfFOV / 4; 294 cams[camInd].K[0] *= (1 + Vec2d::randu(-0.01, 0.04)(0)); 295 cams[camInd].K[1] *= (1 + Vec2d::randu(-0.01, 0.04)(0)); 296 } 297 else if (cams[camInd].camModel == VisionCM::MUCM || cams[camInd].camModel == VisionCM::EUCM) 298 { 299 //if (cams[camInd].simPin == false); 300 double halfFOV = vecFOV[2] / 2 * deg2rad; 301 if (0) 302 { 303 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; 304 double b = cams[camInd].ab[1], b2 = b * b; 305 double e = tan(abs(halfFOV - pi / 2)), e2 = e * e; 306 307 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 }; 308 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 }; 309 Mat_<double> Fs; cv::solvePoly(coeffs, Fs); 310 cout << endl << Fs << endl; getchar(); 311 } 312 cams[camInd].K[0] = cams[camInd].K[1] = (cams[camInd].K[2] + cams[camInd].K[3]) * 0.66; 313 cams[camInd].K[0] *= (1 + Vec2d::randu(-0.01, 0.01)(0)); 314 cams[camInd].K[1] *= (1 + Vec2d::randu(-0.01, 0.01)(0)); 315 } 316 else if (cams[camInd].camModel == VisionCM::DSCM) {} 317 318 //3.DistortionParams 319 memset(cams[camInd].D, 0, sizeof(cams[camInd].D)); cv::randu(Mat_<double>(cams[camInd].nDist, 1, cams[camInd].D), -maxDist, maxDist); 320 stable_sort(cams[camInd].D, cams[camInd].D + cams[camInd].nDist, [](double a, double b)->bool { return abs(a) > abs(b); }); 321 } 322 void resetCamPose(double* rvec, double* tvec, int camInd = 0) 323 { 324 camPoses[camInd].setPos(tvec); 325 camPoses[camInd].setRot(rvec, 3); 326 } 327 328 void createRawMotion(int step = 1, int camInd = 0) 329 { 330 rawObs[camInd].clear(); 331 for (int k = 0; k < nboard; k += step) 332 { 333 //2.1 GetPose 334 SolidPose viewPose = camPoses[camInd].inv().mul(boardPoses[cams[camInd].camModel][k]); 335 336 //2.2 ProjectPoints 337 vector<Point2f> corner2D; 338 if (cams[camInd].camModel == VisionCM::RTCM) 339 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); 340 else if (cams[camInd].camModel == VisionCM::KBCM) 341 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)); 342 else if (cams[camInd].camModel == VisionCM::MUCM) 343 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)); 344 else if (cams[camInd].camModel == VisionCM::EUCM) {} 345 else if (cams[camInd].camModel == VisionCM::DSCM) {} 346 //2.3 CheckValid 347 bool ok = true; 348 for (int k = 0; k < corner2D.size(); ++k) 349 if (corner2D[k].x < 10 || corner2D[k].x > imaCols - 10 || corner2D[k].y < 10 || corner2D[k].y > imaRows - 10) ok = false; 350 //if (!ok) spdlog::warn("Invalid view and tvec{} rvec{}: {} {} {} {} {} {}", i, j, tvec[0], tvec[1], tvec[2], rvec[0], rvec[1], rvec[2]); 351 352 //2.4 AddGroup 353 rawObs[camInd].poses.push_back(viewPose); 354 rawObs[camInd].point3D.push_back(corner3D); 355 rawObs[camInd].point2D.push_back(corner2D); 356 rawObs[camInd].oksView.push_back(ok); 357 } 358 } 359 void refineRawMotion(vector<bool> oksView, int camInd = 0) 360 { 361 if (oksView.size() != rawObs[camInd].oksView.size()) { spdlog::critical("No same views: {}!={}", oksView.size(), rawObs[camInd].oksView.size()); assert(0); } 362 for (int k = 0; k < rawObs[camInd].oksView.size(); ++k) rawObs[camInd].oksView[k] = rawObs[camInd].oksView[k] && oksView[k]; 363 } 364 void createDstMotion(int step = 1, int camInd = 0) 365 { 366 dstObs[camInd].clear(); 367 for (int k = 0; k < rawObs[camInd].oksView.size(); k += step) 368 { 369 if (!rawObs[camInd].oksView[k]) continue; 370 dstObs[camInd].poses.push_back(rawObs[camInd].poses[k]); 371 dstObs[camInd].point3D.push_back(rawObs[camInd].point3D[k]); 372 dstObs[camInd].point2D.push_back(rawObs[camInd].point2D[k]); 373 dstObs[camInd].oksView.push_back(true); 374 } 375 Vec6d idealFOV = cams[camInd].idealFOV(imaRows, imaCols); 376 spdlog::info("Cam{:}: ValidViews={:}/{:} ImaSize=({}, {}) idealFOV=({:.1f}, {:.1f}, {:.1f}, {:.1f}, {:.1f}, {:.1f}, {:.1f})", camInd, dstObs[camInd].point3D.size(), rawObs[camInd].point3D.size(), imaCols, imaRows, 377 idealFOV[0], idealFOV[1], idealFOV[2], idealFOV[3], idealFOV[4], idealFOV[5], cams[camInd].limitFOV()); 378 } 379 380 public://Visualize 381 void visMotion(int camInd0 = 0) 382 { 383 //1.CreateWidgets 384 static int camInd; camInd = camInd0; 385 viz::WCoordinateSystem worldSys(cmBoardMaxSide / 2);//basicSolid 386 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 387 viz::WText worldTip(fmt::format("FunKey: w-s-d-a-8-5-6-4\nCamInd: {}\n{}{}", camInd, cams[camInd].print(imaRows, imaCols), camPoses[camInd].print()), Point(10, 480), 10); 388 vector<cv::Affine3d> camAffs; for (int k = 0; k < ncamera; ++k) camAffs.push_back(cv::Affine3d(Matx33d(camPoses[k].mdata), Vec3d(camPoses[k].tdata))); 389 viz::WTrajectory camTraj1(camAffs, viz::WTrajectory::FRAMES, cmSquareSide * 0.6); 390 viz::WTrajectorySpheres camTraj2(camAffs, 100, cmSquareSide * 0.1); 391 viz::WTrajectoryFrustums camTraj3(camAffs, Matx33d(cmBoardMinSide, 0, cmBoardMinSide * 0.85, 0, cmBoardMinSide, cmBoardMinSide * 0.85, 0, 0, 1), cmSquareSide * 0.4, viz::Color::yellow()); 392 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))); 393 static viz::WTrajectorySpheres boardTraj(boardAffs, 1000, cmSquareSide * 0.1); 394 395 //2.ShowWidgets 396 worldSys.setRenderingProperty(viz::OPACITY, 0.1); 397 worldGrd.setRenderingProperty(viz::OPACITY, 0.1); 398 camTraj1.setRenderingProperty(viz::OPACITY, 0.5); 399 camTraj2.setRenderingProperty(viz::OPACITY, 0.5); 400 camTraj3.setRenderingProperty(viz::OPACITY, 0.5); 401 boardTraj.setRenderingProperty(viz::OPACITY, 0.5); 402 static viz::Viz3d viz3d(__FUNCTION__); 403 viz3d.showWidget("worldSys", worldSys); 404 viz3d.showWidget("worldGrd", worldGrd); 405 viz3d.showWidget("worldTip", worldTip); 406 viz3d.showWidget("camTraj1", camTraj1); 407 viz3d.showWidget("camTraj2", camTraj2); 408 viz3d.showWidget("camTraj3", camTraj3); 409 viz3d.showWidget("boardTraj", boardTraj); 410 411 //3.UpdateWidghts 412 static BoardMotion& thisMotion = *this; 413 viz3d.registerKeyboardCallback([](const viz::KeyboardEvent& keyboarEvent, void* pVizBorad)->void 414 { 415 if (keyboarEvent.action != viz::KeyboardEvent::KEY_DOWN) return; 416 417 //3.1 UpdateViewTraj 418 if (keyboarEvent.code == 'w') thisMotion.vecZ1[thisMotion.cams[camInd].camModel] += thisMotion.cmBoardMinSide * 0.1; 419 else if (keyboarEvent.code == 's') thisMotion.vecZ1[thisMotion.cams[camInd].camModel] -= thisMotion.cmBoardMinSide * 0.1; 420 else if (keyboarEvent.code == 'd') thisMotion.vecS1[thisMotion.cams[camInd].camModel] += thisMotion.cmBoardMinSide * 0.1; 421 else if (keyboarEvent.code == 'a') thisMotion.vecS1[thisMotion.cams[camInd].camModel] -= thisMotion.cmBoardMinSide * 0.1; 422 else if (keyboarEvent.code == '8') thisMotion.vecZ2[thisMotion.cams[camInd].camModel] += thisMotion.cmBoardMinSide * 0.1; 423 else if (keyboarEvent.code == '5') thisMotion.vecZ2[thisMotion.cams[camInd].camModel] -= thisMotion.cmBoardMinSide * 0.1; 424 else if (keyboarEvent.code == '6') thisMotion.vecS2[thisMotion.cams[camInd].camModel] += thisMotion.cmBoardMinSide * 0.1; 425 else if (keyboarEvent.code == '4') thisMotion.vecS2[thisMotion.cams[camInd].camModel] -= thisMotion.cmBoardMinSide * 0.1; 426 if (keyboarEvent.code == 'w' || keyboarEvent.code == 's' || keyboarEvent.code == 'd' || keyboarEvent.code == 'a' || 427 keyboarEvent.code == '8' || keyboarEvent.code == '5' || keyboarEvent.code == '6' || keyboarEvent.code == '4') 428 { 429 thisMotion.resetBoardPose(); 430 thisMotion.createRawMotion(1, camInd); 431 thisMotion.createDstMotion(1, camInd); 432 boardAffs.clear(); 433 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))); 434 boardTraj = viz::WTrajectorySpheres(boardAffs, 1000, 1); 435 boardTraj.setRenderingProperty(viz::OPACITY, 0.5); 436 viz3d.showWidget("boardTraj", boardTraj);//auto cover the widget with same id 437 } 438 439 //3.3 UpdateEachView 440 static int viewPos = 0;//absolute index 441 if (keyboarEvent.code == ' ')//Press space for next view 442 { 443 //1.GetNewView 444 int viewCnt = int(thisMotion.rawObs[camInd].point3D.size()); 445 int viewInd = viewPos % viewCnt; 446 447 //2.FindMinMax 448 Point3f min3D(FLT_MAX, FLT_MAX, FLT_MAX), max3D(-FLT_MAX, -FLT_MAX, -FLT_MAX); 449 Point2f min2D(FLT_MAX, FLT_MAX), max2D(-FLT_MAX, -FLT_MAX); 450 for (int k = 0; k < thisMotion.rawObs[camInd].point3D[k].size(); ++k)//Find 451 { 452 Point3f pt3 = thisMotion.rawObs[camInd].point3D[viewInd][k]; 453 Point2f pt2 = thisMotion.rawObs[camInd].point2D[viewInd][k]; 454 if (pt3.x < min3D.x) min3D.x = pt3.x; 455 if (pt3.x > max3D.x) max3D.x = pt3.x; 456 if (pt3.y < min3D.y) min3D.y = pt3.y; 457 if (pt3.y > max3D.y) max3D.y = pt3.y; 458 if (pt2.x < min2D.x) min2D.x = pt2.x; 459 if (pt2.x > max2D.x) max2D.x = pt2.x; 460 if (pt2.y < min2D.y) min2D.y = pt2.y; 461 if (pt2.y > max2D.y) max2D.y = pt2.y; 462 } 463 464 //3.AddWidget 465 if (viewPos != 0)//exclude first start 466 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 467 viz3d.showWidget("viewSys", viz::WCoordinateSystem(thisMotion.cmBoardMinSide / 2), boardAffs[viewInd]); 468 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 469 Mat_<uchar> ima2D(thisMotion.imaRows, thisMotion.imaCols, uchar(255)); 470 for (int k = 0; k < thisMotion.rawObs[camInd].point2D[viewInd].size(); ++k) 471 { 472 Point2f pt = thisMotion.rawObs[camInd].point2D[viewInd][k]; 473 if (pt.x < 10 || pt.x > thisMotion.imaCols - 10 || pt.y < 10 || pt.y > thisMotion.imaRows - 10) continue; 474 cv::circle(ima2D, Point(int(pt.x), int(pt.y)), 2, Scalar::all(0), 4, 8, 0); 475 } 476 viz3d.showWidget("ima2D", viz::WImage3D(ima2D, Size2d(thisMotion.cmBoardWidth, thisMotion.cmBoardHeight))); 477 viz3d.getWidget("ima2D").setRenderingProperty(viz::OPACITY, 0.8); 478 479 //4.ShowText 480 string viewTip; 481 viewTip += fmt::format("ViewCount: {}\n", thisMotion.rawObs[camInd].point3D.size()); 482 viewTip += fmt::format("ValidCount: {}\n", thisMotion.dstObs[camInd].point3D.size()); 483 viewTip += fmt::format("CurrentView: {}\n", viewInd); 484 viewTip += fmt::format("CurrentValid: {}\n", thisMotion.rawObs[camInd].oksView[viewInd]); 485 viewTip += fmt::format("Z1-Z2-S1-S2: {:.1f} {:.1f} {:.1f} {:.1f}\n", 486 thisMotion.vecZ1[thisMotion.cams[camInd].camModel] / thisMotion.cmBoardMinSide, 487 thisMotion.vecZ2[thisMotion.cams[camInd].camModel] / thisMotion.cmBoardMinSide, 488 thisMotion.vecS1[thisMotion.cams[camInd].camModel] / thisMotion.cmBoardMinSide, 489 thisMotion.vecS2[thisMotion.cams[camInd].camModel] / thisMotion.cmBoardMinSide); 490 viewTip += fmt::format("MinMaxPoint3D: {:.1f} {:.1f} {:.1f} {:.1f}\n", min3D.x, min3D.y, max3D.x, max3D.y); 491 viewTip += fmt::format("MinMaxPoint2D: {:.1f} {:.1f} {:.1f} {:.1f}\n", min2D.x, min2D.y, max2D.x, max2D.y); 492 viewTip += fmt::format("CurrentBoardPose:\n{}", thisMotion.boardPoses[thisMotion.cams[camInd].camModel][viewInd].print()); 493 viewTip += fmt::format("CurrentRelativePose:\n{}", thisMotion.rawObs[camInd].poses[viewInd].print()); 494 viz3d.showWidget("viewTip", viz::WText(viewTip, Point(10, 10), 10)); 495 ++viewPos; 496 } 497 }, 0); 498 viz3d.spin(); 499 } 500 }; 501 502 public://VisMotion 503 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); } 504 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); } 505 static void VisMUCM(int argc, char** argv) { BoardMotion boardMotion; boardMotion.initMotion(4, VisionCM::MUCM, false, false); boardMotion.createRawMotion(1, 6); boardMotion.createDstMotion(1, 6); boardMotion.visMotion(6); } 506 507 public://TestCalib 508 static void CalibMono(int argc, char** argv) 509 { 510 for (int k = 0; k < 99; ++k) 511 { 512 //0.GenerateData 513 int whichModel = k % 3; 514 const int camInd = 0; 515 BoardMotion boardMotion; 516 if (whichModel == 0) boardMotion.initMotion(5, VisionCM::RTCM, true, false); 517 else if (whichModel == 1) boardMotion.initMotion(4, VisionCM::KBCM, true, false); 518 else if (whichModel == 2) boardMotion.initMotion(4, VisionCM::MUCM, false, false); 519 boardMotion.createRawMotion(2, camInd); 520 boardMotion.createDstMotion(1, camInd); 521 Size imaSize(boardMotion.imaCols, boardMotion.imaRows); 522 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 }); 523 Mat_<double> camD0(boardMotion.cams[camInd].nDist, 1, boardMotion.cams[camInd].D); 524 Mat_<double> camAB0(boardMotion.cams[camInd].nModel, 1, boardMotion.cams[camInd].ab); 525 Mat_<Vec3d> camRs0; for (int i = 0; i < boardMotion.dstObs[camInd].poses.size(); ++i) camRs0.push_back(Vec3d(boardMotion.dstObs[camInd].poses[i].vdata)); 526 Mat_<Vec3d> camTs0; for (int i = 0; i < boardMotion.dstObs[camInd].poses.size(); ++i) camTs0.push_back(Vec3d(boardMotion.dstObs[camInd].poses[i].tdata)); 527 528 //1.Calibration 529 Mat_<double> camK1; 530 Mat_<double> camD1; 531 Mat_<double> camAB1; 532 Mat_<Vec3d> camRs; 533 Mat_<Vec3d> camTs; 534 Mat_<double> errCamKD; 535 Mat_<double> errCamRT; 536 Mat_<double> errReprs; 537 Mat_<int> validIds; 538 double errRepr = 0; 539 if (whichModel == 0) errRepr = 540 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)); 541 if (whichModel == 1) errRepr = 542 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)); 543 if (whichModel == 2) errRepr = 544 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); 545 546 //2.AnalyzeError 547 double infcamK0camK1 = cv::norm(camK0, camK1, NORM_INF); 548 double infcamD0camD1 = cv::norm(camD0.reshape(1, 1), camD1.reshape(1, 1), NORM_INF); 549 double infcamAB0camAB1 = 0; if (!camAB0.empty() && !camAB1.empty()) infcamAB0camAB1 = cv::norm(camAB0.row(0), camAB1, NORM_INF); 550 551 //3.PrintError 552 spdlog::info("LOOP{} {} calibrate done: {}", k, whichModel == 0 ? "RTCM" : whichModel == 1 ? "KBCM" : "MUCM", errRepr); 553 if (infcamK0camK1 > 1E-6 || infcamD0camD1 > 1E-6 || infcamAB0camAB1 > 1E-6) 554 { 555 cout << "infcamK0camK1: " << infcamK0camK1 << endl; 556 cout << "infcamD0camD1: " << infcamD0camD1 << endl; 557 cout << "infcamAB0camAB1: " << infcamAB0camAB1 << endl; 558 if (1) 559 { 560 cout << endl << "camK0: " << camK0.reshape(1, 1) << endl; 561 cout << endl << "camD0: " << camD0.reshape(1, 1) << endl; 562 if (!camAB0.empty()) cout << endl << "camAB0: " << camAB0.reshape(1, 1) << endl; 563 cout << endl << "camK1: " << camK1.reshape(1, 1) << endl; 564 cout << endl << "camD1: " << camD1.reshape(1, 1) << endl; 565 if (!camAB1.empty()) cout << endl << "camAB1: " << camAB1.reshape(1, 1) << endl; 566 } 567 cout << endl << "Press any key to continue" << endl; getchar(); 568 } 569 } 570 } 571 static void CalibBino(int argc, char** argv) 572 { 573 for (int k = 0; k < 99; ++k) 574 { 575 //0.GenerateData 576 int whichModel = k % 3; 577 const int camInd1 = 1; 578 const int camInd2 = 3; 579 BoardMotion boardMotion; 580 if (whichModel == 0) boardMotion.initMotion(5, VisionCM::RTCM, true, false); 581 else if (whichModel == 1) boardMotion.initMotion(4, VisionCM::KBCM, true, false); 582 else if (whichModel == 2) boardMotion.initMotion(4, VisionCM::MUCM, false, false); 583 boardMotion.createRawMotion(2, camInd1); 584 boardMotion.createRawMotion(2, camInd2); 585 boardMotion.refineRawMotion(boardMotion.rawObs[camInd1].oksView, camInd2); 586 boardMotion.refineRawMotion(boardMotion.rawObs[camInd2].oksView, camInd1); 587 boardMotion.createDstMotion(1, camInd1); 588 boardMotion.createDstMotion(1, camInd2); 589 Size imaSize(boardMotion.imaCols, boardMotion.imaRows); 590 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 }); 591 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 }); 592 Mat_<double> cam1D0(boardMotion.cams[camInd1].nDist, 1, boardMotion.cams[camInd1].D); 593 Mat_<double> cam2D0(boardMotion.cams[camInd2].nDist, 1, boardMotion.cams[camInd2].D); 594 Mat_<double> cam1AB0(boardMotion.cams[camInd1].nModel, 1, boardMotion.cams[camInd1].ab); 595 Mat_<double> cam2AB0(boardMotion.cams[camInd2].nModel, 1, boardMotion.cams[camInd2].ab); 596 Mat_<Vec3d> cam1Rs0; for (int i = 0; i < boardMotion.dstObs[camInd1].poses.size(); ++i) cam1Rs0.push_back(Vec3d(boardMotion.dstObs[camInd1].poses[i].vdata)); 597 Mat_<Vec3d> cam1Ts0; for (int i = 0; i < boardMotion.dstObs[camInd1].poses.size(); ++i) cam1Ts0.push_back(Vec3d(boardMotion.dstObs[camInd1].poses[i].tdata)); 598 Mat_<Vec3d> cam2Rs0; for (int i = 0; i < boardMotion.dstObs[camInd2].poses.size(); ++i) cam2Rs0.push_back(Vec3d(boardMotion.dstObs[camInd2].poses[i].vdata)); 599 Mat_<Vec3d> cam2Ts0; for (int i = 0; i < boardMotion.dstObs[camInd2].poses.size(); ++i) cam2Ts0.push_back(Vec3d(boardMotion.dstObs[camInd2].poses[i].tdata)); 600 Mat_<double> cam21R0 = Mat_<double>(3, 3, boardMotion.camPoses[camInd2].inv().mul(boardMotion.camPoses[camInd1]).mdata).clone(); 601 Mat_<double> cam21T0 = Mat_<double>(3, 1, boardMotion.camPoses[camInd2].inv().mul(boardMotion.camPoses[camInd1]).tdata).clone(); 602 603 //1.Calibration 604 Mat_<double> cam1K1, cam2K1; 605 Mat_<double> cam1D1, cam2D1; 606 Mat_<double> cam1AB1, cam2AB1; 607 Mat_<Vec3d> cam1Rs1, cam2Rs1; 608 Mat_<Vec3d> cam1Ts1, cam1Ts2; 609 Mat_<double> errCamKD1, errCamKD2; 610 Mat_<double> errCamRT1, errCamRT2; 611 Mat_<double> errReprs1, errReprs2; 612 Mat_<double> cam21R1, cam21T1, cam21E1, cam21F1; 613 Mat_<int> validIds; 614 double errRepr = 0; 615 if (whichModel == 0) errRepr = stereoCalibrate(boardMotion.rawObs[camInd1].point3D, boardMotion.rawObs[camInd1].point2D, boardMotion.rawObs[camInd2].point2D, cam1K1, cam1D1, cam2K1, cam2D1, 616 imaSize, cam21R1, cam21T1, cam21E1, cam21F1, 0, TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 60, 1e-6)); 617 if (whichModel == 1) errRepr = fisheye::stereoCalibrate(boardMotion.rawObs[camInd1].point3D, boardMotion.rawObs[camInd1].point2D, boardMotion.rawObs[camInd2].point2D, cam1K1, cam1D1, cam2K1, cam2D1, 618 imaSize, cam21R1, cam21T1, fisheye::CALIB_RECOMPUTE_EXTRINSIC, TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 80, 1e-6)); 619 if (whichModel == 2) errRepr = omnidir::stereoCalibrate(boardMotion.rawObs[camInd1].point3D, boardMotion.rawObs[camInd1].point2D, boardMotion.rawObs[camInd2].point2D, imaSize, imaSize, 620 cam1K1, cam1AB1, cam1D1, cam2K1, cam2AB1, cam2D1, cam21R1, cam21T1, cam1Rs1, cam1Ts1, omnidir::CALIB_FIX_SKEW, TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, 1e-6), validIds); 621 if (cam21R1.total() == 3) cv::Rodrigues(cam21R1, cam21R1); 622 623 //2.AnalyzeError 624 double infcam1K0cam1K1 = cv::norm(cam1K0, cam1K1, NORM_INF); 625 double infcam1D0cam1D1 = cv::norm(cam1D0.reshape(1, 1), cam1D1.reshape(1, 1), NORM_INF); 626 double infcam2K0cam2K1 = cv::norm(cam2K0, cam2K1, NORM_INF); 627 double infcam2D0cam2D1 = cv::norm(cam2D0.reshape(1, 1), cam2D1.reshape(1, 1), NORM_INF); 628 double infcam1AB0cam1AB1 = 0; if (!cam1AB0.empty() && !cam1AB1.empty()) infcam1AB0cam1AB1 = cv::norm(cam1AB0.row(0), cam1AB1, NORM_INF); 629 double infcam2AB0cam2AB1 = 0; if (!cam2AB0.empty() && !cam2AB1.empty()) infcam2AB0cam2AB1 = cv::norm(cam2AB0.row(0), cam2AB1, NORM_INF); 630 double infcam21R0cam21R1 = cv::norm(cam21R0, cam21R1, NORM_INF); 631 double infcam21T0cam21T1 = cv::norm(cam21T0, cam21T1, NORM_INF); 632 633 //3.PrintError 634 spdlog::info("LOOP{} {} calibrate done: {}", k, whichModel == 0 ? "RTCM" : whichModel == 1 ? "KBCM" : "MUCM", errRepr); 635 if (infcam1K0cam1K1 > 1E-6 || infcam1D0cam1D1 > 1E-6 || 636 infcam2K0cam2K1 > 1E-6 || infcam2D0cam2D1 > 1E-6 || 637 infcam21R0cam21R1 > 1E-6 || infcam21T0cam21T1 > 1E-6) 638 { 639 cout << "infcam1K0cam1K1: " << infcam1K0cam1K1 << endl; 640 cout << "infcam1D0cam1D1: " << infcam1D0cam1D1 << endl; 641 cout << "infcam2K0cam2K1: " << infcam2K0cam2K1 << endl; 642 cout << "infcam2D0cam2D1: " << infcam2D0cam2D1 << endl; 643 cout << "infcam1AB0cam1AB1: " << infcam1AB0cam1AB1 << endl; 644 cout << "infcam2AB0cam2AB1: " << infcam2AB0cam2AB1 << endl; 645 cout << "infcam21R0cam21R1: " << infcam21R0cam21R1 << endl; 646 cout << "infcam21T0cam21T1: " << infcam21T0cam21T1 << endl; 647 if (0) 648 { 649 cout << endl << "cam1K0: " << cam1K0.reshape(1, 1) << endl; 650 cout << endl << "cam1D0: " << cam1D0.reshape(1, 1) << endl; 651 if (!cam1AB0.empty()) cout << endl << "cam1AB0: " << cam1AB0.reshape(1, 1) << endl; 652 cout << endl << "cam1K1: " << cam1K1.reshape(1, 1) << endl; 653 cout << endl << "cam1D1: " << cam1D1.reshape(1, 1) << endl; 654 if (!cam1AB1.empty()) cout << endl << "cam1AB1: " << cam1AB1.reshape(1, 1) << endl; 655 cout << endl << "cam2K0: " << cam2K0.reshape(1, 1) << endl; 656 cout << endl << "cam2D0: " << cam2D0.reshape(1, 1) << endl; 657 if (!cam2AB0.empty()) cout << endl << "cam2AB0: " << cam2AB0.reshape(1, 1) << endl; 658 cout << endl << "cam2K1: " << cam2K1.reshape(1, 1) << endl; 659 cout << endl << "cam2D1: " << cam2D1.reshape(1, 1) << endl; 660 if (!cam2AB1.empty()) cout << endl << "cam2AB1: " << cam2AB1.reshape(1, 1) << endl; 661 cout << endl << "cam21R0: " << cam21R0.reshape(1, 1) << endl; 662 cout << endl << "cam21T0: " << cam21R0.reshape(1, 1) << endl; 663 cout << endl << "cam21R1: " << cam21R1.reshape(1, 1) << endl; 664 cout << endl << "cam21T1: " << cam21T1.reshape(1, 1) << endl; 665 } 666 cout << endl << "Press any key to continue" << endl; getchar(); 667 } 668 } 669 } 670 }; 671 672 #ifdef Motion2DTest 673 int main(int argc, char** argv) { Motion2D::CalibMono(argc, argv); return 0; } 674 int main1(int argc, char** argv) { Motion2D::VisRTCM(argc, argv); return 0; } 675 int main2(int argc, char** argv) { Motion2D::VisKBCM(argc, argv); return 0; } 676 int main3(int argc, char** argv) { Motion2D::VisMUCM(argc, argv); return 0; } 677 int main4(int argc, char** argv) { Motion2D::CalibMono(argc, argv); return 0; } 678 int main5(int argc, char** argv) { Motion2D::CalibBino(argc, argv); return 0; } 679 #endif 680 681 #if 0 682 static void TestMonoMotion2D() 683 { 684 for (int k = 0; k < 99; ++k) 685 { 686 //0.GenerateData 687 string modelName = k % 2 == 0 ? "KBCM" : "MUCM"; 688 const int camInd = 0; 689 Motion2D::BoardMotion boardMotion; 690 if (modelName == "KBCM") boardMotion.initMotion(4, Motion2D::VisionCM::KBCM, true, false); 691 if (modelName == "MUCM") boardMotion.initMotion(4, Motion2D::VisionCM::MUCM, false, false); 692 boardMotion.createRawMotion(2, camInd); 693 boardMotion.createDstMotion(1, camInd); 694 Size imaSize(boardMotion.imaCols, boardMotion.imaRows); 695 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 }); 696 Mat_<double> camD0(boardMotion.cams[camInd].nDist, 1, boardMotion.cams[camInd].D); 697 Mat_<double> camAB0(boardMotion.cams[camInd].nModel, 1, boardMotion.cams[camInd].ab); 698 Mat_<Vec3d> camRs0; for (int i = 0; i < boardMotion.dstObs[camInd].poses.size(); ++i) camRs0.push_back(Vec3d(boardMotion.dstObs[camInd].poses[i].vdata)); 699 Mat_<Vec3d> camTs0; for (int i = 0; i < boardMotion.dstObs[camInd].poses.size(); ++i) camRs0.push_back(Vec3d(boardMotion.dstObs[camInd].poses[i].tdata)); 700 Size boardSize(boardMotion.nHorCorner, boardMotion.nVerCorner); 701 float cmSquareSide = boardMotion.cmSquareSide; 702 703 //1.Calibration 704 camodocal::Camera::ModelType modelIndex = modelName == "KBCM" ? camodocal::Camera::KANNALA_BRANDT : camodocal::Camera::MEI; 705 camodocal::CameraCalibration calibration(modelIndex, modelName, imaSize, boardSize, cmSquareSide); calibration.setVerbose(true); 706 calibration.setVerbose(true); 707 calibration.m_imagePoints = boardMotion.dstObs[camInd].point2D; 708 calibration.m_scenePoints = boardMotion.dstObs[camInd].point3D; 709 calibration.calibrate(); 710 Mat_<double> camK1(3, 3); 711 Mat_<double> camD1(4, 1); 712 Mat_<double> camAB1(1, 1, 0.); 713 vector<double> caliParams; calibration.m_camera->writeParameters(caliParams); 714 if (modelName == "MUCM") camK1 << caliParams[5], 0, caliParams[7], 0, caliParams[6], caliParams[8], 0, 0, 1; else camK1 << caliParams[4], 0, caliParams[6], 0, caliParams[5], caliParams[7], 0, 0, 1; 715 if (modelName == "MUCM") camD1 << caliParams[1], caliParams[2], caliParams[3], caliParams[4]; else camD1 << caliParams[0], caliParams[1], caliParams[2], caliParams[3]; 716 if (modelName == "MUCM") camAB1 << caliParams[0], 0; else camAB1 << 0; 717 718 //2.AnalyzeError 719 double infcamK0camK1 = cv::norm(camK0, camK1, NORM_INF); 720 double infcamD0camD1 = cv::norm(camD0.reshape(1, 1), camD1.reshape(1, 1), NORM_INF); 721 double infcamAB0camAB1 = 0; if (!camAB0.empty() && !camAB1.empty()) infcamAB0camAB1 = cv::norm(camAB0.row(0), camAB1, NORM_INF); 722 723 //3.PrintError 724 spdlog::info("LOOP{} {} calibrate done", k, modelName); 725 if (infcamK0camK1 > 1E-6 || infcamD0camD1 > 1E-6 || infcamAB0camAB1 > 1E-6) 726 { 727 cout << "infcamK0camK1: " << infcamK0camK1 << endl; 728 cout << "infcamD0camD1: " << infcamD0camD1 << endl; 729 cout << "infcamAB0camAB1: " << infcamAB0camAB1 << endl; 730 if (0) 731 { 732 cout << endl << "camK0: " << camK0.reshape(1, 1) << endl; 733 cout << endl << "camD0: " << camD0.reshape(1, 1) << endl; 734 if (!camAB0.empty()) cout << endl << "camAB0: " << camAB0.reshape(1, 1) << endl; 735 cout << endl << "camK1: " << camK1.reshape(1, 1) << endl; 736 cout << endl << "camD1: " << camD1.reshape(1, 1) << endl; 737 if (!camAB1.empty()) cout << endl << "camAB1: " << camAB1.reshape(1, 1) << endl; 738 } 739 cout << endl << "Press any key to continue" << endl; getchar(); 740 } 741 } 742 } 743 #endif 744 745 #endif
