Corrections to gpumat fixstars

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

@@ -19,23 +19,25 @@ FixstarsSGM::FixstarsSGM(nlohmann::json &config) : Disparity(config) {
 }
 
 void FixstarsSGM::compute(const cv::cuda::GpuMat &l, const cv::cuda::GpuMat &r, cv::cuda::GpuMat &disp, cv::cuda::Stream &stream) {
-	
+	// TODO Move to member vars to prevent reallocations
 	GpuMat lbw, rbw;
 	cv::cuda::cvtColor(l, lbw, cv::COLOR_BGR2GRAY, 0, stream);
 	cv::cuda::cvtColor(r, rbw, cv::COLOR_BGR2GRAY, 0, stream);
 
-	if (disp.type() != CV_16SC1 || disp.rows != l.rows || disp.cols != l.cols) {
-		disp = GpuMat(l.rows, l.cols, CV_16SC1);
-	}
+	//if (disp.type() != CV_16SC1 || disp.rows != l.rows || disp.cols != l.cols) {
+		//disp = GpuMat(l.rows, l.cols, CV_16SC1);
+	//}
+
+	GpuMat dispt(l.rows, l.cols, CV_16SC1);
 
 	stream.waitForCompletion();
 	if (!ssgm_) { // todo: move to constructor
-		ssgm_ = new sgm::StereoSGM(l.cols, l.rows, max_disp_, 8, 16, lbw.step, disp.step / sizeof(short),
-			sgm::EXECUTE_INOUT_HOST2HOST, sgm::StereoSGM::Parameters(10,120,0.95f,true));
+		ssgm_ = new sgm::StereoSGM(l.cols, l.rows, max_disp_, 8, 16, lbw.step, dispt.step / sizeof(short),
+			sgm::EXECUTE_INOUT_CUDA2CUDA, sgm::StereoSGM::Parameters(10,120,0.95f,true));
 	}
 
 	//auto start = std::chrono::high_resolution_clock::now();
-	ssgm_->execute(lbw.data, rbw.data, disp.data);
+	ssgm_->execute(lbw.data, rbw.data, dispt.data);
 	//std::chrono::duration<double> elapsed =
 	//		std::chrono::high_resolution_clock::now() - start;
 	//LOG(INFO) << "CUDA sgm in " << elapsed.count() << "s";
@@ -45,20 +47,21 @@ void FixstarsSGM::compute(const cv::cuda::GpuMat &l, const cv::cuda::GpuMat &r,
 	//Mat bad_pixels = (disp == (256 << 5)); 
 	
 	//disp.setTo(0, bad_pixels, stream_);
-	GpuMat left_pixels(disp, cv::Rect(0, 0, max_disp_, disp.rows));
+	GpuMat left_pixels(dispt, cv::Rect(0, 0, max_disp_, dispt.rows));
 	left_pixels.setTo(0);
 
 	if (use_filter_) {
 		// parameters need benchmarking, impact of image
 		// quick tests show with parameters (max_disp_, 25, 3)
 		// roughly 50% in disparity calculation and 50% in filter;
-		filter_->apply(disp, l, disp, stream);
+		filter_->apply(dispt, l, dispt, stream);
 	}
 	
-	disp.convertTo(disp, CV_32F, 1.0f/16.0f);
+	dispt.convertTo(disp, CV_32F, 1.0f/16.0f, stream);
 }
 
 void FixstarsSGM::setMask(Mat &mask) {
+	return; // TODO(Nick) Not needed, but also code below does not work with new GPU pipeline
 	CHECK(mask.type() == CV_8UC1) << "mask type must be CV_8U";
 	
 	if (!ssgm_) { // todo: move to constructor