From 8d8c1fe5a7a0c0d89c8f11b41f8eef6c32c0684f Mon Sep 17 00:00:00 2001
From: Sebastian Hahta <joseha@utu.fi>
Date: Thu, 30 Jul 2020 14:43:12 +0300
Subject: [PATCH] rectification interpolation
---
.../src/sources/stereovideo/rectification.cpp | 45 +++++++++----------
.../src/sources/stereovideo/rectification.hpp | 13 ++++--
.../src/sources/stereovideo/stereovideo.cpp | 8 ++++
3 files changed, 40 insertions(+), 26 deletions(-)
diff --git a/components/rgbd-sources/src/sources/stereovideo/rectification.cpp b/components/rgbd-sources/src/sources/stereovideo/rectification.cpp
index 0b59d2ead..213aada07 100644
--- a/components/rgbd-sources/src/sources/stereovideo/rectification.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/rectification.cpp
@@ -24,13 +24,10 @@ StereoRectification::StereoRectification(nlohmann::json &config, cv::Size image_
enabled_(false), valid_(false), interpolation_(cv::INTER_LINEAR),
baseline_(0.0) {
-}
-
-void StereoRectification::setSize(cv::Size size) {
- image_resolution_ = size;
- if (calibrated()) {
- calculateParameters();
- }
+ map_l_.first.create(image_resolution_, map_format_);
+ map_l_.second.create(image_resolution_, map_format_);
+ map_r_.first.create(image_resolution_, map_format_);
+ map_r_.second.create(image_resolution_, map_format_);
}
void StereoRectification::setInterpolation(int interpolation) {
@@ -53,11 +50,11 @@ void StereoRectification::setCalibration(CalibrationData &calib) {
if (calib.hasCalibration(Channel::Left) && calib.hasCalibration(Channel::Right)) {
calib_left_ = calib.get(Channel::Left);
calib_right_ = calib.get(Channel::Right);
- calculateParameters();
+ updateCalibration_();
}
}
-void StereoRectification::calculateParameters() {
+void StereoRectification::updateCalibration_() {
using namespace ftl::calibration;
// TODO: lock
{
@@ -80,34 +77,36 @@ void StereoRectification::calculateParameters() {
tmp_r_ = cv::Mat(image_resolution_, CV_8UC4);
}
- cv::Mat K_l = calib_left_.intrinsic.matrix(image_resolution_);
- cv::Mat K_r = calib_right_.intrinsic.matrix(image_resolution_);
- cv::Mat dc_l = calib_left_.intrinsic.distCoeffs.Mat();
- cv::Mat dc_r = calib_right_.intrinsic.distCoeffs.Mat();
-
// calculate rotation and translation from left to right using calibration
cv::Mat T_l = calib_left_.extrinsic.matrix();
cv::Mat T_r = calib_right_.extrinsic.matrix();
cv::Mat T = T_r * transform::inverse(T_l);
- cv::Mat R, t;
- transform::getRotationAndTranslation(T, R, t);
- baseline_ = cv::norm(t);
+ transform::getRotationAndTranslation(T, R_, t_);
+ baseline_ = cv::norm(t_);
if (baseline_ == 0.0) { return; }
+ valid_ = true;
+ calculateParameters_();
+}
+
+void StereoRectification::calculateParameters_() {
+ if (!valid_) { return; }
+
+ cv::Mat K_l = calib_left_.intrinsic.matrix(image_resolution_);
+ cv::Mat K_r = calib_right_.intrinsic.matrix(image_resolution_);
+ cv::Mat dc_l = calib_left_.intrinsic.distCoeffs.Mat();
+ cv::Mat dc_r = calib_right_.intrinsic.distCoeffs.Mat();
// calculate rectification parameters
cv::stereoRectify( K_l, dc_l, K_r, dc_r, image_resolution_,
- R, t, R_l_, R_r_, P_l_, P_r_, Q_, 0, 0);
+ R_, t_, R_l_, R_r_, P_l_, P_r_, Q_, 0, 0);
- // for CPU remap, CV_16SC2 should give best performance
- // https://docs.opencv.org/master/da/d54/group__imgproc__transform.html
cv::initUndistortRectifyMap(K_l, dc_l, R_l_, P_l_, image_resolution_,
- CV_16SC2, map_l_.first, map_l_.second);
+ map_format_, map_l_.first, map_l_.second);
cv::initUndistortRectifyMap(K_r, dc_r, R_r_, P_r_, image_resolution_,
- CV_16SC2, map_r_.first, map_r_.second);
+ map_format_, map_r_.first, map_r_.second);
- valid_ = true;
}
void StereoRectification::rectify(cv::InputOutputArray im, Channel c) {
diff --git a/components/rgbd-sources/src/sources/stereovideo/rectification.hpp b/components/rgbd-sources/src/sources/stereovideo/rectification.hpp
index 07ec58052..2e924b7f3 100644
--- a/components/rgbd-sources/src/sources/stereovideo/rectification.hpp
+++ b/components/rgbd-sources/src/sources/stereovideo/rectification.hpp
@@ -29,10 +29,12 @@ class StereoRectification : public ftl::Configurable {
public:
StereoRectification(nlohmann::json &config, cv::Size image_size);
- /** Set OpenCV interpolation mode, see cv::InterpolationFlags */
+ /** Set OpenCV interpolation mode, see cv::InterpolationFlags.
+ * NOTE: Artifacts possible if modified and rectify() is called in another
+ * thread (no synchronization)
+ */
void setInterpolation(int interpolation);
- void setSize(cv::Size);
/**
* Calculate rectification parameters from given calibration.
*/
@@ -62,7 +64,8 @@ public:
double doff(cv::Size);
protected:
- void calculateParameters();
+ void updateCalibration_(); // update calibration and calculate new params
+ void calculateParameters_(); // re-calculate rectification maps and params
private:
ftl::calibration::CalibrationData::Calibration calib_left_;
@@ -75,6 +78,8 @@ private:
bool valid_;
int interpolation_;
double baseline_;
+ cv::Mat R_; // rotation left to right
+ cv::Mat t_; // translation left to right
cv::Mat Q_;
cv::Mat R_l_;
cv::Mat R_r_;
@@ -84,6 +89,8 @@ private:
// 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())
+ // https://docs.opencv.org/master/da/d54/group__imgproc__transform.html
+ int map_format_ = CV_16SC2;
std::pair<cv::Mat,cv::Mat> map_l_;
std::pair<cv::Mat,cv::Mat> map_r_;
diff --git a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
index 12da67957..b7f3d6ce3 100644
--- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
@@ -185,6 +185,14 @@ void StereoVideoSource::init(const string &file) {
do_update_params_ = true;
});
+ rectification_->setInterpolation(
+ host_->value("rectify_inter_cubic", false) ? cv::INTER_CUBIC : cv::INTER_LINEAR);
+
+ host_->on("rectify_inter_cubic", [this]() {
+ bool v = host_->value("rectify_inter_cubic", false);
+ rectification_->setInterpolation(v ? cv::INTER_CUBIC : cv::INTER_LINEAR);
+ });
+
host_->on("offset_z", [this]() {
do_update_params_ = true;
});
--
GitLab