Add variance filtering of depth

applications/registration/src/manual.cpp

@@ -128,7 +128,7 @@ static void build_correspondances(const vector<Source*> &sources, map<string, Co
 	}
 }
 
-void averageDepth(vector<Mat> &in, Mat &out) {
+static void averageDepth(vector<Mat> &in, Mat &out, float varThresh) {
 	const int rows = in[0].rows;
 	const int cols = in[0].cols;
 
@@ -138,16 +138,36 @@ void averageDepth(vector<Mat> &in, Mat &out) {
 	for (int i = 0; i < rows * cols; ++i) {
 		float sum = 0;
 		int good_values = 0;
+
+		// Calculate mean
 		for (int i_in = 0; i_in < in.size(); ++i_in) {
 			float d = in[i_in].at<float>(i);
-			if (d < 40) {
+			if (d < 40.0f) {
 				good_values++;
 				sum += d;
 			}
 		}
 
 		if (good_values > 0) {
-			out.at<float>(i) = sum / (float) good_values;
+			sum /= (float)good_values;
+
+			// Calculate variance
+			float var = 0.0f;
+			for (int i_in = 0; i_in < in.size(); ++i_in) {
+				float d = in[i_in].at<float>(i);
+				if (d < 40.0f) {
+					float delta = d - sum;
+					var += delta*delta;
+				}
+			}
+			if (good_values > 1) var /= (good_values-1);
+			else var = 0.0f;
+
+			if (var < varThresh) {
+				out.at<float>(i) = sum;
+			} else {
+				out.at<float>(i) = 41.0f;
+			}
 		} else {
 			out.at<float>(i) = 41.0f;
 		}
@@ -368,8 +388,8 @@ void ftl::registration::manual(ftl::Configurable *root) {
 
 				std::this_thread::sleep_for(std::chrono::milliseconds(20));
 			}
-			averageDepth(buffer[0], d1);
-			averageDepth(buffer[1], d2);
+			averageDepth(buffer[0], d1, 1.0f);
+			averageDepth(buffer[1], d2, 1.0f);
 			Mat d1_v, d2_v;
 			d1.convertTo(d1_v, CV_8U, 255.0f / 10.0f);
 			d2.convertTo(d2_v, CV_8U, 255.0f / 10.0f);