Feature/libsgm mask

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, const cv::Mat &mask_l) {
+void FixstarsSGM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
 	Mat left_disp;
 	Mat right_disp;
 
@@ -25,7 +25,7 @@ void FixstarsSGM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp, con
 	cv::cvtColor(l, lbw, cv::COLOR_BGR2GRAY);
 	cv::cvtColor(r, rbw, cv::COLOR_BGR2GRAY);
 
-	if (!ssgm_) {
+	if (!ssgm_) { // todo: move to constructor
 		ssgm_ = new sgm::StereoSGM(l.cols, l.rows, max_disp_, 8, 16,
 			sgm::EXECUTE_INOUT_HOST2HOST,
 			sgm::StereoSGM::Parameters(10,120,0.95f,true));
@@ -43,7 +43,6 @@ void FixstarsSGM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp, con
 	// 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;
@@ -59,7 +58,15 @@ void FixstarsSGM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp, con
 	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)!";
+void FixstarsSGM::setMask(Mat &mask) {
+	LOG_ASSERT(mask.type() == CV_8UC1) << "mask type must be CV_8U";
+	
+	if (!ssgm_) { // todo: move to constructor
+		ssgm_ = new sgm::StereoSGM(mask.cols, mask.rows, max_disp_, 8, 16,
+			sgm::EXECUTE_INOUT_HOST2HOST,
+			sgm::StereoSGM::Parameters(10,120,0.95f,true));
+	}
+	
+	mask_l_ = mask;
+	ssgm_->setMask((uint8_t*) mask.data, mask.cols);
 }
\ No newline at end of file

components/rgbd-sources/src/algorithms/fixstars_sgm.hpp

@@ -27,7 +27,7 @@ class FixstarsSGM : public ftl::Disparity {
 	explicit FixstarsSGM(nlohmann::json &config);
 
 	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;
+	void setMask(cv::Mat &mask) override;
 	
 	/* Factory creator */
 	static inline Disparity *create(ftl::Configurable *p, const std::string &name) {

components/rgbd-sources/src/disparity.hpp

@@ -24,14 +24,13 @@ class Disparity : public ftl::Configurable {
 	virtual void setMinDisparity(size_t min) { min_disp_ = min; }
 	virtual void setMaxDisparity(size_t max) { max_disp_ = max; }
 	
+	virtual void setMask(cv::Mat &mask) { mask_l_ = mask; }
+	
 	/**
 	 * Pure virtual function representing the actual computation of
 	 * 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.
@@ -56,6 +55,7 @@ class Disparity : public ftl::Configurable {
 	//nlohmann::json &config_;
 	size_t min_disp_;
 	size_t max_disp_;
+	cv::Mat mask_l_;
 	
 	private:
 	static std::map<std::string,std::function<Disparity*(ftl::Configurable *, const std::string &)>> *algorithms__;

components/rgbd-sources/src/stereovideo_source.cpp

@@ -76,6 +76,7 @@ StereoVideoSource::StereoVideoSource(nlohmann::json &config, const string &file)
 	
 	disp_ = Disparity::create(this, "disparity");
     if (!disp_) LOG(FATAL) << "Unknown disparity algorithm : " << *get<ftl::config::json_t>("disparity");
+	disp_->setMask(mask_l_);
 
 	LOG(INFO) << "StereoVideo source ready...";
 	ready_ = true;
@@ -114,7 +115,7 @@ void StereoVideoSource::grab() {
 	calib_->rectified(left_, right_);
 
 	cv::Mat disp;
-	disp_->compute(left_, right_, disp, mask_l_);
+	disp_->compute(left_, right_, disp);
 
 	unique_lock<mutex> lk(mutex_);
 	left_.copyTo(rgb_);