From 33466dfd39bfea728c01521a0c8228c2be30b3c5 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Tue, 12 Mar 2019 17:14:48 +0200
Subject: [PATCH] Load calibration from file
---
cv-node/include/ftl/calibrate.hpp | 1 +
cv-node/src/calibrate.cpp | 51 ++++++++++++++++++++++++++++++-
2 files changed, 51 insertions(+), 1 deletion(-)
diff --git a/cv-node/include/ftl/calibrate.hpp b/cv-node/include/ftl/calibrate.hpp
index b9a95a939..b0530bd4a 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 e7a5f35de..86d9af2e7 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() {
--
GitLab