From e3d5e86fe2a38634dc471f374ebba4ce88823e7e Mon Sep 17 00:00:00 2001
From: Sebastian Hahta <joseha@utu.fi>
Date: Thu, 27 Feb 2020 16:06:35 +0200
Subject: [PATCH] bug/resolution
---
.../operators/src/disparity/disparity_to_depth.cpp | 2 +-
components/rgbd-sources/src/source.cpp | 1 +
.../src/sources/stereovideo/calibrate.cpp | 8 ++++++++
.../src/sources/stereovideo/calibrate.hpp | 13 ++++++++++++-
.../src/sources/stereovideo/stereovideo.cpp | 2 +-
5 files changed, 23 insertions(+), 3 deletions(-)
diff --git a/components/operators/src/disparity/disparity_to_depth.cpp b/components/operators/src/disparity/disparity_to_depth.cpp
index f9ae8b127..d00cced27 100644
--- a/components/operators/src/disparity/disparity_to_depth.cpp
+++ b/components/operators/src/disparity/disparity_to_depth.cpp
@@ -13,8 +13,8 @@ bool DisparityToDepth::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out,
throw FTL_Error("Missing disparity before convert to depth");
}
- const auto params = in.getLeftCamera();
const GpuMat &disp = in.get<GpuMat>(Channel::Disparity);
+ const auto params = in.getLeftCamera().scaled(disp.cols, disp.rows);
GpuMat &depth = out.create<GpuMat>(Channel::Depth);
depth.create(disp.size(), CV_32FC1);
diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp
index f26591281..06470b21d 100644
--- a/components/rgbd-sources/src/source.cpp
+++ b/components/rgbd-sources/src/source.cpp
@@ -304,6 +304,7 @@ Camera Camera::scaled(int width, int height) const {
newcam.fy *= scaleY;
newcam.cx *= scaleX;
newcam.cy *= scaleY;
+ newcam.doffs *= scaleX;
return newcam;
}
diff --git a/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp b/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp
index 7851fb154..c4200e0c3 100644
--- a/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp
@@ -76,6 +76,14 @@ double Calibrate::getBaseline() const {
return cv::norm(t_);
}
+double Calibrate::getDoff() const {
+ return -(Q_.at<double>(3,3) * getBaseline());
+}
+
+double Calibrate::getDoff(const Size& size) const {
+ return getDoff() * ((double) size.width / (double) img_size_.width);
+}
+
Mat Calibrate::getCameraMatrixLeft(const cv::Size res) {
if (rectify_) {
return ftl::calibration::scaleCameraMatrix(Mat(P1_, cv::Rect(0, 0, 3, 3)), res, img_size_);
diff --git a/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp b/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp
index 06a129b7f..967d84676 100644
--- a/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp
+++ b/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp
@@ -53,12 +53,14 @@ class Calibrate : public ftl::Configurable {
/**
* @brief Get disparity to depth matrix
- *
+ * @note Disparity offset is in image_size (scale)
+ *
* 2020/01/15: StereoVideoSource creates a Camera object which is used to
* calculate depth from disparity (disp2depth.cu). Seems to be
* used only in StereoVideoSource to get doff and baseline
* parameter values in updateParameters()
*/
+ [[deprecated]]
const cv::Mat &getQ() const { return Q_; }
/**
@@ -66,6 +68,15 @@ class Calibrate : public ftl::Configurable {
*/
double getBaseline() const;
+ /**
+ * @brief Get camera pair disparity offset
+ * @param size (optional) scale to given resolution.
+ *
+ * Returns disparity offset for image_size resolution if size not provided.
+ */
+ double getDoff() const;
+ double getDoff(const cv::Size& size) 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 94854624e..5c4b67772 100644
--- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
@@ -202,7 +202,7 @@ void StereoVideoSource::updateParameters() {
// same for left and right
float baseline = static_cast<float>(calib_->getBaseline());
- float doff = static_cast<float>(-calib_->getQ().at<double>(3,3) * baseline);
+ float doff = calib_->getDoff(color_size_);
double d_resolution = this->host_->getConfig().value<double>("depth_resolution", 0.0);
float min_depth = this->host_->getConfig().value<double>("min_depth", 0.45);
--
GitLab