Resolve "Masking of invalid areas"

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_;
 };
 
 }

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)!";
+}

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);

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);

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();
 
 	/**

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.
 	 */

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);