diff --git a/components/rgbd-sources/src/sources/stereovideo/opencv.cpp b/components/rgbd-sources/src/sources/stereovideo/opencv.cpp
index b395422ad99125bc0904ef2cd0e18e3355bc4df7..08e2a0f7523881e7a4e00c644a12994326d73b9a 100644
--- a/components/rgbd-sources/src/sources/stereovideo/opencv.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/opencv.cpp
@@ -46,7 +46,8 @@ using std::chrono::milliseconds;
using std::this_thread::sleep_for;
OpenCVDevice::OpenCVDevice(nlohmann::json &config, bool stereo)
- : ftl::rgbd::detail::Device(config), timestamp_(0.0) {
+ : ftl::rgbd::detail::Device(config), timestamp_(0.0),
+ interpolation_(cv::INTER_CUBIC) {
std::vector<ftl::rgbd::detail::DeviceDetails> devices_ = getDevices();
@@ -147,6 +148,12 @@ OpenCVDevice::OpenCVDevice(nlohmann::json &config, bool stereo)
left_hm_ = cv::cuda::HostMem(dheight_, dwidth_, CV_8UC4);
right_hm_ = cv::cuda::HostMem(dheight_, dwidth_, CV_8UC4);
hres_hm_ = cv::cuda::HostMem(height_, width_, CV_8UC4);
+
+ interpolation_ = value("inter_cubic", false) ? cv::INTER_CUBIC : cv::INTER_LINEAR;
+ on("inter_cubic", [this](){
+ interpolation_ = value("inter_cubic_", false) ?
+ cv::INTER_CUBIC : cv::INTER_LINEAR;
+ });
}
OpenCVDevice::~OpenCVDevice() {
@@ -355,7 +362,7 @@ bool OpenCVDevice::get(ftl::rgbd::Frame &frame, cv::cuda::GpuMat &l_out, cv::cud
if (hasHigherRes()) {
// TODO: Use threads?
- cv::resize(rfull, r, r.size(), 0.0, 0.0, cv::INTER_CUBIC);
+ cv::resize(rfull, r, r.size(), 0.0, 0.0, interpolation_);
r_hres_out = rfull;
}
else {
@@ -396,7 +403,7 @@ bool OpenCVDevice::get(ftl::rgbd::Frame &frame, cv::cuda::GpuMat &l_out, cv::cud
// Need to resize
//if (hasHigherRes()) {
// TODO: Use threads?
- // cv::resize(rfull, r, r.size(), 0.0, 0.0, cv::INTER_CUBIC);
+ // cv::resize(rfull, r, r.size(), 0.0, 0.0, interpolation_);
//}
} else {
cv::cvtColor(frame_l_, lfull, cv::COLOR_BGR2BGRA);
@@ -404,7 +411,7 @@ bool OpenCVDevice::get(ftl::rgbd::Frame &frame, cv::cuda::GpuMat &l_out, cv::cud
if (hasHigherRes()) {
//FTL_Profile("Frame Resize", 0.01);
- cv::resize(lfull, l, l.size(), 0.0, 0.0, cv::INTER_CUBIC);
+ cv::resize(lfull, l, l.size(), 0.0, 0.0, interpolation_);
l_hres_out.upload(hres, stream);
} else {
l_hres_out = cv::cuda::GpuMat();
diff --git a/components/rgbd-sources/src/sources/stereovideo/opencv.hpp b/components/rgbd-sources/src/sources/stereovideo/opencv.hpp
index f33e6dc3f8eb80d43a2e09136640d3158bb0b327..37389e63f286458a1eeb4f8180c534a98e556941 100644
--- a/components/rgbd-sources/src/sources/stereovideo/opencv.hpp
+++ b/components/rgbd-sources/src/sources/stereovideo/opencv.hpp
@@ -64,6 +64,7 @@ class OpenCVDevice : public ftl::rgbd::detail::Device {
cv::Mat frame_l_;
cv::Mat frame_r_;
+ int interpolation_;
};
}
diff --git a/components/rgbd-sources/src/sources/stereovideo/pylon.cpp b/components/rgbd-sources/src/sources/stereovideo/pylon.cpp
index d470f22bb31f289dad6b35bd9926ce5b20f5838e..48999538562bd26286fb258e7b131dc3d1ba4f74 100644
--- a/components/rgbd-sources/src/sources/stereovideo/pylon.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/pylon.cpp
@@ -24,7 +24,8 @@ using cv::Mat;
using namespace Pylon;
PylonDevice::PylonDevice(nlohmann::json &config)
- : ftl::rgbd::detail::Device(config), ready_(false), lcam_(nullptr), rcam_(nullptr) {
+ : ftl::rgbd::detail::Device(config), ready_(false), lcam_(nullptr), rcam_(nullptr),
+ interpolation_(cv::INTER_CUBIC) {
auto &inst = CTlFactory::GetInstance();
@@ -115,6 +116,12 @@ PylonDevice::PylonDevice(nlohmann::json &config)
});
on("buffer_size", buffer_size_, 1);
+
+ interpolation_ = value("inter_cubic", false) ? cv::INTER_CUBIC : cv::INTER_LINEAR;
+ on("inter_cubic", [this](){
+ interpolation_ = value("inter_cubic", false) ?
+ cv::INTER_CUBIC : cv::INTER_LINEAR;
+ });
}
PylonDevice::~PylonDevice() {
@@ -299,7 +306,7 @@ bool PylonDevice::get(ftl::rgbd::Frame &frame, cv::cuda::GpuMat &l_out, cv::cuda
c->rectify(rtmp2_, rfull, Channel::Right);
if (hasHigherRes()) {
- cv::resize(rfull, r, r.size(), 0.0, 0.0, cv::INTER_CUBIC);
+ cv::resize(rfull, r, r.size(), 0.0, 0.0, interpolation_);
h_r = rfull;
}
else {
@@ -340,7 +347,7 @@ bool PylonDevice::get(ftl::rgbd::Frame &frame, cv::cuda::GpuMat &l_out, cv::cuda
}
if (hasHigherRes()) {
- cv::resize(lfull, l, l.size(), 0.0, 0.0, cv::INTER_CUBIC);
+ cv::resize(lfull, l, l.size(), 0.0, 0.0, interpolation_);
h_l.upload(hres, stream);
} else {
h_l = cv::cuda::GpuMat();
diff --git a/components/rgbd-sources/src/sources/stereovideo/pylon.hpp b/components/rgbd-sources/src/sources/stereovideo/pylon.hpp
index 3cc4d0908e40dd5027291b1b8688a6fe7add22eb..707e8f437dd971d71ed5ec95ed28592b2f9145be 100644
--- a/components/rgbd-sources/src/sources/stereovideo/pylon.hpp
+++ b/components/rgbd-sources/src/sources/stereovideo/pylon.hpp
@@ -59,6 +59,7 @@ class PylonDevice : public ftl::rgbd::detail::Device {
cv::Mat rtmp_;
cv::Mat rtmp2_;
cv::Mat ltmp_;
+ int interpolation_;
void _configureCamera(Pylon::CBaslerUniversalInstantCamera *cam);
bool _retrieveFrames(Pylon::CGrabResultPtr &result, Pylon::CBaslerUniversalInstantCamera *cam);
diff --git a/components/rgbd-sources/src/sources/stereovideo/rectification.cpp b/components/rgbd-sources/src/sources/stereovideo/rectification.cpp
index edf221d6ddb0f1b0064b0522214ca403587cfecb..f755d5de89c62fc5272fc125d43ab7d2452cecfb 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::InputArray im, cv::OutputArray im_out, Channel c) {
diff --git a/components/rgbd-sources/src/sources/stereovideo/rectification.hpp b/components/rgbd-sources/src/sources/stereovideo/rectification.hpp
index 02e7aabc03b2899b7c3799c24790eec99e395aa1..d326d0d167b6cae7f06bdccd1bad6be1b6bfca40 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 12da679572a0bbdd0933f2456aa7cffcc6d6557f..b7f3d6ce3123335fcbe77d7cf720beb27f9db6dc 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;
});