Visualise focal point disparity

lib/libstereo/src/algorithms/clustersf.cu

@@ -43,16 +43,41 @@ void StereoCSF::compute(cv::InputArray l, cv::InputArray r, cv::OutputArray disp
 	SalientGradientGrouped sgr = {impl_->r.data(), impl_->gr.data(), impl_->temp.data(), impl_->buckets_r.data(), impl_->r.width, impl_->r.height};
 	parallel1DWarpSM(sgr, r.rows(), r.cols());
 
-	FocalCluster fc = {make_short2(300, 300), impl_->buckets_l.data(), impl_->buckets_r.data(), impl_->focal.data(), 1024};
+	impl_->focal.toGpuMat().setTo(cv::Scalar(0));	
+
+	FocalCluster fc = {make_short2(1000, 800), impl_->buckets_l.data(), impl_->buckets_r.data(), impl_->focal.data(), 1024};
 	parallel1DWarp(fc, l.rows(), 1);
 
 	cv::Mat tmp;
 	impl_->focal.toGpuMat().download(tmp);
+
+	double minval;
+	double maxval;
+	int minloc[2];
+	int maxloc[2];
+	cv::minMaxIdx(tmp, &minval, &maxval, minloc, maxloc);
+
+	std::cout << "Focal Disparity = " << maxloc[1] << std::endl;
+
 	tmp.convertTo(tmp, CV_8UC1, 0.1);
 	cv::resize(tmp,tmp, cv::Size(tmp.cols, 100));
 	cv::applyColorMap(tmp, tmp, cv::COLORMAP_TURBO);
 	cv::imshow("Gradients Right", tmp);
 
+	cv::Mat imgleft, imgright;
+	impl_->l.toGpuMat().download(imgleft);
+	impl_->r.toGpuMat().download(imgright);
+
+	cv::cvtColor(imgleft,imgleft, cv::COLOR_GRAY2BGR);
+	cv::cvtColor(imgright,imgright, cv::COLOR_GRAY2BGR);
+	cv::drawMarker(imgleft, cv::Point(1000,800), cv::Scalar(0,0,255));
+	cv::drawMarker(imgright, cv::Point(1000-maxloc[1],800), cv::Scalar(0,0,255));
+
+	cv::resize(imgleft,imgleft, cv::Size(imgleft.cols/2, imgleft.rows/2));
+	cv::resize(imgright,imgright, cv::Size(imgright.cols/2, imgright.rows/2));
+	cv::imshow("Left Focal", imgleft);
+	cv::imshow("Right Focal", imgright);
+
 	//impl_->gr.toGpuMat().download(tmp);
 	//cv::resize(tmp,tmp, cv::Size(tmp.cols/2, tmp.rows/2));
 	//cv::imshow("Gradients Right", tmp);