split into many files

add better outlier rejection
add normalization
add background removal

matutils.cpp

@@ -0,0 +1,166 @@
+#include "matutils.h"
+#include <opencv2/core/ocl.hpp>
+#include <iostream>
+
+void sanityCheckMap(cv::Mat& mat, const float min, const float max, float minValue, float maxValue)
+{
+	for(int y = 0; y < mat.rows; y++)
+	{
+		float* col = mat.ptr<float>(y);
+		for(int x = 0; x < mat.cols; ++x)
+		{
+			if(col[x] < min) 
+				col[x] = minValue;
+			else if(col[x] > max)
+				col[x] = maxValue;
+		}
+	}
+}
+
+std::vector<cv::Point2f>::iterator getTopLeft(std::vector<cv::Point2f>& points)
+{
+	return std::min_element(points.begin(), points.end(), [](const cv::Point2f& a, const cv::Point2f& b){
+		return sqrt(a.y*a.y+a.x*a.x) < sqrt(b.y*b.y+b.x*b.x);
+	});
+}
+
+std::vector<cv::Point2f>::iterator getBottomRight(std::vector<cv::Point2f>& points)
+{
+	return std::max_element(points.begin(), points.end(), [](const cv::Point2f& a, const cv::Point2f& b){
+		return sqrt(a.y*a.y+a.x*a.x) < sqrt(b.y*b.y+b.x*b.x);
+	});
+}
+
+double distance(const cv::Point2f& a, const cv::Point2f& b)
+{
+	return sqrt((a.y-b.y)*(a.y-b.y)+(a.x-b.x)*(a.x-b.x));
+}
+
+bool findClosest(size_t& xIndex, size_t& yIndex, 
+				 cv::Point2f point, const std::vector< std::vector<cv::Point2f> >& array, 
+				 float xTolerance, float yTolerance)
+{	
+	size_t rowBelow = 0;
+	while(rowBelow < array.size() && !array[rowBelow].empty() && array[rowBelow][0].y < point.y) ++rowBelow;
+	
+	if(rowBelow == array.size()) rowBelow = array.size()-1;
+	
+	size_t rightAbove = 0;
+	size_t rightBelow = 0;
+	
+	while(rightBelow < array[rowBelow].size() && array[rowBelow][rightBelow].x < point.x ) ++rightBelow;
+	
+	float distRB = distance(point, array[rowBelow][rightBelow]);
+	float distLB = rightBelow > 0 ? distance(point, array[rowBelow][rightBelow-1]) : std::numeric_limits<float>::max();
+	float distRA = std::numeric_limits<float>::max();
+	float distLA = std::numeric_limits<float>::max();
+	
+	if(rowBelow > 0)
+	{
+		while(rightAbove < array[rowBelow-1].size() && array[rowBelow-1][rightAbove].x < point.x ) ++rightAbove;
+		distRA = distance(point, array[rowBelow-1][rightAbove]);
+		if(rightAbove > 0)  distLA = distance(point, array[rowBelow-1][rightAbove-1]);
+	}
+	
+	float* min = &distRB;
+	if(distLB < *min) min = &distLB;
+	if(distRA < *min) min = &distRA;
+	if(distLA < *min) min = &distLA;
+	
+	if(min == &distRB)
+	{
+		yIndex = rowBelow;
+		xIndex = rightBelow;
+	} 
+	else if(min == &distLB)
+	{
+		yIndex = rowBelow;
+		xIndex = rightBelow-1;
+	} 
+	else if(min == &distRA)
+	{
+		yIndex = rowBelow-1;
+		xIndex = rightBelow;
+	} 
+	else if(min == &distLA)
+	{
+		yIndex = rowBelow-1;
+		xIndex = rightBelow-1;
+	}
+	else return false;
+	return abs(array[yIndex][xIndex].x - point.x) < xTolerance && abs(array[yIndex][xIndex].y - point.y) < yTolerance;
+}
+
+
+void interpolateMissing(cv::Mat& mat)
+{
+	for(int y = 0; y < mat.rows; y++)
+	{
+		float* col = mat.ptr<float>(y);
+		for(int x = 0; x < mat.cols; ++x)
+		{
+			if(col[x] < 0) 
+			{
+				int closestA = -1;
+				int closestB = -1;
+				int dist = std::numeric_limits<int>::max();
+				for(int i = 0; i < mat.cols; ++i)
+				{
+					if(i != closestA && col[i] >= 0 && abs(i-x) <= dist)
+					{
+						closestB = closestA;
+						closestA = i;
+						dist = abs(i-x);
+					}
+				}
+				if(closestA < 0 || closestB < 0)
+				{
+					closestA = -1;
+					closestB = -1;
+					dist = std::numeric_limits<int>::max();
+					for(int i = mat.cols-1; i >= 0; --i)
+					{
+						if(i != closestA && col[i] >= 0 && abs(i-x) <= dist)
+						{
+							closestB = closestA;
+							closestA = i;
+							dist = abs(i-x);
+						}
+					}
+				}
+				float slope = (col[closestB] - col[closestA])/(closestB-closestA);
+				col[x] = col[closestA] - (closestA-x)*slope;
+			}
+		}
+	}
+}
+
+void fillMissing(cv::Mat& mat)
+{
+	bool finished = true;
+	for(int y = 0; y < mat.rows; y++)
+	{
+		float* col = mat.ptr<float>(y);
+		for(int x = 0; x < mat.cols; ++x)
+		{
+			if(col[x] < 0 && col[x] > -2) 
+			{
+				if(y > 0 && mat.at<float>(y-1,x) >= 0)
+				{
+					col[x] = mat.at<float>(y-1,x);
+					finished = false;
+				}
+				else if(y < mat.rows-1 && mat.at<float>(y+1,x) >= 0)
+				{
+					col[x] = mat.at<float>(y+1,x);
+					finished = false;
+				}
+				if(col[x] > 0 && ((x+1 < mat.cols && col[x] > col[x+1]) || (x > 0 && col[x] < col[x-1])))
+					col[x] = -2;
+			}
+		}
+	}
+	if(!finished) fillMissing(mat);
+}
+
+