OpenCV-3D学习日志：搭建成像解算框架

1.本文要点说明
本文介绍如何基于OpenCV提供的标定函数搭建一套简易的标定框架，从而掌握OpenCV标定模块的核心API。
此框架的主要目的是通过保存中间结果为YML文件来解耦整个标定流程，使得各模块可以独立运行及任意组合运行，整个标定框架被拆分为如下几个模块：采集图像、提取角点、标定相机(分单双目)、矫正相机(分单双目)、矫正图像(分单双目)。
除采集图像和矫正图像外都存在多种实现方式，
(1)不同成像模型的属性差异不大，所以不专门设计一个基类而通过重载函数或异名函数或条件判断来实现不同的标定算法和矫正算法，
(2)不同标定板子的属性差异较大，所以需专门设计一个基类来提供公用的接口然后在每个子类为相应的板子定义专有属性。
考虑到OpenCV当前在标定方面的实际能力，本文
(1)在标定板子方面只阐述棋盘板子，
(2)在标定成像模型方面只阐述透视模型和等距模型。
对于其它标定板子和成像模型按相同设计方式加入即可。
测试数据来源opencv/samples/data和opencv_extra/testdata/cv/cameracalibration/fisheye/calib-3_stereo_from_JY，复制到../data/calib/cvrtcm/cam1|cam2|all和../data/calib/cvkbcm/cam1|cam2|all。
2.实验测试代码
依赖于OpenCV、Ceres和Spdlog，封装为如下类：
(0)CapCam：用户指定抓取保存目录CapDir，若为双目相机则在此目录内新建all、cam1及cam2目录，分别用于保存双目图、左相机图及右相机图。
(1)CalibAPP：用户指定配置所在文件ConfigYML，包括是否执行DetBD、CalibMono、RectMono、UndMono、CalibBino、RectBino、UndBino及它们的配置，其中输出路径根据输入路径自动生成，不能配置。
(2)DetBD/DetChBD/...：用户指定图像所在目录MonoDir，程序保存角点到MonoDir/rst/det.yml。
(3)CalibMono：用户指定角点所在文件DetYML，程序保存标定结果到同级目录下的calib.yml。
(4)RectMono：用户指定标定结果所在文件CalibYML，程序保存矫正结果到同级目录下的rect.yml。
(5)UndMono：用户指定矫正结果所在文件RectYML及畸变图像目录ImaDir，程序保存去畸变图像到畸变图像同级目录下的rst目录。
(6)CalibBino：CalibBino：用户指定标定目录CalibDir(包含cam1/rst/det.yml|calib.yml和cam2/rst/det.yml|calib.yml)，程序保存标定结果到该目录下的calib.yml。
(7)RectBino：RectBino：用户指定标定结果所在文件CalibYML，程序保存矫正结果到同级目录下的rect.yml。
(8)undBino：用户指定矫正结果所在文件RectYML及二合一的双目畸变图像目录ImaDir，程序保存去畸变图像到畸变图像同级目录下的rst目录。
以上类的resetIO、read、load及构造函数都将自动生成输出路径，不能指定。
   1 #ifndef __MPPCalibFrame_h__
   2 #define __MPPCalibFrame_h__
   3 
   4 #include <cscv/Motion2D.h>
   5 #include <opencv2/aruco.hpp>
   6 
   7 class MPPCalibFrame
   8 {
   9 public://CapCam
  10     class CapCam
  11     {
  12     public://1.CapArgs
  13         int capId = 0;
  14         int capCnt = INT_MAX;
  15         int capRows = 480;
  16         int capCols = 640;
  17         bool capSplitted = false;
  18 
  19     public://2.IOConfig
  20         string capIO = "../data/calib";
  21         CapCam(string capIO0 = "") { if (!capIO0.empty()) capIO = capIO0; }
  22 
  23     public://3.IOAction
  24         void write(FileStorage& fs, bool rdArgs = true, bool rdData = true)
  25         {
  26             if (rdArgs)
  27             {
  28                 fs.writeComment("######CapCam######");
  29                 fs << "capId" << capId;
  30                 fs << "capCnt" << capCnt;
  31                 fs << "capRows" << capRows;
  32                 fs << "capCols" << capCols;
  33                 fs << "capSplitted" << capSplitted;
  34                 fs << "capIO" << capIO;
  35             }
  36         }
  37 
  38         void read(FileStorage& fs, bool rdArgs = true, bool rdData = true)
  39         {
  40             if (rdArgs)
  41             {
  42                 fs["capId"] >> capId;
  43                 fs["capCnt"] >> capCnt;
  44                 fs["capRows"] >> capRows;
  45                 fs["capCols"] >> capCols;
  46                 fs["capSplitted"] >> capSplitted;
  47                 fs["capIO"] >> capIO;
  48             }
  49         }
  50 
  51     public://4.CoreTask
  52         void capture()
  53         {
  54             //0.Initialization
  55             utils::fs::createDirectories(capIO);
  56             if (capSplitted)
  57             {
  58                 utils::fs::createDirectories(capIO + "/all");
  59                 utils::fs::createDirectories(capIO + "/cam1");
  60                 utils::fs::createDirectories(capIO + "/cam2");
  61             }
  62             cv::namedWindow(__FUNCTION__, WINDOW_NORMAL);
  63 
  64             //1.SetCamera
  65             VideoCapture cap(capId);
  66             cap.set(CAP_PROP_FRAME_HEIGHT, capRows);
  67             cap.set(CAP_PROP_FRAME_WIDTH, capCols);
  68 
  69             //2.CaptureImages
  70             Mat ima;
  71             int cnt = 0;
  72             while (cap.read(ima))
  73             {
  74                 cv::imshow(__FUNCTION__, ima);
  75                 char c = cv::waitKey(30);
  76                 if (c == ' ')
  77                 {
  78                     if (capSplitted)
  79                     {
  80                         int64 ts = tsns;
  81                         cv::imwrite(fmt::format("{}/bino/{}.png", capIO, ts), ima);
  82                         cv::imwrite(fmt::format("{}/cam1/{}.png", capIO, ts), ima.colRange(0, ima.cols >> 1));
  83                         cv::imwrite(fmt::format("{}/cam2/{}.png", capIO, ts), ima.colRange(ima.cols >> 1, ima.cols));
  84                     }
  85                     else cv::imwrite(fmt::format("{}/{}.png", capIO, tsns), ima);
  86                     cv::displayStatusBar(__FUNCTION__, fmt::format("Captured image count: {}   Tips: press space to save and q/Q to exit", ++cnt), 1000);
  87                 }
  88                 if (c == 'q' || c == 'Q' || cnt >= capCnt) break;
  89             }cv::destroyWindow(__FUNCTION__);
  90         }
  91 
  92     public://9.Utils
  93         static void split2merge(string allDir, string cam1Dir, string cam2Dir, bool splitted = true, string pattern = "*", bool verbose = true)
  94         {
  95             if (splitted)
  96             {
  97                 //CreateDir
  98                 utils::fs::createDirectories(cam1Dir);
  99                 utils::fs::createDirectories(cam2Dir);
 100 
 101                 //SplitFrame
 102                 vector<string> allPaths = Tool2D::globPaths(allDir, pattern, 0, false);
 103                 for (uint k = 0; k < allPaths.size(); ++k)
 104                 {
 105                     Mat frame = imread(allPaths[k], -1);
 106                     Mat frame1 = frame.colRange(0, frame.cols >> 1);
 107                     Mat    frame2 = frame.colRange(frame.cols >> 1, frame.cols);
 108                     string cam1Path = fmt::format("{}/{}", cam1Dir, Tool2D::pathProps(allPaths[k])[4]);
 109                     string cam2Path = fmt::format("{}/{}", cam2Dir, Tool2D::pathProps(allPaths[k])[4]);
 110                     imwrite(cam1Path, frame1);
 111                     imwrite(cam2Path, frame1);
 112                     if (verbose) spdlog::info("Splitted {} as {} and {}", allPaths[k], cam1Path, cam2Path);
 113                 }
 114             }
 115             else
 116             {
 117                 //CreateDir
 118                 utils::fs::createDirectories(allDir);
 119 
 120                 //MergeFrame
 121                 vector<string> cam1Paths = Tool2D::globPaths(cam1Dir, pattern, 0, false);
 122                 vector<string> cam2Paths = Tool2D::globPaths(cam2Dir, pattern, 0, false);
 123                 if (cam1Paths.size() != cam2Paths.size()) { spdlog::critical("cam1Paths.size() != cam2Paths.size()"); return; }
 124                 for (size_t k = 0; k < cam1Paths.size(); ++k)
 125                 {
 126                     Mat frame1 = imread(cam1Paths[k], -1);
 127                     Mat frame2 = imread(cam2Paths[k], -1);
 128                     Mat frame; hconcat(frame1, frame2, frame);
 129                     string allPath = fmt::format("{}/{}", allDir, Tool2D::pathProps(cam1Paths[k])[4]);
 130                     imwrite(allPath, frame);
 131                     if (verbose) spdlog::info("Merged {} and {} to {}", cam1Paths[k], cam2Paths[k], allPath);
 132                 }
 133             }
 134         }
 135     };
 136 
 137 public://DetBD
 138     class DetBD
 139     {
 140     public://1.DetArgs
 141 
 142     public://2.IOConfig
 143         string detIn = "../data/calib/cam1";
 144         string detOut = fmt::format("{}/rst/det.yml", detIn);
 145         DetBD(string detIn0 = "") { resetIO(detIn0); }
 146         void resetIO(string detIn0 = "") { if (!detIn0.empty()) { detIn = detIn0; detOut = fmt::format("{}/rst/det.yml", detIn); } }
 147         int validViews() { int cnt = 0; for (int k = 0; k < oksView.size(); ++k) cnt += oksView[k]; return cnt; }
 148 
 149     public://3.IOAction
 150         void load(string inPath) { FileStorage fs(inPath, FileStorage::READ); read(fs); fs.release(); }
 151 
 152         virtual void write(FileStorage& fs, bool rdArgs = true, bool rdData = true) = 0;
 153 
 154         virtual void read(FileStorage& fs, bool rdArgs = true, bool rdData = true) = 0;
 155 
 156     public://4.CoreTask
 157         vector<short> oksView;
 158         vector<vector<Point2f>> point2D;
 159         vector<vector<Point3f>> point3D;
 160         virtual void detect() = 0;
 161     };
 162 
 163     class DetCHBD : public DetBD
 164     {
 165     public://1.DetArgs
 166         int chHorCross = 8;
 167         int chVerCross = 6;
 168         int chSqrSide = 50;
 169         int chDetFlag = 3;
 170         int chWinSize = 5;
 171         int chShowMS = 30;
 172         string chPattern = "*";
 173 
 174     public://2.IOConfig
 175         DetCHBD(string detIn0 = "") : DetBD(detIn0) { }
 176 
 177     public://3.IOAction
 178         void write(FileStorage& fs, bool rdArgs = true, bool rdData = true)
 179         {
 180             if (rdArgs)
 181             {
 182                 fs.writeComment("######DetectCHBD######");
 183                 fs << "chHorCross" << chHorCross;
 184                 fs << "chVerCross" << chVerCross;
 185                 fs << "chSqrSide" << chSqrSide;
 186                 fs << "chDetFlag" << chDetFlag; fs.writeComment("Refer to opencv docs for details", true);
 187                 fs << "chWinSize" << chWinSize;
 188                 fs << "chShowMS" << chShowMS;
 189                 fs << "chPattern" << chPattern;
 190                 fs << "detIn" << detIn;
 191             }
 192             if (rdData)
 193             {
 194                 fs << "oksView" << oksView;
 195                 fs << "point2D" << point2D;
 196                 fs << "point3D" << point3D;
 197             }
 198         }
 199 
 200         void read(FileStorage& fs, bool rdArgs = true, bool rdData = true)
 201         {
 202             if (rdArgs)
 203             {
 204                 fs["chHorCross"] >> chHorCross;
 205                 fs["chVerCross"] >> chVerCross;
 206                 fs["chSqrSide"] >> chSqrSide;
 207                 fs["chDetFlag"] >> chDetFlag;
 208                 fs["chWinSize"] >> chWinSize;
 209                 fs["chShowMS"] >> chShowMS;
 210                 fs["chPattern"] >> chPattern;
 211                 fs["detIn"] >> detIn; resetIO(detIn);
 212             }
 213             if (rdData)
 214             {
 215                 fs["oksView"] >> oksView;
 216                 fs["point2D"] >> point2D;
 217                 fs["point3D"] >> point3D;
 218             }
 219         }
 220 
 221     public://4.CoreTask
 222         void detect()
 223         {
 224             //0.Initialize
 225             if (!utils::fs::exists(detIn)) spdlog::critical("{}: {} not exist", __FUNCTION__, detIn); else spdlog::info("Current node: {}", __FUNCTION__);
 226             oksView.clear();
 227             point2D.clear();
 228             point3D.clear();
 229             if (chShowMS >= 0) cv::namedWindow(__FUNCTION__, WINDOW_NORMAL);
 230 
 231             //1.DetectPoint2D
 232             vector<string> filePaths = Tool2D::globPaths(detIn, chPattern, 0, false);
 233             for (size_t fileId = 0, cnt = 0; fileId < filePaths.size(); ++fileId)
 234             {
 235                 //1.1
 236                 Mat_<Vec3b> color = imread(filePaths[fileId]);
 237                 Mat_<uchar> gray; cvtColor(color, gray, COLOR_BGR2GRAY);
 238 
 239                 //1.2
 240                 vector<Point2f> corner2D;
 241                 bool found = findChessboardCorners(gray, Size(chHorCross, chVerCross), corner2D, chDetFlag);
 242                 if (found == false)
 243                 {
 244                     Mat_<uchar> tmp; resize(gray, tmp, gray.size() / 2);
 245                     found = findChessboardCorners(tmp, Size(chHorCross, chVerCross), corner2D, chDetFlag);
 246                     for (size_t k = 0; k < corner2D.size(); ++k) corner2D[k] *= 2;
 247                 }
 248 
 249                 //1.3
 250                 if (found) cornerSubPix(gray, corner2D, Size(chWinSize, chWinSize), Size(-1, -1), TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 30, 0.01));
 251                 if (found) drawChessboardCorners(color, Size(chHorCross, chVerCross), corner2D, found);
 252                 point2D.push_back(found ? corner2D : vector<Point2f>());
 253                 oksView.push_back(found);
 254                 string tips = fmt::format("{} detect {} and current vaild views {}", found ? "Could" : "Cannot", filePaths[fileId], cnt += found);
 255                 if (!found) spdlog::warn(tips);
 256                 if (chShowMS >= 0) cv::displayStatusBar(__FUNCTION__, tips, 1000);
 257 
 258                 //1.4
 259                 if (chShowMS < 0) continue;
 260                 cv::imshow(__FUNCTION__, color);
 261                 cv::waitKey(chShowMS);
 262             } if (chShowMS >= 0) cv::destroyWindow(__FUNCTION__);
 263 
 264             //2.CalcPoint3D
 265             vector<Point3f> corner3D;
 266             for (int i = 0; i < chVerCross; ++i)
 267                 for (int j = 0; j < chHorCross; ++j)
 268                     corner3D.push_back(Point3f(float(j * chSqrSide), float(i * chSqrSide), 0));
 269             for (int i = 0; i < oksView.size(); ++i) point3D.push_back(oksView[i] ? corner3D : vector<Point3f>());
 270 
 271             //3.SaveResult
 272             utils::fs::createDirectories(utils::fs::getParent(detOut));
 273             FileStorage fs(detOut, FileStorage::WRITE);
 274             this->write(fs);
 275             fs.release();
 276         }
 277 
 278     public://9.Utils
 279         static Mat_<uchar> createChessboard(int chSqrSide = 300, int chVerCross = 6, int chHorCross = 9)
 280         {
 281             Mat_<uchar> chessboard(chVerCross * chSqrSide, chHorCross * chSqrSide, (uchar)255);
 282             for (int i = 0; i < chVerCross; ++i)
 283             {
 284                 Mat_<uchar> _roi = chessboard.rowRange(i * chSqrSide, (i + 1) * chSqrSide);
 285                 for (int j = 0; j < chHorCross; ++j)
 286                 {
 287                     Mat_<uchar> roi = _roi.colRange(j * chSqrSide, (j + 1) * chSqrSide);
 288                     if (i == 0 && j == 0)  roi = 0;
 289                     if (i == 0 && j != 0)  roi = ~roi(0, -1);
 290                     if (i != 0 && j == 0)  roi = ~roi(-1, 0);
 291                     if (i != 0 && j != 0)  roi = ~roi(-1, 0);
 292                 }
 293             }
 294             return chessboard;
 295         }
 296     };
 297 
 298     class DetARBD : public DetBD
 299     {
 300     public:
 301         int arHorMarker = 8;
 302         int arVerMarker = 5;
 303         int arMarkerSide = 50;
 304         int arMarkerGap = 10;
 305         int arFirstId = 0;
 306         int arDicType = aruco::DICT_ARUCO_ORIGINAL;
 307         int arMinMarker = arHorMarker * arVerMarker / 2;
 308         int arShowMS = 30;
 309         string arPattern = "*";
 310 
 311     public://2.IOConfig
 312         DetARBD(string detIn0 = "") : DetBD(detIn0) { }
 313 
 314     public://3.IOAction
 315         void write(FileStorage& fs, bool rdArgs = true, bool rdData = true)
 316         {
 317             if (rdArgs)
 318             {
 319                 fs.writeComment("######DetectARBD######");
 320                 fs << "arHorMarker" << arHorMarker;
 321                 fs << "arVerMarker" << arVerMarker;
 322                 fs << "arMarkerSide" << arMarkerSide;
 323                 fs << "arMarkerGap" << arMarkerGap;
 324                 fs << "arFirstId" << arFirstId;
 325                 fs << "arDicType" << arDicType; fs.writeComment("Refer to opencv docs for details", true);
 326                 fs << "arMinMarker" << arMinMarker;
 327                 fs << "arShowMS" << arShowMS;
 328                 fs << "arPattern" << arPattern;
 329                 fs << "detIn" << detIn;
 330             }
 331             if (rdData)
 332             {
 333                 fs << "oksView" << oksView;
 334                 fs << "point2D" << point2D;
 335                 fs << "point3D" << point3D;
 336             }
 337         }
 338 
 339         void read(FileStorage& fs, bool rdArgs = true, bool rdData = true)
 340         {
 341             if (rdArgs)
 342             {
 343                 fs["arHorMarker"] >> arHorMarker;
 344                 fs["arVerMarker"] >> arVerMarker;
 345                 fs["arMarkerSide"] >> arMarkerSide;
 346                 fs["arMarkerGap"] >> arMarkerGap;
 347                 fs["arFirstId"] >> arFirstId;
 348                 fs["arDicType"] >> arDicType;
 349                 fs["arMinMarker"] >> arMinMarker;
 350                 fs["arShowMS"] >> arShowMS;
 351                 fs["arPattern"] >> arPattern;
 352                 fs["detIn"] >> detIn; resetIO(detIn);
 353             }
 354             if (rdData)
 355             {
 356                 fs["oksView"] >> oksView;
 357                 fs["point2D"] >> point2D;
 358                 fs["point3D"] >> point3D;
 359             }
 360         }
 361 
 362     public://4.CoreTask
 363         void detect()
 364         {
 365             //0.Initialize
 366             using namespace cv::aruco;
 367             if (!utils::fs::exists(detIn)) spdlog::critical("{}: {} not exist", __FUNCTION__, detIn); else spdlog::info("Current node: {}", __FUNCTION__);
 368             oksView.clear();
 369             point2D.clear();
 370             point3D.clear();
 371             if (arShowMS >= 0) cv::namedWindow(__FUNCTION__, WINDOW_NORMAL);
 372 
 373             //1.ConfigeBoard
 374             Ptr<Dictionary> dic = getPredefinedDictionary(arDicType);
 375             Ptr<GridBoard> board = GridBoard::create(arHorMarker, arVerMarker, float(arMarkerSide), float(arMarkerGap), dic, arFirstId);
 376             //{ Mat_<uchar> boardIma; board->draw(Size(1280, 1280 * 0.9), boardIma, 1280 * 0.1); imwrite("../data/markerboard.png", boardIma); }
 377             Ptr<DetectorParameters> detectParams = DetectorParameters::create();
 378 
 379             //2.DetectCorner2D
 380             vector<string> filePaths = Tool2D::globPaths(detIn, arPattern, 0, false);
 381             for (size_t fileId = 0, cnt = 0; fileId < filePaths.size(); ++fileId)
 382             {
 383                 //2.1
 384                 Mat_<Vec3b> color = imread(filePaths[fileId]);
 385                 Mat_<uchar> gray; cvtColor(color, gray, COLOR_BGR2GRAY);
 386 
 387                 //2.2
 388                 vector<vector<Point2f>> marker2D, marker2DReject; vector<int> markerId;
 389                 detectMarkers(gray, dic, marker2D, markerId, detectParams, marker2DReject);
 390                 bool found = markerId.size() >= arMinMarker;
 391 
 392                 //2.3
 393                 if (found)
 394                 {
 395                     refineDetectedMarkers(gray, (Ptr<Board>)board, marker2D, markerId, marker2DReject);
 396                     vector<Point2f> corner2D;
 397                     vector<Point3f> corner3D;
 398                     getBoardObjectAndImagePoints((Ptr<Board>)board, marker2D, markerId, corner3D, corner2D);
 399                     point2D.push_back(corner2D);
 400                     point3D.push_back(corner3D);
 401                     drawDetectedMarkers(color, marker2D, markerId);
 402                 }
 403                 else { point2D.push_back(vector<Point2f>()); point3D.push_back(vector<Point3f>()); }
 404                 oksView.push_back(found);
 405                 string tips = fmt::format("{} detect {} and current vaild views {}", found ? "Could" : "Cannot", filePaths[fileId], cnt += found);
 406                 if (!found) spdlog::warn(tips);
 407                 if (arShowMS >= 0) cv::displayStatusBar(__FUNCTION__, tips, 1000);
 408 
 409                 //2.5
 410                 if (arShowMS < 0) continue;
 411                 cv::imshow(__FUNCTION__, color);
 412                 cv::waitKey(arShowMS);
 413             } if (arShowMS >= 0) cv::destroyWindow(__FUNCTION__);
 414         }
 415 
 416     public:
 417         static Mat_<uchar> createArucoboard(int arFirstId = 0, int arVerMarker = 5, int arHorMarker = 8, int arMarkerSide = 300, int arMarkerGap = 60, int arDicType = aruco::DICT_ARUCO_ORIGINAL, vector<Mat_<uchar>>& markers = vector<Mat_<uchar>>())
 418         {
 419             //1.Init
 420             using namespace cv::aruco;
 421             int cnt = arVerMarker * arHorMarker;
 422             Ptr<Dictionary> dic = getPredefinedDictionary(arDicType);
 423             markers.clear();
 424 
 425             //2.Create
 426             Mat_<uchar> marker, _makerboard, markerboard;
 427             for (int i = 0, id = arFirstId, num = 0; i < arVerMarker; ++i)
 428             {
 429                 _makerboard.release();
 430                 for (int j = 0; j < arHorMarker; ++j, ++id, ++num)
 431                 {
 432                     //2.1 CreateOne
 433                     if (num < cnt)
 434                     {
 435                         drawMarker(dic, id, arMarkerSide, marker);
 436                         markers.push_back(marker.clone());
 437                     }
 438                     else marker = Mat_<uchar>(arMarkerSide, arMarkerSide, (uchar)0);
 439 
 440                     //2.2 ExtendMargin
 441                     copyMakeBorder(marker, marker, arMarkerGap >> 1, arMarkerGap >> 1, arMarkerGap >> 1, arMarkerGap >> 1, cv::BORDER_CONSTANT, Scalar(255));
 442 
 443                     //2.3 ConcatOneRow
 444                     if (_makerboard.empty()) _makerboard = marker.clone();
 445                     else hconcat(_makerboard, marker, _makerboard);
 446                 }
 447 
 448                 //2.4 ConcatOneColumn
 449                 if (markerboard.empty()) markerboard = _makerboard;
 450                 else vconcat(markerboard, _makerboard, markerboard);
 451             }
 452             return markerboard;
 453         }
 454     };
 455 
 456 public://MonoSuite
 457     class CalibMono
 458     {
 459     public://1.CalibArgs
 460         int calibRows = 480;
 461         int calibCols = 640;
 462         int calibBoard = CHBD;
 463         int calibModel = RTCM;
 464         int calibFlag = calibModel == KBCM ? fisheye::CALIB_RECOMPUTE_EXTRINSIC : 0;
 465         enum { CHBD, SBBD, CCBD, ARBD, APBD, RDBD };
 466         enum { RTCM, KBCM, MUCM, EUCM, DSCM };//From ModelCamera
 467         const string strCalibBoard = fmt::format("{}=chessboard {}=chessboardSB {}=circleboard {}=arucoboard {}=aprilboard {}=randomboard", CHBD, SBBD, CCBD, ARBD, APBD, RDBD);
 468         const string strCalibModel = fmt::format("{}=RTCV {}=KBCM {}=MUCM {}=EUCM {}=DSCM", RTCM, KBCM, MUCM, EUCM, DSCM);
 469 
 470     public://2.IOConfig
 471         string calibIn = "../data/calib/cam1/rst/det.yml";
 472         string calibOut = fmt::format("{}/calib.yml", utils::fs::getParent(calibIn));
 473         CalibMono(string calibIn0 = "") { resetIO(calibIn0); }
 474         void resetIO(string calibIn0 = "") { if (!calibIn0.empty()) { calibIn = calibIn0; calibOut = fmt::format("{}/calib.yml", utils::fs::getParent(calibIn)); } }
 475 
 476     public://3.IOAction
 477         void load(string inPath) { FileStorage fs(inPath, FileStorage::READ); read(fs); fs.release(); }
 478 
 479         void write(FileStorage& fs, bool rdArgs = true, bool rdData = true)
 480         {
 481             if (rdArgs)
 482             {
 483                 fs.writeComment("######CalibMono######");
 484                 fs << "calibRows" << calibRows;
 485                 fs << "calibCols" << calibCols;
 486                 fs << "calibBoard" << calibBoard; fs.writeComment(strCalibBoard, true);
 487                 fs << "calibModel" << calibModel; fs.writeComment(strCalibModel, true);
 488                 fs << "calibFlag" << calibFlag; fs.writeComment("Refer to opencv docs for details", true);
 489                 fs << "calibIn" << calibIn;
 490             }
 491             if (rdData)
 492             {
 493                 fs << "camK" << camK;
 494                 fs << "camD" << camD;
 495                 fs << "camAB" << camAB;
 496                 fs << "camRs" << camRs;
 497                 fs << "camTs" << camTs;
 498                 fs << "rmsView" << rmsView;
 499                 fs << "rmsMean" << rmsMean;
 500                 fs << "errPoints" << errPoints;
 501             }
 502         }
 503 
 504         void read(FileStorage& fs, bool rdArgs = true, bool rdData = true)
 505         {
 506             if (rdArgs)
 507             {
 508                 fs["calibRows"] >> calibRows;
 509                 fs["calibCols"] >> calibCols;
 510                 fs["calibBoard"] >> calibBoard;
 511                 fs["calibModel"] >> calibModel;
 512                 fs["calibFlag"] >> calibFlag;
 513                 fs["calibIn"] >> calibIn; resetIO(calibIn);
 514             }
 515             if (rdData)
 516             {
 517                 fs["camK"] >> camK;
 518                 fs["camD"] >> camD;
 519                 fs["camAB"] >> camAB;
 520                 fs["camRs"] >> camRs;
 521                 fs["camTs"] >> camTs;
 522                 fs["rmsView"] >> rmsView;
 523                 fs["rmsMean"] >> rmsMean;
 524                 fs["errPoints"] >> errPoints;
 525             }
 526         }
 527 
 528     public://4.CoreTask
 529         Mat_<double> camK;
 530         Mat_<double> camD;
 531         Mat_<double> camAB;
 532         Mat_<Vec3d> camRs;
 533         Mat_<Vec3d> camTs;
 534         Mat_<double> rmsKD;
 535         Mat_<double> rmsRT;
 536         Mat_<double> rmsView;
 537         double rmsMean = -1;
 538         vector<vector<Vec2d>> errPoints;
 539         void calibrate()
 540         {
 541             //0.LoadDetection
 542             if (!utils::fs::exists(calibIn)) spdlog::critical("{}: {} not exist", __FUNCTION__, calibIn); else spdlog::info("Current node: {}", __FUNCTION__);
 543             shared_ptr<DetBD> detBD;
 544             if (calibBoard == CHBD) detBD = make_shared<DetCHBD>();
 545             detBD->load(calibIn);
 546 
 547             //1.ExtractDetection
 548             vector<vector<Point2f>> point2D;
 549             vector<vector<Point3f>> point3D;
 550             for (size_t k = 0; k < detBD->oksView.size(); ++k)
 551             {
 552                 if (detBD->oksView[k]) point2D.push_back(detBD->point2D[k]);
 553                 if (detBD->oksView[k]) point3D.push_back(detBD->point3D[k]);
 554             }
 555 
 556             //2.CalibCamera
 557             if (calibModel == RTCM) rmsMean = calibrateCamera(point3D, point2D, Size(calibCols, calibRows), camK, camD, camRs, camTs, rmsKD, rmsRT, rmsView, calibFlag, TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, 1e-5));
 558             if (calibModel == KBCM) rmsMean = fisheye::calibrate(point3D, point2D, Size(calibCols, calibRows), camK, camD, camRs, camTs, calibFlag, TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 200, 1e-8));
 559 
 560             //3.SaveResults
 561             utils::fs::createDirectories(utils::fs::getParent(calibOut));
 562             FileStorage fs(calibOut, FileStorage::WRITE);
 563             this->write(fs);
 564             fs.release();
 565         }
 566     };
 567 
 568     class RectMono
 569     {
 570     public://1.RectArgs
 571         int rectRows = 480;
 572         int rectCols = 640;
 573         float rectFocusScale = 1.;
 574         Vec3d rectReviewEuler = Vec3d(0, 0, 0);
 575 
 576     public://2.IOConfig
 577         string rectIn = "../data/calib/cam1/rst/calib.yml";
 578         string rectOut = fmt::format("{}/rect.yml", utils::fs::getParent(rectIn));
 579         RectMono(string rectIn0 = "") { resetIO(rectIn0); }
 580         void resetIO(string rectIn0 = "") { if (!rectIn0.empty()) { rectIn = rectIn0; rectOut = fmt::format("{}/rect.yml", utils::fs::getParent(rectIn)); } }
 581 
 582     public://3.IOAction
 583         void load(string inPath) { FileStorage fs(inPath, FileStorage::READ); read(fs); fs.release(); }
 584 
 585         void write(FileStorage& fs, bool rdArgs = true, bool rdData = true)
 586         {
 587             if (rdArgs)
 588             {
 589                 fs.writeComment("######RectMono######");
 590                 fs << "rectRows" << rectRows;
 591                 fs << "rectCols" << rectCols;
 592                 fs << "rectFocusScale" << rectFocusScale;
 593                 fs << "rectReviewEuler" << rectReviewEuler;
 594                 fs << "rectIn" << rectIn;
 595             }
 596             if (rdData)
 597             {
 598                 fs << "camP" << camP;
 599                 fs << "validRect" << validRect;
 600                 fs << "mapx" << mapx;
 601                 fs << "mapy" << mapy;
 602             }
 603         }
 604 
 605         void read(FileStorage& fs, bool rdArgs = true, bool rdData = true)
 606         {
 607             if (rdArgs)
 608             {
 609                 fs["rectRows"] >> rectRows;
 610                 fs["rectCols"] >> rectCols;
 611                 fs["rectFocusScale"] >> rectFocusScale;
 612                 fs["rectReviewEuler"] >> rectReviewEuler;
 613                 fs["rectIn"] >> rectIn; resetIO(rectIn);
 614             }
 615             if (rdData)
 616             {
 617                 fs["camP"] >> camP;
 618                 fs["validRect"] >> validRect;
 619                 fs["mapx"] >> mapx;
 620                 fs["mapy"] >> mapy;
 621             }
 622         }
 623 
 624     public://4.CoreTask
 625         Mat_<double> camP;
 626         Rect validRect;
 627         Mat_<float> mapx;
 628         Mat_<float> mapy;
 629         void rectify()
 630         {
 631             //0.LoadCalibration
 632             if (!utils::fs::exists(rectIn)) spdlog::critical("{}: {} not exist", __FUNCTION__, rectIn); else spdlog::info("Current node: {}", __FUNCTION__);
 633             CalibMono calibMono;
 634             calibMono.load(rectIn);
 635 
 636             //1.ExtractCalibration
 637             int calibModel = calibMono.calibModel;
 638             Size calibSize(calibMono.calibCols, calibMono.calibRows);
 639             Mat_<double> camK = calibMono.camK;
 640             Mat_<double> camD = calibMono.camD;
 641             Mat_<double> camAB = calibMono.camAB;
 642 
 643             //2.RectCamera
 644             Mat_<double> reviewRMat = Mat_<double>::eye(3, 3);
 645             ceres::EulerAnglesToRotationMatrix(rectReviewEuler.val, 0, reviewRMat.ptr<double>());
 646             if (calibModel == CalibMono::RTCM)
 647             {
 648                 camP = getOptimalNewCameraMatrix(camK, camD, calibSize, rectFocusScale, Size(rectCols, rectRows), &validRect, false);
 649                 initUndistortRectifyMap(camK, camD, reviewRMat, camP, Size(rectCols, rectRows), CV_32F, mapx, mapy);
 650             }
 651             if (calibModel == CalibMono::KBCM)
 652             {
 653                 fisheye::estimateNewCameraMatrixForUndistortRectify(camK, camD, calibSize, Mat(), camP, rectFocusScale, Size(rectCols, rectRows), 1.0);
 654                 fisheye::initUndistortRectifyMap(camK, camD, reviewRMat, camP, Size(rectCols, rectRows), CV_32F, mapx, mapy);
 655             }
 656 
 657             //3.SaveResults
 658             utils::fs::createDirectories(utils::fs::getParent(rectOut));
 659             FileStorage fs(rectOut, FileStorage::WRITE);
 660             this->write(fs);
 661             fs.release();
 662         }
 663     };
 664 
 665     class UndMono
 666     {
 667     public://1.UndArgs
 668         int undShowMS = 30;
 669         string undPattern = "*";
 670         string undRectFile = "../data/calib/cam1/rst/rect.yml";
 671 
 672     public://2.IOConfig
 673         string undIn = "../data/calib";
 674         string undOut = fmt::format("{}/rst", undIn);
 675         UndMono(string undIn0 = "") { resetIO(undIn0); }
 676         void resetIO(string undIn0 = "") { if (!undIn0.empty()) { undIn = undIn0; undOut = fmt::format("{}/rst", undIn); } }
 677 
 678     public://3.IOAction
 679         void load(string inPath) { FileStorage fs(inPath, FileStorage::READ); read(fs); fs.release(); }
 680 
 681         void write(FileStorage& fs, bool rdArgs = true, bool rdData = true)
 682         {
 683             if (rdArgs)
 684             {
 685                 fs.writeComment("######UndMono######");
 686                 fs << "undShowMS" << undShowMS;
 687                 fs << "undPattern" << undPattern;
 688                 fs << "undRectFile" << undRectFile;
 689                 fs << "undIn" << undIn;
 690             }
 691         }
 692 
 693         void read(FileStorage& fs, bool rdArgs = true, bool rdData = true)
 694         {
 695             if (rdArgs)
 696             {
 697                 fs["undShowMS"] >> undShowMS;
 698                 fs["undPattern"] >> undPattern;
 699                 fs["undRectFile"] >> undRectFile;
 700                 fs["undIn"] >> undIn; resetIO(undIn);
 701             }
 702         }
 703 
 704     public://4.CoreTask
 705         void undistort()
 706         {
 707             //0.LoadRectification
 708             if (!utils::fs::exists(undRectFile)) spdlog::critical("{}: {} not exist", __FUNCTION__, undRectFile); else spdlog::info("Current node: {}", __FUNCTION__);
 709             RectMono rectMono;
 710             rectMono.load(undRectFile);
 711             utils::fs::createDirectories(undOut);
 712 
 713             //1.ExtractRectification
 714             Mat_<float> mapx = rectMono.mapx;
 715             Mat_<float> mapy = rectMono.mapy;
 716 
 717             //2.UndCamera
 718             if (undShowMS >= 0) cv::namedWindow(__FUNCTION__, WINDOW_NORMAL);
 719             vector<string> filePaths = Tool2D::globPaths(undIn, undPattern, 0, false);
 720             for (size_t fileId = 0; fileId < filePaths.size(); ++fileId)
 721             {
 722                 //2.1
 723                 Mat_<Vec3b> color = imread(filePaths[fileId], 1);
 724                 Mat_<uchar> gray; cvtColor(color, gray, COLOR_BGR2GRAY);
 725 
 726                 //2.2
 727                 Mat_<uchar> undIma;
 728                 remap(gray, undIma, mapx, mapy, INTER_LINEAR);
 729 
 730                 //2.3
 731                 Mat_<uchar> catIma;
 732                 if (gray.size() != undIma.size()) resize(gray, gray, undIma.size());
 733                 hconcat(gray, undIma, catIma);
 734                 cv::imwrite(fmt::format("{}/und_{}", undOut, Tool2D::pathProps(filePaths[fileId])[4]), catIma);
 735                 if (undShowMS >= 0) cv::displayStatusBar(__FUNCTION__, "Undistorting " + filePaths[fileId], 1000);
 736 
 737                 //2.4
 738                 if (undShowMS < 0) continue;
 739                 cv::imshow(__FUNCTION__, catIma);
 740                 cv::waitKey(undShowMS);
 741             } if (undShowMS >= 0) cv::destroyWindow(__FUNCTION__);
 742         }
 743     };
 744 
 745 public://BinoSuite
 746     class CalibBino
 747     {
 748     public://1.CalibArgs
 749         int binoCalibFlag = CALIB_FIX_INTRINSIC;//same as fisheye::CALIB_FIX_INTRINSIC;
 750 
 751     public://2.IOConfig
 752         string binoCalibIn = "../data/calib";
 753         string binoCalibOut = fmt::format("{}/calib.yml", binoCalibIn);
 754         string detPath1 = fmt::format("{}/cam1/rst/det.yml", binoCalibIn);
 755         string detPath2 = fmt::format("{}/cam2/rst/det.yml", binoCalibIn);
 756         string calibPath1 = fmt::format("{}/cam1/rst/calib.yml", binoCalibIn);
 757         string calibPath2 = fmt::format("{}/cam2/rst/calib.yml", binoCalibIn);
 758         CalibBino(string binoCalibIn0 = "") { resetIO(binoCalibIn0); }
 759         void resetIO(string binoCalibIn0 = "")
 760         {
 761             if (!binoCalibIn0.empty())
 762             {
 763                 binoCalibIn = binoCalibIn0;
 764                 binoCalibOut = fmt::format("{}/calib.yml", binoCalibIn);
 765                 detPath1 = fmt::format("{}/cam1/rst/det.yml", binoCalibIn);
 766                 detPath2 = fmt::format("{}/cam2/rst/det.yml", binoCalibIn);
 767                 calibPath1 = fmt::format("{}/cam1/rst/calib.yml", binoCalibIn);
 768                 calibPath2 = fmt::format("{}/cam2/rst/calib.yml", binoCalibIn);
 769             }
 770         }
 771 
 772     public://3.IOAction
 773         void load(string inPath)
 774         {
 775             FileStorage fs(inPath, FileStorage::READ);
 776             if (!detBD1 || !detBD2)
 777             {
 778                 CalibMono ca; ca.read(fs, true, false);
 779                 if (ca.calibBoard == CalibMono::CHBD) detBD1 = make_shared<DetCHBD>();
 780                 if (ca.calibBoard == CalibMono::CHBD) detBD2 = make_shared<DetCHBD>();
 781             }
 782             read(fs);
 783             fs.release();
 784         }
 785 
 786         void write(FileStorage& fs, bool rdArgs = true, bool rdData = true, bool rdMono = true)
 787         {
 788             if (rdArgs)
 789             {
 790                 if (rdMono)
 791                 {
 792                     detBD1->write(fs, true, false);
 793                     calibMono1.write(fs, true, false);
 794                 }
 795                 fs.writeComment("######CalibBino######");
 796                 fs << "binoCalibFlag" << binoCalibFlag; fs.writeComment("Refer to opencv docs for details", true);
 797                 fs << "binoCalibIn" << binoCalibIn;
 798             }
 799             if (rdData)
 800             {
 801                 if (rdMono)
 802                 {
 803                     fs << "camK1" << calibMono1.camK;
 804                     fs << "camK2" << calibMono2.camK;
 805                     fs << "camD1" << calibMono1.camD;
 806                     fs << "camD2" << calibMono2.camD;
 807                     fs << "camAB1" << calibMono1.camAB;
 808                     fs << "camAB2" << calibMono2.camAB;
 809                 }
 810                 fs << "cam21R" << cam21R;
 811                 fs << "cam21T" << cam21T;
 812                 fs << "cam21E" << cam21E;
 813                 fs << "cam21F" << cam21F;
 814                 fs << "rmsView" << rmsView;
 815                 fs << "rmsMean" << rmsMean;
 816                 fs << "errPoints" << errPoints;
 817                 if (rdMono)
 818                 {
 819                     fs << "oksView1" << detBD1->oksView;
 820                     fs << "oksView2" << detBD2->oksView;
 821                     fs << "point3D" << detBD1->point3D;
 822                     fs << "point2D1" << detBD1->point2D;
 823                     fs << "point2D2" << detBD2->point2D;
 824                 }
 825             }
 826         }
 827 
 828         void read(FileStorage& fs, bool rdArgs = true, bool rdData = true, bool rdMono = true)
 829         {
 830             if (rdArgs)
 831             {
 832                 if (rdMono)
 833                 {
 834                     detBD1->read(fs, true, false);
 835                     detBD2->read(fs, true, false);
 836                     calibMono1.read(fs, true, false);
 837                     calibMono2.read(fs, true, false);
 838                 }
 839                 fs["binoCalibFlag"] >> binoCalibFlag;
 840                 fs["binoCalibIn"] >> binoCalibIn; resetIO(binoCalibIn);
 841             }
 842             if (rdData)
 843             {
 844                 if (rdMono)
 845                 {
 846                     fs["camK1"] >> calibMono1.camK;
 847                     fs["camK2"] >> calibMono2.camK;
 848                     fs["camD1"] >> calibMono1.camD;
 849                     fs["camD2"] >> calibMono2.camD;
 850                     fs["camAB1"] >> calibMono1.camAB;
 851                     fs["camAB2"] >> calibMono2.camAB;
 852                 }
 853                 fs["cam21R"] >> cam21R;
 854                 fs["cam21T"] >> cam21T;
 855                 fs["cam21E"] >> cam21E;
 856                 fs["cam21F"] >> cam21F;
 857                 fs["rmsView"] >> rmsView;
 858                 fs["rmsMean"] >> rmsMean;
 859                 fs["errPoints"] >> errPoints;
 860                 if (rdMono)
 861                 {
 862                     fs["oksView1"] >> detBD1->oksView;
 863                     fs["oksView2"] >> detBD2->oksView;
 864                     fs["point3D"] >> detBD1->point3D;
 865                     fs["point2D1"] >> detBD1->point2D;
 866                     fs["point2D2"] >> detBD2->point2D;
 867                 }
 868             }
 869         }
 870 
 871     public:
 872         Mat_<double> cam21R;
 873         Mat_<double> cam21T;
 874         Mat_<double> cam21E;
 875         Mat_<double> cam21F;
 876         Mat_<double> rmsView;
 877         double rmsMean = -1;
 878         vector<vector<Vec4d>> errPoints;
 879         shared_ptr<DetBD> detBD1;
 880         shared_ptr<DetBD> detBD2;
 881         CalibMono calibMono1;
 882         CalibMono calibMono2;
 883         void calibrate()
 884         {
 885             //0.LoadMono
 886             if (!utils::fs::exists(binoCalibIn)) spdlog::critical("{}: {} not exist", __FUNCTION__, binoCalibIn);
 887             else if (!utils::fs::exists(detPath1)) spdlog::critical("{}: {} not exist", __FUNCTION__, detPath1);
 888             else if (!utils::fs::exists(detPath2)) spdlog::critical("{}: {} not exist", __FUNCTION__, detPath2);
 889             else if (!utils::fs::exists(calibPath1)) spdlog::critical("{}: {} not exist", __FUNCTION__, calibPath1);
 890             else if (!utils::fs::exists(calibPath2)) spdlog::critical("{}: {} not exist", __FUNCTION__, calibPath2);
 891             else spdlog::info("Current node: {}", __FUNCTION__);
 892             calibMono1.load(calibPath1);
 893             calibMono2.load(calibPath2);
 894             if (calibMono1.calibBoard == CalibMono::CHBD) detBD1 = make_shared<DetCHBD>();
 895             if (calibMono1.calibBoard == CalibMono::CHBD) detBD2 = make_shared<DetCHBD>();
 896             detBD1->load(detPath1);
 897             detBD2->load(detPath2);
 898 
 899             //1.ExtractMono
 900             vector<vector<Point3f>> point3D;
 901             vector<vector<Point2f>> point2D1;
 902             vector<vector<Point2f>> point2D2;
 903             for (size_t k = 0; k < detBD1->oksView.size(); ++k)
 904             {
 905                 if (detBD1->oksView[k] && detBD2->oksView[k]) point3D.push_back(detBD1->point3D[k]);
 906                 if (detBD1->oksView[k] && detBD2->oksView[k]) point2D1.push_back(detBD1->point2D[k]);
 907                 if (detBD1->oksView[k] && detBD2->oksView[k]) point2D2.push_back(detBD2->point2D[k]);
 908             }
 909             int calibModel = calibMono1.calibModel;
 910             Size calibSize(calibMono1.calibCols, calibMono1.calibRows);
 911             Mat_<double> camK1 = calibMono1.camK, camK2 = calibMono2.camK;
 912             Mat_<double> camD1 = calibMono1.camD, camD2 = calibMono2.camD;
 913             Mat_<double> camAB1 = calibMono1.camAB, camAB2 = calibMono2.camAB;
 914 
 915             //2.CalibCamera
 916             if (calibModel == CalibMono::RTCM) rmsMean = stereoCalibrate(point3D, point2D1, point2D2, camK1, camD1, camK2, camD2, calibSize, cam21R, cam21T, cam21E, cam21F, rmsView, binoCalibFlag, TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, 1e-5));
 917             if (calibModel == CalibMono::KBCM) rmsMean = fisheye::stereoCalibrate(point3D, point2D1, point2D2, camK1, camD1, camK2, camD2, calibSize, cam21R, cam21T, binoCalibFlag, TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 200, 1e-8));
 918             utils::fs::createDirectories(utils::fs::getParent(binoCalibOut));
 919             FileStorage fs(binoCalibOut, FileStorage::WRITE);
 920             this->write(fs);
 921             fs.release();
 922         }
 923     };
 924 
 925     class RectBino
 926     {
 927     public://1.RectArgs
 928         int binoRectRows = 480;
 929         int binoRectCols = 640;
 930         float binoRectFocusScale = 1.;
 931         Vec3d binoRectReviewEuler = Vec3d(0, 0, 0);
 932 
 933     public://2.IOConfig
 934         string binoRectIn = "../data/calib/calib.yml";
 935         string binoRectOut = fmt::format("{}/rect.yml", utils::fs::getParent(binoRectIn));
 936         RectBino(string binoRectIn0 = "") { resetIO(binoRectIn0); }
 937         void resetIO(string binoRectIn0 = "") { if (!binoRectIn0.empty()) { binoRectIn = binoRectIn0; binoRectOut = fmt::format("{}/rect.yml", utils::fs::getParent(binoRectIn)); } }
 938 
 939     public://3.IOAction
 940         void load(string inPath) { FileStorage fs(inPath, FileStorage::READ); read(fs); fs.release(); }
 941 
 942         void write(FileStorage& fs, bool rdArgs = true, bool rdData = true)
 943         {
 944             if (rdArgs)
 945             {
 946                 fs.writeComment("######RectBino######");
 947                 fs << "binoRectRows" << binoRectRows;
 948                 fs << "binoRectCols" << binoRectCols;
 949                 fs << "binoRectFocusScale" << binoRectFocusScale;
 950                 fs << "binoRectReviewEuler" << binoRectReviewEuler;
 951                 fs << "binoRectIn" << binoRectIn;
 952             }
 953             if (rdData)
 954             {
 955                 fs << "camP1" << camP1;
 956                 fs << "camP2" << camP2;
 957                 fs << "camR1" << camR1;
 958                 fs << "camR2" << camR2;
 959                 fs << "rectQ" << rectQ;
 960                 fs << "validRect1" << validRect1;
 961                 fs << "validRect2" << validRect2;
 962                 fs << "mapx1" << mapx1;
 963                 fs << "mapx2" << mapx2;
 964                 fs << "mapy1" << mapy1;
 965                 fs << "mapy2" << mapy2;
 966             }
 967         }
 968 
 969         void read(FileStorage& fs, bool rdArgs = true, bool rdData = true)
 970         {
 971             if (rdArgs)
 972             {
 973                 fs["binoRectRows"] >> binoRectRows;
 974                 fs["binoRectCols"] >> binoRectCols;
 975                 fs["binoRectFocusScale"] >> binoRectFocusScale;
 976                 fs["binoRectReviewEuler"] >> binoRectReviewEuler;
 977                 fs["binoRectIn"] >> binoRectIn; resetIO(binoRectIn);
 978             }
 979             if (rdData)
 980             {
 981                 fs["camP1"] >> camP1;
 982                 fs["camP2"] >> camP2;
 983                 fs["camR1"] >> camR1;
 984                 fs["camR2"] >> camR2;
 985                 fs["rectQ"] >> rectQ;
 986                 fs["validRect1"] >> validRect1;
 987                 fs["validRect2"] >> validRect2;
 988                 fs["mapx1"] >> mapx1;
 989                 fs["mapx2"] >> mapx2;
 990                 fs["mapy1"] >> mapy1;
 991                 fs["mapy2"] >> mapy2;
 992             }
 993         }
 994 
 995     public://4.CoreTask
 996         Mat_<double> camP1;
 997         Mat_<double> camP2;
 998         Mat_<double> camR1;
 999         Mat_<double> camR2;
1000         Mat_<double> rectQ;
1001         Rect validRect1;
1002         Rect validRect2;
1003         Mat_<float> mapx1;
1004         Mat_<float> mapx2;
1005         Mat_<float> mapy1;
1006         Mat_<float> mapy2;
1007         void rectify()
1008         {
1009             //0.LoadCalibration
1010             if (!utils::fs::exists(binoRectIn)) spdlog::critical("{}: {} not exist", __FUNCTION__, binoRectIn); else spdlog::info("Current node: {}", __FUNCTION__);
1011             CalibBino calibBino;
1012             calibBino.load(binoRectIn);
1013 
1014             //1.ExtractCalibration
1015             int calibModel = calibBino.calibMono1.calibModel;
1016             Size calibSize(calibBino.calibMono1.calibCols, calibBino.calibMono1.calibRows);
1017             Mat_<double> camK1 = calibBino.calibMono1.camK, camK2 = calibBino.calibMono2.camK;
1018             Mat_<double> camD1 = calibBino.calibMono1.camD, camD2 = calibBino.calibMono2.camD;
1019             Mat_<double> camAB1 = calibBino.calibMono1.camAB, camAB2 = calibBino.calibMono2.camAB;
1020             Mat_<double> cam21R = calibBino.cam21R, cam21T = calibBino.cam21T;
1021 
1022             //2.RectCamera
1023             Mat_<double> reviewRMat = Mat_<double>::eye(3, 3);
1024             ceres::EulerAnglesToRotationMatrix(binoRectReviewEuler.val, 0, reviewRMat.ptr<double>());
1025             if (calibModel == CalibMono::RTCM)
1026             {
1027                 stereoRectify(camK1, camD1, camK2, camD2, calibSize, cam21R, cam21T, camR1, camR2, camP1, camP2, rectQ, CALIB_ZERO_DISPARITY, binoRectFocusScale, Size(binoRectCols, binoRectRows), &validRect1, &validRect2);
1028                 initUndistortRectifyMap(camK1, camD1, reviewRMat * camR1, camP1, Size(binoRectCols, binoRectRows), CV_32F, mapx1, mapy1);
1029                 initUndistortRectifyMap(camK2, camD2, reviewRMat * camR2, camP2, Size(binoRectCols, binoRectRows), CV_32F, mapx2, mapy2);
1030             }
1031             if (calibModel == CalibMono::KBCM)
1032             {
1033                 fisheye::stereoRectify(camK1, camD1, camK2, camD2, calibSize, cam21R, cam21T, camR1, camR2, camP1, camP2, rectQ, CALIB_ZERO_DISPARITY, Size(binoRectCols, binoRectRows), binoRectFocusScale, 1.0);
1034                 fisheye::initUndistortRectifyMap(camK1, camD1, reviewRMat * camR1, camP1, Size(binoRectCols, binoRectRows), CV_32F, mapx1, mapy1);
1035                 fisheye::initUndistortRectifyMap(camK2, camD2, reviewRMat * camR2, camP2, Size(binoRectCols, binoRectRows), CV_32F, mapx2, mapy2);
1036             }
1037 
1038             //3.SaveResults
1039             utils::fs::createDirectories(utils::fs::getParent(binoRectOut));
1040             FileStorage fs(binoRectOut, FileStorage::WRITE);
1041             this->write(fs);
1042             fs.release();
1043         }
1044     };
1045 
1046     class UndBino
1047     {
1048     public://1.UndArgs
1049         int binoUndShowMS = 30;
1050         string binoUndPattern = "*";
1051         string binoUndRectFile = "../data/calib/rect.yml";
1052 
1053     public://2.IOConfig
1054         string binoUndIn = "../data/calib/all";
1055         string binoUndOut = fmt::format("{}/rst", binoUndIn);
1056         UndBino(string binoUndIn0 = "") { resetIO(binoUndIn0); }
1057         void resetIO(string binoUndIn0 = "") { if (!binoUndIn0.empty()) { binoUndIn = binoUndIn0; binoUndOut = fmt::format("{}/rst", binoUndIn); } }
1058 
1059     public://3.IOAction
1060         void load(string inPath) { FileStorage fs(inPath, FileStorage::READ); read(fs); fs.release(); }
1061 
1062         void write(FileStorage& fs, bool rdArgs = true, bool rdData = true)
1063         {
1064             if (rdArgs)
1065             {
1066                 fs.writeComment("######UndBino######");
1067                 fs << "binoUndShowMS" << binoUndShowMS;
1068                 fs << "binoUndPattern" << binoUndPattern;
1069                 fs << "binoUndRectFile" << binoUndRectFile;
1070                 fs << "binoUndIn" << binoUndIn;
1071             }
1072         }
1073 
1074         void read(FileStorage& fs, bool rdArgs = true, bool rdData = true)
1075         {
1076             if (rdArgs)
1077             {
1078                 fs["binoUndShowMS"] >> binoUndShowMS;
1079                 fs["binoUndPattern"] >> binoUndPattern;
1080                 fs["binoUndRectFile"] >> binoUndRectFile;
1081                 fs["binoUndIn"] >> binoUndIn; resetIO(binoUndIn);
1082             }
1083         }
1084 
1085     public://4.CoreTask
1086         void undistort()
1087         {
1088             //0.LoadRectification
1089             if (!utils::fs::exists(binoUndRectFile)) spdlog::critical("{}: {} not exist", __FUNCTION__, binoUndRectFile); else spdlog::info("Current node: {}", __FUNCTION__);
1090             RectBino rectBino;
1091             rectBino.load(binoUndRectFile);
1092             utils::fs::createDirectories(binoUndOut);
1093 
1094             //1.ExtractRectification
1095             Mat_<float> mapx1 = rectBino.mapx1;
1096             Mat_<float> mapx2 = rectBino.mapx2;
1097             Mat_<float> mapy1 = rectBino.mapy1;
1098             Mat_<float> mapy2 = rectBino.mapy2;
1099 
1100             //2.UndCamera
1101             if (binoUndShowMS >= 0) cv::namedWindow(__FUNCTION__, WINDOW_NORMAL);
1102             vector<string> filePaths = Tool2D::globPaths(binoUndIn, binoUndPattern, 0, false);
1103             for (size_t fileId = 0; fileId < filePaths.size(); ++fileId)
1104             {
1105                 //2.1
1106                 Mat_<Vec3b> color = imread(filePaths[fileId], 1);
1107                 Mat_<uchar> gray1, gray2;
1108                 cvtColor(color.colRange(0, color.cols >> 1), gray1, COLOR_BGR2GRAY);
1109                 cvtColor(color.colRange(color.cols >> 1, color.cols), gray2, COLOR_BGR2GRAY);
1110 
1111                 //2.2
1112                 Mat_<uchar> undIma1, undIma2;
1113                 remap(gray1, undIma1, mapx1, mapy1, INTER_LINEAR);
1114                 remap(gray2, undIma2, mapx2, mapy2, INTER_LINEAR);
1115 
1116                 //2.3
1117                 Mat_<uchar> catIma;
1118                 hconcat(undIma1, undIma2, catIma);
1119                 cv::imwrite(fmt::format("{}/und_{}", binoUndOut, Tool2D::pathProps(filePaths[fileId])[4]), catIma);
1120                 if (binoUndShowMS >= 0) cv::displayStatusBar(__FUNCTION__, "Undistorting " + filePaths[fileId], 1000);
1121 
1122                 //2.4
1123                 if (binoUndShowMS < 0) continue;
1124                 cv::imshow(__FUNCTION__, catIma);
1125                 cv::waitKey(binoUndShowMS);
1126             } if (binoUndShowMS >= 0) cv::destroyWindow(__FUNCTION__);
1127         }
1128     };
1129 
1130 public://CalibAPP
1131     class CalibAPP
1132     {
1133     public:
1134         bool doDet = true;
1135         bool doCalib = true;
1136         bool doRect = true;
1137         bool doUnd = true;
1138         bool doBinoCalib = true;
1139         bool doBinoRect = true;
1140         bool doBinoUnd = true;
1141         DetCHBD detCHBD;
1142         CalibMono calibMono;
1143         RectMono rectMono;
1144         UndMono undMono;
1145         CalibBino calibBino;
1146         RectBino rectBino;
1147         UndBino undBino;
1148         void writeArgs(string cfgPath)
1149         {
1150             FileStorage fs(cfgPath, FileStorage::WRITE);
1151             fs.writeComment("######CalibAPP######");
1152             fs << "doDet" << doDet;
1153             fs << "doCalib" << doCalib;
1154             fs << "doRect" << doRect;
1155             fs << "doUnd" << doUnd;
1156             fs << "doBinoCalib" << doBinoCalib;
1157             fs << "doBinoRect" << doBinoRect;
1158             fs << "doBinoUnd" << doBinoUnd;
1159             detCHBD.write(fs, true, false);
1160             calibMono.write(fs, true, false);
1161             rectMono.write(fs, true, false);
1162             undMono.write(fs, true, false);
1163             calibBino.write(fs, true, false, false);
1164             rectBino.write(fs, true, false);
1165             undBino.write(fs, true, false);
1166             fs.release();
1167         }
1168         void readArgs(string cfgPath)
1169         {
1170             FileStorage fs(cfgPath, FileStorage::READ);
1171             fs["doDet"] >> doDet;
1172             fs["doCalib"] >> doCalib;
1173             fs["doRect"] >> doRect;
1174             fs["doUnd"] >> doUnd;
1175             fs["doBinoCalib"] >> doBinoCalib;
1176             fs["doBinoRect"] >> doBinoRect;
1177             fs["doBinoUnd"] >> doBinoUnd;
1178             detCHBD.read(fs, true, false);
1179             calibMono.read(fs, true, false);
1180             rectMono.read(fs, true, false);
1181             undMono.read(fs, true, false);
1182             calibBino.read(fs, true, false, false);
1183             rectBino.read(fs, true, false);
1184             undBino.read(fs, true, false);
1185         }
1186         void calibCam(string cfgPath = "../data/calib/cfg.yml")
1187         {
1188             if (cfgPath.empty()) { spdlog::critical("Config file must be given"); return; }
1189             if (!utils::fs::exists(cfgPath)) { spdlog::critical("Config file not exist and created {}", cfgPath); writeArgs(cfgPath); return; }
1190             readArgs(cfgPath);
1191             if (doDet) detCHBD.detect();
1192             if (doCalib) calibMono.calibrate();
1193             if (doRect) rectMono.rectify();
1194             if (doUnd) undMono.undistort();
1195             if (doBinoCalib) calibBino.calibrate();
1196             if (doBinoRect) rectBino.rectify();
1197             if (doBinoUnd) undBino.undistort();
1198         }
1199         static void TestMe_CHBD_RTCM_KBCM(int argc, char** argv)
1200         {
1201             //0.AutoParams
1202             int calibRows[2] = { 480, 800 };
1203             int calibCols[2] = { 640, 1280 };
1204             int camModels[2] = { CalibMono::RTCM, CalibMono::KBCM };
1205             int calibFlag[2] = { 0, fisheye::CALIB_RECOMPUTE_EXTRINSIC };
1206             int rectRows[2] = { 480, 800 };
1207             int rectCols[2] = { 640, 1280 };
1208             string modDirs[2] = { "../data/calib/cvrtcm", "../data/calib/cvkbcm" };//from opencv/samples/data
1209             string camDirs[2] = { "cam1", "cam2" };//and opencv_extra/testdata/cv/cameracalibration/fisheye/calib-3_stereo_from_JY/
1210 
1211             //1.CalibCamera
1212             for (int i = 0; i < 2; ++i)
1213             {
1214                 for (int j = 0; j < 2; ++j)
1215                 {
1216                     //1.1 ConfigApp
1217                     CalibAPP calibApp;
1218                     calibApp.doBinoCalib = false;
1219                     calibApp.doBinoRect = false;
1220                     calibApp.doBinoUnd = false;
1221 
1222                     //1.2 ConfigBoard
1223                     calibApp.detCHBD.resetIO(fmt::format("{}/{}", modDirs[i], camDirs[j]));
1224 
1225                     //1.3 ConfigCalib
1226                     calibApp.calibMono.resetIO(calibApp.detCHBD.detOut);
1227                     calibApp.calibMono.calibRows = calibRows[i];
1228                     calibApp.calibMono.calibCols = calibCols[i];
1229                     calibApp.calibMono.calibModel = camModels[i];
1230                     calibApp.calibMono.calibFlag = calibFlag[i];
1231 
1232                     //1.4 ConfigRect
1233                     calibApp.rectMono.resetIO(calibApp.calibMono.calibOut);
1234                     calibApp.rectMono.rectRows = rectRows[i];
1235                     calibApp.rectMono.rectCols = rectCols[i];
1236                     calibApp.rectMono.rectReviewEuler = Vec3d(30, 0, 0);
1237 
1238                     //1.5 ConfigUnd
1239                     calibApp.undMono.resetIO(calibApp.detCHBD.detIn);
1240                     calibApp.undMono.undRectFile = calibApp.rectMono.rectOut;
1241 
1242                     //1.6 CalibCam
1243                     calibApp.writeArgs("../data/tmp.yml");//write config and then for read
1244                     calibApp.calibCam("../data/tmp.yml");
1245                 }
1246 
1247                 //2.1 ConfigApp
1248                 CalibAPP calibApp;
1249                 calibApp.doDet = false;
1250                 calibApp.doCalib = false;
1251                 calibApp.doRect = false;
1252                 calibApp.doUnd = false;
1253 
1254                 //2.2 ConfigCalib
1255                 calibApp.calibBino.resetIO(modDirs[i]);
1256                 calibApp.calibBino.binoCalibFlag = CALIB_FIX_INTRINSIC;
1257 
1258                 //2.3 ConfigRect
1259                 calibApp.rectBino.resetIO(calibApp.calibBino.binoCalibOut);
1260                 calibApp.rectBino.binoRectRows = rectRows[i];
1261                 calibApp.rectBino.binoRectCols = rectCols[i];
1262                 calibApp.rectBino.binoRectReviewEuler = Vec3d(30, 0, 0);
1263 
1264                 //2.4 ConfigUnd
1265                 calibApp.undBino.resetIO(modDirs[i] + "/all");
1266                 calibApp.undBino.binoUndRectFile = calibApp.rectBino.binoRectOut;
1267 
1268                 //2.5 CalibCam
1269                 calibApp.writeArgs("../data/tmp.yml");//write config and then for read
1270                 calibApp.calibCam("../data/tmp.yml");
1271             }
1272         }
1273     };
1274 };
1275 
1276 #ifdef MPPCalibFrameTest
1277 int main(int argc, char** argv) { MPPCalibFrame::CalibAPP::TestMe_CHBD_RTCM_KBCM(argc, argv); return 0; }
1278 int main1(int argc, char** argv) { MPPCalibFrame::CapCam ccam; ccam.capRows = 480; ccam.capCols = 640; ccam.capSplitted = true; return 0; }
1279 int main2(int argc, char** argv) { MPPCalibFrame::CalibAPP::TestMe_CHBD_RTCM_KBCM(argc, argv); return 0; }
1280 #endif
1281 
1282 #endif
View Code
相关阅读:
由类型名得到该类型
 TypeName of CA1505 is AvoidUnmaintainableCode, not AvoidUnmantainableCode.
多核
 SSRS throws "The operation has timed out."
XmlRootAttribute与XmlTypeAttribute
littleendian and bigendian
Assembly Exp
如何在Ubuntu 11.10下成功安装Java [转载]
*uck up ~,纯发泄
 [转载]读书相关，，
原文地址：https://www.cnblogs.com/dzyBK/p/14497566.html