From 119c3fb4957d3137abbcbbccf39e683c5848b7f5 Mon Sep 17 00:00:00 2001
From: Sebastian Hahta <joseha@utu.fi>
Date: Fri, 7 Feb 2020 16:17:27 +0200
Subject: [PATCH] set max_depth paramter using depth_resolution
---
.../src/sources/stereovideo/calibrate.cpp | 16 +++++++++++-----
.../src/sources/stereovideo/calibrate.hpp | 5 +++++
.../src/sources/stereovideo/stereovideo.cpp | 14 ++++++++++++--
3 files changed, 28 insertions(+), 7 deletions(-)
diff --git a/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp b/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp
index eef55614e..aaa4b54f5 100644
--- a/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp
@@ -7,11 +7,6 @@
#include <ftl/configuration.hpp>
#include <ftl/threads.hpp>
-#include <iostream>
-#include <sstream>
-#include <string>
-#include <ctime>
-
#include "calibrate.hpp"
#include "ftl/exception.hpp"
@@ -137,6 +132,11 @@ Mat Calibrate::_getK(size_t idx) {
return _getK(idx, img_size_);
}
+double Calibrate::getBaseline() const {
+ if (t_.empty()) { return 0.0; }
+ return cv::norm(t_);
+}
+
Mat Calibrate::getCameraMatrixLeft(const cv::Size res) {
if (rectify_) {
return scaleCameraIntrinsics(Mat(P1_, cv::Rect(0, 0, 3, 3)), res, img_size_);
@@ -279,6 +279,12 @@ bool Calibrate::writeCalibration( const string &fname, const Size &size,
bool Calibrate::saveCalibration(const string &fname) {
// note: never write rectified parameters!
+
+ // TODO: make a backup of old file
+ //if (std::filesystem::is_regular_file(fname)) {
+ // // copy to fname + ".bak"
+ //}
+
return writeCalibration(fname, calib_size_, K_, D_, R_, t_, pose_);
}
diff --git a/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp b/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp
index 3f2aff620..58313b1eb 100644
--- a/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp
+++ b/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp
@@ -51,6 +51,11 @@ class Calibrate : public ftl::Configurable {
*/
const cv::Mat &getQ() const { return Q_; }
+ /**
+ * @brief Get camera pair baseline
+ */
+ double getBaseline() const;
+
/**
* @brief Get intrinsic paramters. If rectification is enabled, returns
* rectified intrinsic parameters, otherwise returns values from
diff --git a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
index bfb247349..0184d1f04 100644
--- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
@@ -200,16 +200,26 @@ void StereoVideoSource::updateParameters() {
cv::Mat K;
// same for left and right
- float baseline = static_cast<float>(1.0 / calib_->getQ().at<double>(3,2));
+ float baseline = static_cast<float>(calib_->getBaseline());
float doff = static_cast<float>(-calib_->getQ().at<double>(3,3) * baseline);
+
+ double d_resolution = this->host_->getConfig().value<double>("depth_resolution", 0.0);
float min_depth = this->host_->getConfig().value<double>("min_depth", 0.0);
float max_depth = this->host_->getConfig().value<double>("max_depth", 15.0);
// left
K = calib_->getCameraMatrixLeft(color_size_);
+ float fx = static_cast<float>(K.at<double>(0,0));
+
+ if (d_resolution > 0.0) {
+ // Learning OpenCV p. 442
+ float max_depth_new = sqrt(d_resolution * fx * baseline);
+ max_depth = (max_depth_new > max_depth) ? max_depth : max_depth_new;
+ }
+
state_.getLeft() = {
- static_cast<float>(K.at<double>(0,0)), // Fx
+ fx,
static_cast<float>(K.at<double>(1,1)), // Fy
static_cast<float>(-K.at<double>(0,2)), // Cx
static_cast<float>(-K.at<double>(1,2)), // Cy
--
GitLab