Variance based hierarchical census

lib/libstereo/include/stereo.hpp

@@ -135,6 +135,11 @@ public:
 		int paths = AggregationDirections::HORIZONTAL |
 					AggregationDirections::VERTICAL |
 					AggregationDirections::DIAGONAL;
+		/** normalization of variance to range [alpha, beta] */
+		float alpha = 0.2;
+		float beta = 1.0;
+		/** variance window size */
+		int var_window = 9;
 		bool debug = false;
 	};
 	Parameters params;

lib/libstereo/src/algorithms/hcensussgm.cu

@@ -3,8 +3,40 @@
 #include "../costs/census.hpp"
 #include "../costs/dual.hpp"
 #include <opencv2/cudawarping.hpp>
+#include <opencv2/cudafilters.hpp>
 
-typedef MultiCosts<CensusMatchingCost,3> MatchingCost;
+typedef MultiCostsWeighted<CensusMatchingCost,3> MatchingCost;
+
+static void variance_mask(cv::InputArray in, cv::OutputArray out, int wsize=3) {
+	if (in.isGpuMat() && out.isGpuMat()) {
+		cv::cuda::GpuMat im;
+		cv::cuda::GpuMat im2;
+		cv::cuda::GpuMat mean;
+		cv::cuda::GpuMat mean2;
+
+		mean.create(in.size(), CV_32FC1);
+		mean2.create(in.size(), CV_32FC1);
+		im2.create(in.size(), CV_32FC1);
+
+		if (in.type() != CV_32FC1) {
+			in.getGpuMat().convertTo(im, CV_32FC1);
+		}
+		else {
+			im = in.getGpuMat();
+		}
+
+		cv::cuda::multiply(im, im, im2);
+		auto filter = cv::cuda::createBoxFilter(CV_32FC1, CV_32FC1, cv::Size(wsize,wsize));
+		filter->apply(im, mean);   // E[X]
+		filter->apply(im2, mean2); // E[X^2]
+		cv::cuda::multiply(mean, mean, mean); // (E[X])^2
+
+		// NOTE: floating point accuracy in subtraction
+		// (cv::cuda::createBoxFilter only supports 8 bit integer types)
+		cv::cuda::subtract(mean2, mean, out.getGpuMatRef()); // E[X^2] - (E[X])^2
+	}
+	else { throw std::exception(); /* todo CPU version */ }
+}
 
 struct StereoHierCensusSgm::Impl : public StereoSgm<MatchingCost, StereoHierCensusSgm::Parameters> {
     CensusMatchingCost cost_fine;
@@ -12,16 +44,22 @@ struct StereoHierCensusSgm::Impl : public StereoSgm<MatchingCost, StereoHierCens
     CensusMatchingCost cost_coarse;
 	Array2D<uchar> l;
     Array2D<uchar> r;
+    Array2D<float> var_fine;
+    Array2D<float> var_medium;
+    Array2D<float> var_coarse;
 
 	Impl(StereoHierCensusSgm::Parameters &params, int width, int height, int dmin, int dmax) :
         StereoSgm(params, width, height, dmin, dmax),
         cost_fine(width, height, dmin, dmax),
         cost_medium(width, height, dmin, dmax),
         cost_coarse(width, height, dmin, dmax),
-        l(width, height), r(width, height) {
-            cost.add(0, cost_fine);
-            cost.add(1, cost_medium);
-            cost.add(2, cost_coarse);
+        l(width, height), r(width, height),
+        var_fine(width, height),
+        var_medium(width, height),
+        var_coarse(width, height) {
+            cost.add(0, cost_fine, var_fine);
+            cost.add(1, cost_medium, var_medium);
+            cost.add(2, cost_coarse, var_coarse);
         }
 };
 
@@ -42,8 +80,8 @@ void StereoHierCensusSgm::compute(cv::InputArray l, cv::InputArray r, cv::Output
 	mat2gray(r, impl_->r);
     timer_set();
 
-    static constexpr int DOWNSCALE_MEDIUM = 2;
-    static constexpr int DOWNSCALE_COARSE = 4;
+    static constexpr int DOWNSCALE_MEDIUM = 4;
+    static constexpr int DOWNSCALE_COARSE = 6;
     
     Array2D<uchar> medium_l(l.cols()/DOWNSCALE_MEDIUM, l.rows()/DOWNSCALE_MEDIUM);
     Array2D<uchar> medium_r(r.cols()/DOWNSCALE_MEDIUM, r.rows()/DOWNSCALE_MEDIUM);
@@ -54,6 +92,18 @@ void StereoHierCensusSgm::compute(cv::InputArray l, cv::InputArray r, cv::Output
     cv::cuda::resize(impl_->l.toGpuMat(), coarse_l.toGpuMat(), cv::Size(coarse_l.width, coarse_l.height));
     cv::cuda::resize(impl_->r.toGpuMat(), coarse_r.toGpuMat(), cv::Size(coarse_r.width, coarse_r.height));
 
+    cv::cuda::GpuMat var_fine = impl_->var_fine.toGpuMat();
+    variance_mask(impl_->l.toGpuMat(), var_fine, params.var_window);
+    cv::cuda::normalize(var_fine, var_fine, params.alpha, params.beta, cv::NORM_MINMAX, -1);
+
+    cv::cuda::GpuMat var_medium = impl_->var_medium.toGpuMat();
+    variance_mask(medium_l.toGpuMat(), var_medium, params.var_window);
+    cv::cuda::normalize(var_medium, var_medium, params.alpha, params.beta, cv::NORM_MINMAX, -1);
+
+    cv::cuda::GpuMat var_coarse = impl_->var_coarse.toGpuMat();
+    variance_mask(coarse_l.toGpuMat(), var_coarse, params.var_window);
+    cv::cuda::normalize(var_coarse, var_coarse, params.alpha, params.beta, cv::NORM_MINMAX, -1);
+
 	// CT
     impl_->cost_fine.set(impl_->l, impl_->r);
     impl_->cost_medium.set(impl_->l, impl_->r, medium_l, medium_r);

lib/libstereo/src/costs/dual.hpp

@@ -44,6 +44,29 @@ namespace impl {
 		static constexpr Type COST_MAX = A::COST_MAX * N;
 	};
 
+	template <typename A, int N>
+	struct MultiCostsWeighted : DSImplBase<typename A::Type> {
+		typedef typename A::Type Type;
+
+		MultiCostsWeighted(ushort w, ushort h, ushort dmin, ushort dmax) : DSImplBase<Type>({w,h,dmin,dmax}) {}
+
+		__host__ __device__ inline Type operator()(const int y, const int x, const int d) const {
+			float cost = 0;
+			float pw = 1.0f;
+			#pragma unroll
+			for (int n=0; n<N; ++n) {
+				float w = 1.0f-weights[n](y,x);
+				cost += pw*w*float(costs[n](y,x,d));
+				pw *= 1.0f-w;
+			}
+			return round(cost);
+		}
+
+		A costs[N];
+		Array2D<float>::Data weights[N];
+		static constexpr Type COST_MAX = A::COST_MAX;
+	};
+
 	template <typename A, typename B>
 	struct DualCostsWeighted : DSImplBase<typename A::Type> {
 		static_assert(std::is_same<typename A::Type, typename B::Type>::value, "Cost types must be the same");
@@ -115,6 +138,35 @@ protected:
 	A *costs[N];
 };
 
+template <typename A, int N>
+class MultiCostsWeighted : public DSBase<impl::MultiCostsWeighted<typename A::DataType,N>> {
+public:
+	typedef impl::MultiCostsWeighted<typename A::DataType,N> DataType;
+	typedef typename A::DataType::Type Type;
+
+	MultiCostsWeighted(int width, int height, int disp_min, int disp_max)
+		: DSBase<DataType>(width, height, disp_min, disp_max) {};
+
+	void add(int n, A &c, Array2D<float> &w) {
+		if (n >= N || n < 0) throw std::exception();
+		costs[n] = &c;
+		weights[n] = &w;
+	}
+
+	void set() {
+		for (int n=0; n<N; ++n) {
+			this->data().costs[n] = costs[n]->data();
+			this->data().weights[n] = weights[n]->data();
+		}
+	}
+
+	static constexpr Type COST_MAX = A::COST_MAX;
+
+protected:
+	A *costs[N];
+	Array2D<float> *weights[N];
+};
+
 /**
  * Combine two cost functions with weight. Weight in Array2D<float> same size
  * as input. Weight values assumed in range [0,1] and cost is calculated as