diff --git a/cv-node/include/ftl/calibrate.hpp b/cv-node/include/ftl/calibrate.hpp index b9a95a939d6192ab62b23e1deb153f9dc2d49563..b0530bd4a18ad9734ba38e1f02a29c79b263676c 100644 --- a/cv-node/include/ftl/calibrate.hpp +++ b/cv-node/include/ftl/calibrate.hpp @@ -79,6 +79,7 @@ class Calibrate { bool _recalibrate(std::vector<std::vector<cv::Point2f>> *imagePoints, cv::Mat *cameraMatrix, cv::Mat *distCoeffs, cv::Size *imageSize); cv::Mat _nextImage(size_t cam); + bool _loadCalibration(); private: ftl::LocalSource *local_; diff --git a/cv-node/src/calibrate.cpp b/cv-node/src/calibrate.cpp index e7a5f35de21894c5a07cb2f3a8f713165805c064..86d9af2e704ff8dff26d8d5568aa55b13d44c5f3 100644 --- a/cv-node/src/calibrate.cpp +++ b/cv-node/src/calibrate.cpp @@ -188,7 +188,56 @@ Calibrate::Calibrate(ftl::LocalSource *s, const std::string &cal) : local_(s) { // TODO Load existing calibration if available... - calibrated_ = false; + calibrated_ = _loadCalibration(); + + if (calibrated_) { + LOG(INFO) << "Calibration loaded from file"; + } +} + +bool Calibrate::_loadCalibration() { + // Capture one frame to get size; + Mat l,r; + local_->get(l,r); + Size img_size = l.size(); + float scale = 1.0f; + + Rect roi1, roi2; + Mat Q; + // reading intrinsic parameters + FileStorage fs(FTL_CONFIG_ROOT "/intrinsics.yml", FileStorage::READ); + if(!fs.isOpened()) + { + LOG(WARNING) << "Calibration file not found"; + return false; + } + + Mat M1, D1, M2, D2; + fs["M1"] >> M1; + fs["D1"] >> D1; + fs["M2"] >> M2; + fs["D2"] >> D2; + + M1 *= scale; + M2 *= scale; + + fs.open(FTL_CONFIG_ROOT "/extrinsics.yml", FileStorage::READ); + if(!fs.isOpened()) + { + LOG(WARNING) << "Calibration file not found"; + return false; + } + + Mat R, T, R1, P1, R2, P2; + fs["R"] >> R; + fs["T"] >> T; + + stereoRectify( M1, D1, M2, D2, img_size, R, T, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY, -1, img_size, &roi1, &roi2 ); + + Mat map11, map12, map21, map22; + initUndistortRectifyMap(M1, D1, R1, P1, img_size, CV_16SC2, map1_[0], map2_[0]); + initUndistortRectifyMap(M2, D2, R2, P2, img_size, CV_16SC2, map1_[1], map2_[1]); + return true; } bool Calibrate::recalibrate() {