diff --git a/components/operators/src/disparity/disparity_to_depth.cpp b/components/operators/src/disparity/disparity_to_depth.cpp index f9ae8b12774ca528cef322b1446a66c75aaef7c1..d00cced27bb6e5e8fc5870e6ac5cde432f964917 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 f2659128145d0908d75db567541ef974b5d14aa0..06470b21d513003fdc1d4b8accdcfcd6cb869c4c 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 7851fb1541e591c65ee5b17167a22ecfaa6baea8..c4200e0c3b6b50465b426f4caaa38b76552b5305 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 06a129b7f94665556a079e49b1f1b4b70946543c..967d846767da8cfd0e24dc2caf125947fa69e17c 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 94854624e01b9256f8a7a793ef740ca472c28000..5c4b6777286267e745e267ea7eb5416d3d24315e 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);