Merge branch 'feature/maxdepth' into 'master'

set max_depth paramter using depth_resolution

See merge request nicolas.pope/ftl!248

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_);
 }
 

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

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