From 2c267f78dba5c3c7bddefea89833b96598261b92 Mon Sep 17 00:00:00 2001
From: Sebastian Hahta <joseha@utu.fi>
Date: Wed, 17 Jul 2019 11:02:26 +0300
Subject: [PATCH] Feature/use intrinsics
---
components/rgbd-sources/src/calibrate.cpp | 36 ++++++++++++++++++++---
components/rgbd-sources/src/calibrate.hpp | 3 +-
2 files changed, 34 insertions(+), 5 deletions(-)
diff --git a/components/rgbd-sources/src/calibrate.cpp b/components/rgbd-sources/src/calibrate.cpp
index 24aa65081..c3a84ef96 100644
--- a/components/rgbd-sources/src/calibrate.cpp
+++ b/components/rgbd-sources/src/calibrate.cpp
@@ -5,6 +5,7 @@
#include <loguru.hpp>
#include <ftl/config.h>
#include <ftl/configuration.hpp>
+#include <ftl/threads.hpp>
#include <iostream>
#include <sstream>
@@ -46,15 +47,42 @@ Calibrate::Calibrate(nlohmann::json &config, cv::Size image_size, cv::cuda::Stre
calibrated_ = _loadCalibration(image_size, map1, map2);
if (calibrated_) {
+ _updateIntrinsics();
LOG(INFO) << "Calibration loaded from file";
- map1_gpu_.first.upload(map1.first, stream);
- map1_gpu_.second.upload(map1.second, stream);
- map2_gpu_.first.upload(map2.first, stream);
- map2_gpu_.second.upload(map2.second, stream);
}
else {
LOG(WARNING) << "Calibration not loaded";
}
+
+ this->on("use_intrinsics", [this](const ftl::config::Event &e) {
+ _updateIntrinsics();
+ });
+}
+
+void Calibrate::_updateIntrinsics() {
+ Mat R1, R2, P1, P2;
+ std::pair<Mat, Mat> map1, map2;
+
+ if (this->value("use_intrinsics", true)) {
+ R1 = R1_;
+ R2 = R2_;
+ P1 = P1_;
+ P2 = P2_;
+ }
+ else {
+ R1 = Mat::eye(Size(3, 3), CV_64FC1);
+ R2 = R1;
+ P1 = M1_;
+ P2 = M2_;
+ }
+
+ initUndistortRectifyMap(M1_, D1_, R1, P1, img_size_, CV_32FC1, map1.first, map2.first);
+ initUndistortRectifyMap(M2_, D2_, R2, P2, img_size_, CV_32FC1, map1.second, map2.second);
+
+ map1_gpu_.first.upload(map1.first);
+ map1_gpu_.second.upload(map1.second);
+ map2_gpu_.first.upload(map2.first);
+ map2_gpu_.second.upload(map2.second);
}
bool Calibrate::_loadCalibration(cv::Size img_size, std::pair<Mat, Mat> &map1, std::pair<Mat, Mat> &map2) {
diff --git a/components/rgbd-sources/src/calibrate.hpp b/components/rgbd-sources/src/calibrate.hpp
index b8b98116b..fd43f21b2 100644
--- a/components/rgbd-sources/src/calibrate.hpp
+++ b/components/rgbd-sources/src/calibrate.hpp
@@ -43,7 +43,7 @@ class Calibrate : public ftl::Configurable {
bool isCalibrated();
void updateCalibration(const ftl::rgbd::Camera &p);
-
+
/**
* Get the camera matrix. Used to convert disparity map back to depth and
* a 3D point cloud.
@@ -52,6 +52,7 @@ class Calibrate : public ftl::Configurable {
const cv::Mat &getCameraMatrix() const { return P_; }
private:
+ void _updateIntrinsics();
bool _loadCalibration(cv::Size img_size, std::pair<cv::Mat, cv::Mat> &map1, std::pair<cv::Mat, cv::Mat> &map2);
private:
--
GitLab