Attempt to focal limit the features

lib/libstereo/src/algorithms/clustersf.cu

@@ -38,7 +38,7 @@ void StereoCSF::compute(cv::InputArray l, cv::InputArray r, cv::OutputArray disp
 	mat2gray(l, impl_->l);
 	mat2gray(r, impl_->r);
 
-	SalientGradient sgl = {impl_->l.data(), impl_->gl.data(), impl_->temp.data(), impl_->buckets_l.data(), impl_->l.width, impl_->l.height};
+	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};
 	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());
@@ -59,10 +59,10 @@ 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, 0.1);
+	tmp.convertTo(tmp, CV_8UC1, 10);
 	cv::resize(tmp,tmp, cv::Size(tmp.cols, 100));
 	cv::applyColorMap(tmp, tmp, cv::COLORMAP_TURBO);
-	cv::imshow("Gradients Right", tmp);
+	cv::imshow("Disparity Hist", tmp);
 
 	cv::Mat imgleft, imgright;
 	impl_->l.toGpuMat().download(imgleft);

lib/libstereo/src/filters/salient_gradient.hpp

@@ -9,8 +9,15 @@
 /**
  * Select salient gradient features and gather into scanline buckets that
  * includes the x-coordinate and feature orientation. Not grouped by orientation.
+ * 
+ * TODO: This needs to be a focal point modified version. Radius from focal
+ * point determines threshold, but the radius itself is determined by feature
+ * density around the focal point ... ultimately keeping the number of features
+ * within a reasonable very small level per scanline (say 2-8).
  */
 struct SalientGradient {
+	short2 focal_pt;
+	int radius;
 	Array2D<uchar>::Data image;
 	Array2D<uchar>::Data angle;
 	Array2D<uchar>::Data magnitude;
@@ -47,6 +54,13 @@ struct SalientGradient {
 		return __ffs(t);
 	}
 
+	__device__ inline float weighting(float r, float h) {
+		if (r >= h) return 0.0f;
+		float rh = r / h;
+		rh = 1.0f - rh*rh;
+		return rh*rh*rh*rh;
+	}
+
 	__device__ void operator()(ushort2 thread, ushort2 stride, ushort2 size, WarpSharedMemory &wsm) {
 		static const float PI = 3.14f;
 		static constexpr float PI2 = PI/2.0f;
@@ -73,7 +87,8 @@ struct SalientGradient {
 
 			// Apply threshold
 			for (int x=thread.x; x<size.x; x+=stride.x) {
-				uchar thresh = gthresh;
+				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;
 				}