diff --git a/components/rgbd-sources/include/ftl/stereovideo_source.hpp b/components/rgbd-sources/include/ftl/stereovideo_source.hpp
index 998c3532e2c75bb7694c8bf2447cdaed971c476a..d81df41865141f2692f3ba8fda414e3f53ebab5c 100644
--- a/components/rgbd-sources/include/ftl/stereovideo_source.hpp
+++ b/components/rgbd-sources/include/ftl/stereovideo_source.hpp
@@ -42,6 +42,7 @@ class StereoVideoSource : public RGBDSource {
bool ready_;
cv::Mat left_;
cv::Mat right_;
+ cv::Mat mask_l_;
};
}
diff --git a/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp b/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp
index 012dffa12947b067f0f09b83d605fa756b75b329..b2c2f110c72945f38e4ae005eed5b52f4a41bf46 100644
--- a/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp
+++ b/components/rgbd-sources/src/algorithms/fixstars_sgm.cpp
@@ -17,7 +17,7 @@ FixstarsSGM::FixstarsSGM(nlohmann::json &config) : Disparity(config) {
filter_ = cv::cuda::createDisparityBilateralFilter(max_disp_ << 4, config.value("filter_radius", 25), config.value("filter_iter", 1));
}
-void FixstarsSGM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
+void FixstarsSGM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp, const cv::Mat &mask_l) {
Mat left_disp;
Mat right_disp;
@@ -43,6 +43,7 @@ void FixstarsSGM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
// disparity values set to (256 << 5) in libSGM consistency check
Mat bad_pixels = (disp == (256 << 5));
disp.setTo(0, bad_pixels);
+ disp.setTo(0, mask_l);
if (use_filter_) {
cv::cuda::GpuMat l_gpu, disp_gpu, disp_gpu_out;
@@ -57,3 +58,8 @@ void FixstarsSGM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
disp.convertTo(disp, CV_32F, 1.0f/16.0f);
}
+
+void FixstarsSGM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
+ // todo: allow using without mask
+ LOG(FATAL) << "libSGM: not using mask (required)!";
+}
diff --git a/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp b/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp
index 1e78aba89e2efe23f546cd28bed31b3eab80ae0b..4f78280f4e09b6f709c40c0d52285b8f2e09d659 100644
--- a/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp
+++ b/components/rgbd-sources/src/algorithms/fixstars_sgm.hpp
@@ -25,8 +25,9 @@ class FixstarsSGM : public ftl::Disparity {
public:
explicit FixstarsSGM(nlohmann::json &config);
- void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp);
-
+ void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) override;
+ void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp, const cv::Mat &mask_l) override;
+
/* Factory creator */
static inline Disparity *create(nlohmann::json &config) {
return new FixstarsSGM(config);
diff --git a/components/rgbd-sources/src/calibrate.cpp b/components/rgbd-sources/src/calibrate.cpp
index 0134afbcd042efe7baf90dd9c9a202593de78a79..c4bcda7a1dffe9809b8a33c54f9808a472267cdb 100644
--- a/components/rgbd-sources/src/calibrate.cpp
+++ b/components/rgbd-sources/src/calibrate.cpp
@@ -492,6 +492,11 @@ bool Calibrate::_recalibrate(vector<vector<Point2f>> *imagePoints,
return true;
}
+void Calibrate::rectifyStereo(cv::Mat &l, cv::Mat &r) {
+ remap(l, l, map1_[0], map2_[0], INTER_LINEAR);
+ remap(r, r, map1_[1], map2_[1], INTER_LINEAR);
+}
+
bool Calibrate::undistort(cv::Mat &l, cv::Mat &r) {
Mat l_tmp, r_tmp;
local_->get(l_tmp, r_tmp);
diff --git a/components/rgbd-sources/src/calibrate.hpp b/components/rgbd-sources/src/calibrate.hpp
index 2e12f0e0efca21577ad543db8a286d7bc13f9b4d..0e6a5d81fc0834816c76914b52db4d900d6cf4df 100644
--- a/components/rgbd-sources/src/calibrate.hpp
+++ b/components/rgbd-sources/src/calibrate.hpp
@@ -101,6 +101,11 @@ class Calibrate {
*/
bool rectified(cv::Mat &l, cv::Mat &r);
+ /**
+ * Rectify and remove distortions from from images l and r using cv::remap()
+ */
+ void rectifyStereo(cv::Mat &l, cv::Mat &r);
+
bool isCalibrated();
/**
diff --git a/components/rgbd-sources/src/disparity.hpp b/components/rgbd-sources/src/disparity.hpp
index d7f24d583357e4d8c2640c2eae9ef42873c3f214..33bf58481161a14f8f5d1721a1f600ffb2d9e658 100644
--- a/components/rgbd-sources/src/disparity.hpp
+++ b/components/rgbd-sources/src/disparity.hpp
@@ -28,7 +28,10 @@ class Disparity {
* disparity from left and right images to be implemented.
*/
virtual void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp)=0;
-
+ virtual void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp, const cv::Mat &mask_l) {
+ compute(l, r, disp);
+ }
+
/**
* Factory registration class.
*/
diff --git a/components/rgbd-sources/src/stereovideo_source.cpp b/components/rgbd-sources/src/stereovideo_source.cpp
index bf6d585be76975e5fa4a6feb8b195fb836bbed7f..571981758d806ede860693c5d369861a45af54af 100644
--- a/components/rgbd-sources/src/stereovideo_source.cpp
+++ b/components/rgbd-sources/src/stereovideo_source.cpp
@@ -58,7 +58,14 @@ StereoVideoSource::StereoVideoSource(nlohmann::json &config, const string &file)
0.0f, // 0m min
15.0f // 15m max
};
-
+
+ // left and right masks (areas outside rectified images)
+ // only left mask used
+ cv::Mat mask_r(lsrc_->height(), lsrc_->width(), CV_8U, 255);
+ cv::Mat mask_l(lsrc_->height(), lsrc_->width(), CV_8U, 255);
+ calib_->rectifyStereo(mask_l, mask_r);
+ mask_l_ = (mask_l == 0);
+
disp_ = Disparity::create(config["disparity"]);
if (!disp_) LOG(FATAL) << "Unknown disparity algorithm : " << config["disparity"];
@@ -105,7 +112,7 @@ static void disparityToDepth(const cv::Mat &disparity, cv::Mat &depth, const cv:
void StereoVideoSource::getRGBD(cv::Mat &rgb, cv::Mat &depth) {
cv::Mat disp;
- disp_->compute(left_, right_, disp);
+ disp_->compute(left_, right_, disp, mask_l_);
rgb = left_;
disparityToDepth(disp, depth, calib_->getQ());
//calib_->distort(rgb,depth);