From ae60ce3cab421e73a09e12539de57ef2a172ec57 Mon Sep 17 00:00:00 2001
From: Sebastian Hahta <joseha@utu.fi>
Date: Thu, 30 Jul 2020 13:26:23 +0300
Subject: [PATCH] bug/rectification
---
.../src/sources/stereovideo/rectification.cpp | 18 ++++++-----
.../src/sources/stereovideo/rectification.hpp | 30 +++++++++----------
2 files changed, 26 insertions(+), 22 deletions(-)
diff --git a/components/rgbd-sources/src/sources/stereovideo/rectification.cpp b/components/rgbd-sources/src/sources/stereovideo/rectification.cpp
index a590b76ed..0b59d2ead 100644
--- a/components/rgbd-sources/src/sources/stereovideo/rectification.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/rectification.cpp
@@ -140,25 +140,29 @@ void StereoRectification::rectify(cv::InputOutputArray im, Channel c) {
}
cv::Mat StereoRectification::getPose(Channel c) {
+ // NOTE: FTL poses are camera-to-world transformations while the parameters
+ // in calibration are world-to-camera. cv::stereoRectify() rotation
+ // is unrectified-to-rectified.
+
using ftl::calibration::transform::inverse;
if (enabled_ && valid_) {
- cv::Mat T = cv::Mat::eye(4, 4, CV_64FC1);
+ cv::Mat R = cv::Mat::eye(4, 4, CV_64FC1);
if (c == Channel::Left) {
- R_l_.copyTo(T(cv::Rect(0, 0, 3, 3)));
- return calib_left_.extrinsic.matrix() * inverse(T);
+ R_l_.copyTo(R(cv::Rect(0, 0, 3, 3)));
+ return inverse(R * calib_left_.extrinsic.matrix());
}
else if (c == Channel::Right) {
- R_r_.copyTo(T(cv::Rect(0, 0, 3, 3)));
- return calib_right_.extrinsic.matrix() * inverse(T);
+ R_r_.copyTo(R(cv::Rect(0, 0, 3, 3)));
+ return inverse(R * calib_right_.extrinsic.matrix());
}
}
else {
if (c == Channel::Left) {
- return calib_left_.extrinsic.matrix();
+ return inverse(calib_left_.extrinsic.matrix());
}
else if (c == Channel::Right) {
- return calib_right_.extrinsic.matrix();
+ return inverse(calib_right_.extrinsic.matrix());
}
}
throw ftl::exception("Invalid channel, expected Left or Right");
diff --git a/components/rgbd-sources/src/sources/stereovideo/rectification.hpp b/components/rgbd-sources/src/sources/stereovideo/rectification.hpp
index 554ddaec5..07ec58052 100644
--- a/components/rgbd-sources/src/sources/stereovideo/rectification.hpp
+++ b/components/rgbd-sources/src/sources/stereovideo/rectification.hpp
@@ -21,12 +21,15 @@ namespace detail {
/**
* Stereo rectification. Performs rectification for left and right channels.
- * Rectified image is same size as input image.
+ * Rectified image is same size as input image. Camera parameters reported by
+ * getPose() and cameraMatrix() are for rectified camera (if enabled and valid
+ * calibration set).
*/
class StereoRectification : public ftl::Configurable {
public:
StereoRectification(nlohmann::json &config, cv::Size image_size);
+ /** Set OpenCV interpolation mode, see cv::InterpolationFlags */
void setInterpolation(int interpolation);
void setSize(cv::Size);
@@ -38,26 +41,23 @@ public:
void rectify(cv::InputOutputArray im, ftl::codecs::Channel c);
- /**
- * Enable/disable rectification.
+ /** Enable/disable rectification. TODO: move outside (to stereovideo)?
*/
void setEnabled(bool enabled);
bool enabled();
- /**
- * Get camera pose (rectified if enabled and valid)
+ /** Get camera pose (camera to world). Returns rectified pose if
+ * rectification is enabled (and valid calibration is set).
*/
cv::Mat getPose(ftl::codecs::Channel c = ftl::codecs::Channel::Left);
- /**
- * Get intrinsic matrix.
- */
+ /** Get intrinsic matrix. */
cv::Mat cameraMatrix(ftl::codecs::Channel c = ftl::codecs::Channel::Left);
cv::Mat cameraMatrix(cv::Size size, ftl::codecs::Channel c = ftl::codecs::Channel::Left);
- /** Stereo baseline */
+ /** Stereo baseline. In same unit as calibration (usually meters) */
double baseline();
- /** Disparity offset */
+ /** Disparity offset (pixels) */
double doff();
double doff(cv::Size);
@@ -65,13 +65,12 @@ protected:
void calculateParameters();
private:
- cv::Size calib_resolution_;
ftl::calibration::CalibrationData::Calibration calib_left_;
ftl::calibration::CalibrationData::Calibration calib_right_;
cv::Size image_resolution_;
- // rectification parameters and maps
+ // rectification parameters
bool enabled_;
bool valid_;
int interpolation_;
@@ -82,12 +81,13 @@ private:
cv::Mat P_l_;
cv::Mat P_r_;
+ // rectification maps for cv::remap(); should be CV_16SC2 if remap done on
+ // CPU and CV_32SC2 for GPU (generated by calculateParameters(), used by
+ // rectify())
std::pair<cv::Mat,cv::Mat> map_l_;
std::pair<cv::Mat,cv::Mat> map_r_;
- // temporary buffers
- // cv::cuda::HostMem tmp_l_;
- // cv::cuda::HostMem tmp_r_;
+ // temporary buffers for left/right
cv::Mat tmp_l_;
cv::Mat tmp_r_;
};
--
GitLab