Implements #69 and seems to work for #100

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

@@ -19,25 +19,18 @@ 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);
-	//}
-
-	GpuMat dispt(l.rows, l.cols, CV_16SC1);
+	cv::cuda::cvtColor(l, lbw_, cv::COLOR_BGR2GRAY, 0, stream);
+	cv::cuda::cvtColor(r, rbw_, cv::COLOR_BGR2GRAY, 0, stream);
 
 	stream.waitForCompletion();
 	if (!ssgm_) { // todo: move to constructor
-		ssgm_ = new sgm::StereoSGM(l.cols, l.rows, max_disp_, 8, 16, lbw.step, dispt.step / sizeof(short),
+		dispt_ = GpuMat(l.rows, l.cols, CV_16SC1);
+		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, dispt.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";
@@ -47,17 +40,17 @@ 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(dispt, cv::Rect(0, 0, max_disp_, dispt.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(dispt, l, dispt, stream);
+		filter_->apply(dispt_, l, dispt_, stream);
 	}
 	
-	dispt.convertTo(disp, CV_32F, 1.0f/16.0f, stream);
+	dispt_.convertTo(disp, CV_32F, 1.0f/16.0f, stream);
 }
 
 void FixstarsSGM::setMask(Mat &mask) {

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

@@ -38,6 +38,9 @@ class FixstarsSGM : public ftl::rgbd::detail::Disparity {
 	bool use_filter_;
 	cv::Ptr<cv::cuda::DisparityBilateralFilter> filter_;
 	sgm::StereoSGM *ssgm_;
+	cv::cuda::GpuMat lbw_;
+	cv::cuda::GpuMat rbw_;
+	cv::cuda::GpuMat dispt_;
 };
 };
 };

components/rgbd-sources/src/stereovideo.cpp

@@ -91,24 +91,24 @@ void StereoVideoSource::init(const string &file) {
 
 static void disparityToDepth(const cv::cuda::GpuMat &disparity, cv::cuda::GpuMat &depth,
 							 const cv::Mat &Q, cv::cuda::Stream &stream) {
-	
-	if (depth.empty()) depth = cv::cuda::GpuMat(disparity.size(), CV_32F);
-
 	// Q(3, 2) = -1/Tx
 	// Q(2, 3) = f
 
 	double val = (1.0f / Q.at<double>(3, 2)) * Q.at<double>(2, 3);
-	cv::cuda::divide(val, disparity, depth, 1.0f, -1, stream);
+	cv::cuda::divide(val, disparity, depth, 1.0f / 1000.0f, -1, stream);
 }
 
 bool StereoVideoSource::grab() {
-	cv::cuda::GpuMat disp, depth;
 	lsrc_->get(left_, right_, stream_);
+	if (depth_tmp_.empty()) depth_tmp_ = cv::cuda::GpuMat(left_.size(), CV_32FC1);
+	if (disp_tmp_.empty()) disp_tmp_ = cv::cuda::GpuMat(left_.size(), CV_32FC1);
 	calib_->rectifyStereo(left_, right_, stream_);
-	disp_->compute(left_, right_, disp, stream_);
-	disparityToDepth(disp, depth, calib_->getQ(), stream_);
+	disp_->compute(left_, right_, disp_tmp_, stream_);
+	disparityToDepth(disp_tmp_, depth_tmp_, calib_->getQ(), stream_);
 	left_.download(rgb_, stream_); // TODO original left RGB image could be stored in host memory
-	depth.download(depth_, stream_);
+	depth_tmp_.download(depth_, stream_);
+
+	// TODO(Nick) Could potentially leave this until getFrames?
 	stream_.waitForCompletion();
 	
 	return true;

components/rgbd-sources/src/stereovideo.hpp

@@ -42,6 +42,8 @@ class StereoVideoSource : public detail::Source {
 
 	cv::cuda::GpuMat left_;
 	cv::cuda::GpuMat right_;
+	cv::cuda::GpuMat disp_tmp_;
+	cv::cuda::GpuMat depth_tmp_;
 	
 	cv::Mat mask_l_;