Fix focal radius weighting

lib/libstereo/src/algorithms/clustersf.cu

@@ -38,16 +38,28 @@ void StereoCSF::compute(cv::InputArray l, cv::InputArray r, cv::OutputArray disp
 	mat2gray(l, impl_->l);
 	mat2gray(r, impl_->r);
 
-	SalientGradient sgl = {make_short2(1000, 800), 200, impl_->l.data(), impl_->gl.data(), impl_->temp.data(), impl_->buckets_l.data(), impl_->l.width, impl_->l.height};
+	short2 focal_pt = {short(1000), short(800)};
+
+	SalientGradient sgl = {focal_pt, 1000, impl_->l.data(), impl_->gl.data(), impl_->temp.data(), impl_->buckets_l.data(), impl_->l.width, impl_->l.height};
 	parallel1DWarpSM(sgl, l.rows(), l.cols());
 	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());
 
 	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};
+	FocalCluster fc = {focal_pt, impl_->buckets_l.data(), impl_->buckets_r.data(), impl_->focal.data(), 1024};
 	parallel1DWarp(fc, l.rows(), 1);
 
+	Array2D<float> disp_array(l.cols(), l.rows());
+	FocalSelector fs = {focal_pt, impl_->buckets_l.data(), impl_->buckets_r.data(), impl_->focal.data(), disp_array.data(), 1024};
+	parallel1DWarp(fs, l.rows(), 1);
+
+	disp_array.toGpuMat().download(disparity);
+
+	cv::Mat gradtmp;
+	impl_->gl.toGpuMat().download(gradtmp);
+	cv::imshow("Gradient Left", gradtmp);
+
 	cv::Mat tmp;
 	impl_->focal.toGpuMat().download(tmp);
 
@@ -59,7 +71,7 @@ void StereoCSF::compute(cv::InputArray l, cv::InputArray r, cv::OutputArray disp
 
 	std::cout << "Focal Disparity = " << maxloc[1] << std::endl;
 
-	tmp.convertTo(tmp, CV_8UC1, 10);
+	tmp.convertTo(tmp, CV_8UC1, 255.0/maxval);
 	cv::resize(tmp,tmp, cv::Size(tmp.cols, 100));
 	cv::applyColorMap(tmp, tmp, cv::COLORMAP_TURBO);
 	cv::imshow("Disparity Hist", tmp);

lib/libstereo/src/filters/focal_cluster.hpp

@@ -3,6 +3,7 @@
 
 #include "../util.hpp"
 #include "../array1d.hpp"
+#include "../array2d.hpp"
 #include "../bucket1d.hpp"
 #include "../bucket2d.hpp"
 
@@ -36,4 +37,47 @@ struct FocalCluster {
 	}
 };
 
+struct FocalSelector {
+	short2 focal_pt;
+	Bucket1D<short2, 64>::Data left;
+	Bucket2D<ushort, 64>::Data right;
+	Array1D<int>::Data histogram;
+	Array2D<float>::Data disparity;
+
+	int max_disparity = 1024;
+
+	__device__ void operator()(ushort2 thread, ushort2 stride, ushort2 size) {
+		for (int y=thread.y; y<size.y; y+=stride.y) {
+			int count = left(y);
+			// Stride a warp of threads over the features
+			for (int f=thread.x; f<count; f+=stride.x) {
+				// For each feature or features near to focal point
+				short2 feature = left(y,f);
+				int distx = feature.x - focal_pt.x;
+
+				int best_disp = 0;
+				int max_v = 0;
+				
+				// - For each feature in right image that matches
+				const ushort *ptr = right.ptr(y, feature.y);
+				int count2 = right(y,feature.y);
+				for (int i=0; i<count2; ++i) {
+					//   - Add focal dist to feature X and add to histogram
+					int d = max(0,focal_pt.x - int(ptr[i]) + distx);
+					
+					int v = histogram(d);
+					if (v > max_v) {
+						max_v = v;
+						best_disp = d;
+					}
+				}
+
+				if (max_v > 0) {
+					disparity(y,feature.x) = float(best_disp);
+				}
+			}
+		}
+	}
+};
+
 #endif

lib/libstereo/src/filters/salient_gradient.hpp

@@ -87,19 +87,24 @@ struct SalientGradient {
 
 			// Apply threshold
 			for (int x=thread.x; x<size.x; x+=stride.x) {
-				uchar focal_thresh = max(255,int((1.0f - weighting(float(abs(focal_pt.x - x)), float(radius)))*255.0f));
-				uchar thresh = max(gthresh, focal_thresh);
-				for (int u=-3; u<=3; ++u) {
-					thresh = (x+u >= 0 && x+u < width) ? max(thresh, magnitude(y,x+u)) : thresh;
-				}
-
-				uchar m = magnitude(y,x);
-				//if (m < thresh) angle(y,x) = 0;
+				float dx = focal_pt.x - x;
+				float dy = focal_pt.y - y;
+				float dist = sqrt(dx*dx + dy*dy);
+				float weight = weighting(dist, float(radius));
+				float thresh = gthresh; //max(gthresh, focal_thresh);
+				//for (int u=-3; u<=3; ++u) {
+				//	thresh = (x+u >= 0 && x+u < width) ? max(thresh, float(magnitude(y,x+u))) : thresh;
+				//}
+
+				float m = float(magnitude(y,x)) * weight;
+				if (m < thresh) angle(y,x) = 0;
 				//output(y,x) = (m < thresh) ? 0 : 255;
 				if (m >= thresh) {
 					int a = angle(y,x);
 					buckets.add(y, make_short2(x, a));
 				}
+
+				angle(y,x) = uchar(weight*255.0f);
 			}
 		}
 	}
@@ -175,9 +180,9 @@ struct SalientGradientGrouped {
 			// Apply threshold
 			for (int x=thread.x; x<size.x; x+=stride.x) {
 				uchar thresh = gthresh;
-				for (int u=-3; u<=3; ++u) {
-					thresh = (x+u >= 0 && x+u < width) ? max(thresh, magnitude(y,x+u)) : thresh;
-				}
+				//for (int u=-3; u<=3; ++u) {
+				//	thresh = (x+u >= 0 && x+u < width) ? max(thresh, magnitude(y,x+u)) : thresh;
+				//}
 
 				uchar m = magnitude(y,x);
 				//if (m < thresh) angle(y,x) = 0;