RTCensus attempt

cv-node/include/ftl/rtcensus.hpp

+#ifndef _FTL_RTCENSUS_HPP_
+#define _FTL_RTCENSUS_HPP_
+
+namespace ftl {
+class RTCensus {
+	public:
+	void disparity(cv::Mat &l, cv::Mat &r, cv::Mat &disp, size_t num_disp=32, float gamma=0.0f, float tau=0.0f);
+};
+};
+
+#endif // _FTL_RTCENSUS_HPP_
+

cv-node/src/rtcensus.cpp

+#include <ftl/rtcensus.hpp>
+#include <vector>
+
+using ftl::RTCensus;
+using std::vector;
+
+#define XHI(P1,P2) ((P1 <= P2) ? 0 : 1)
+
+static vector<int64_t> sparse_census_16x16(Mat &arr) {
+	vector<int64_t> result;
+	result.resize(arr.cols*arr.rows);
+	
+	auto N = [-7,-5,-3,-1,1,3,5,7];
+	auto M = [-7,-5,-3,-1,1,3,5,7];
+
+	for (size_t v=0; v<arr.cols; v++) {
+	for (size_t u=0; u<arr.rows; u++) {
+		int64_t r = 0;
+
+		for (auto n : N) {
+		for (auto m : M) {
+			auto u_ = u + n;
+			auto v_ = v + m;
+
+			if (u_ < 0 || v_ < 0 || u_ >= arr.rows || v_ >= arr.cols) continue;
+
+			r << 1;
+			r |= XHI(arr.at<uint8_t>(v,u), arr.at<uint8_t>(v_,u_));
+		}
+		}
+
+		result[v+u*arr.cols] = r;
+	}
+	}
+
+	return result;
+}
+
+static int hamming(int n1, int n2) { 
+    int x = n1 ^ n2; 
+    int setBits = 0; 
+  
+    while (x > 0) { 
+        setBits += x & 1; 
+        x >>= 1; 
+    } 
+  
+    return setBits; 
+}
+
+static vector<int64_t> dsi_ca(vector<int64_t> census_R, vector<int64_t> census_L, size_t w, size_t h, size_t d_start, size_t d_stop, int sign=1) {
+	// TODO Add asserts
+
+	auto ds = d_stop - d_start;
+	vector<int64_t> result;
+	result.resize(census_R.size()*ds);
+
+	//auto N = [-2,-1,0,1,2];
+	//auto M = [-2,-1,0,1,2];
+
+	for (size_t d=0; d<ds; d++) {
+		for (size_t v=0; v<w; v++) {
+		for (size_t u=0; u<h; u++) {
+			for (int n=-2; n<=2; n++) {
+			for (int n=-2; n<=2; n++) {
+				auto u_ = u + n;
+				auto v_ = v + m;
+				auto d_ = d * sign;
+
+				if (!(0<=(u_+d_) < h) continue;
+				if (!(0<=u_<h) || !(0<=v_<w)) continue;
+
+				auto r = census_R[v_+u_*w];
+				auto l = census_L[v_+(u_+d_)*w];
+				result[v+u*w+d*w*h];
+			}
+			}
+			
+		}
+		}
+	}
+
+	return result;
+}
+
+int64_t arrmin(vector<int64_t> a, size_t ix, size_t len) {
+	int64_t m = INT64_MAX;
+	for (size_t i=ix,i<ix+len; i++) {
+		if (a[i] < m) m = a[i];
+	}
+	return m;
+}
+
+cv::Mat d_sub(vector<int64_t> dsi, size_t w, size_t h, size_t ds) {
+	Mat result = Mat::zeros(Size(w,h), CV_64FC1);
+
+	for (size_t v=0; v<w; v++) {
+	for (size_t u=0; u<h; u++) {
+		auto d_min = arrmin(dsi, v+u*w, ds);
+		decltype(d_min) d_sub;
+
+		if (d_min == 0 || d_min == ds-1) d_sub = d_min;
+		else {
+			Point p(d_min, dsi[v+u*w+d_min*w*h]);
+			Point pl(d_min-1, dsi[v+u*w+(d_min-1)*w*h]);
+			Point pr(d_min+1, dsi[v+u*w+(d_min+1)*w*h]);
+
+			d_sub = fit_parabola(p,pl,pr);
+		}
+
+		result.at<uint8_t>(v,u) = d_sub;
+	}
+	}
+
+	return result;
+}
+
+void RTCensus::disparity(cv::Mat &l, cv::Mat &r, cv::Mat &disp, size_t num_disp, float gamma, float tau) {
+	size_t d_min = 0;
+	size_t d_max = num_disp;
+
+	auto census_R = sparse_census_16x16(l);
+	auto census_L = sparse_census_16x16(r);
+
+	auto dsi_ca_R = dsi_ca(census_R, census_L, l.cols, l.rows, d_min, d_max);
+	auto dsi_ca_L = dsi_ca(census_L, census_R, l.cols, l.rows, d_min, d_max, -1);
+
+	auto disp_R = d_sub(dsi_ca_R);
+	auto disp_L = d_sub(dsi_ca_L);
+
+	disp = consistency(disp_R, disp_L);
+
+	// TODO confidence and texture filtering
+}
+