From 6defcad11b3a554dbd2eea04027eb91a0aa78ca0 Mon Sep 17 00:00:00 2001
From: uvos <devnull@uvos.xyz>
Date: Thu, 22 Oct 2020 11:21:12 +0200
Subject: [PATCH] split into many files add better outlier rejection add
 normalization add background removal

---
 CMakeLists.txt |   4 +-
 argpopt.h      |  21 +-
 bgremoval.cpp  |  21 ++
 bgremoval.h    |   5 +
 drawing.cpp    |  19 ++
 drawing.h      |   6 +
 main.cpp       | 518 ++++++++---------------------------------------
 matutils.cpp   | 166 +++++++++++++++
 matutils.h     |  22 ++
 normalize.h    |  29 +++
 unwrap.cpp     | 532 +++++++++++++++++++++++++++++++++++++++++++++++++
 unwrap.h       |   7 +
 12 files changed, 904 insertions(+), 446 deletions(-)
 create mode 100644 bgremoval.cpp
 create mode 100644 bgremoval.h
 create mode 100644 drawing.cpp
 create mode 100644 drawing.h
 create mode 100644 matutils.cpp
 create mode 100644 matutils.h
 create mode 100644 normalize.h
 create mode 100644 unwrap.cpp
 create mode 100644 unwrap.h

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 95de801..a43c95e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -2,14 +2,14 @@ cmake_minimum_required(VERSION 2.4)
 
 project(unwap)
 
-set(SRC_FILES main.cpp )
+set(SRC_FILES main.cpp unwrap.cpp drawing.cpp matutils.cpp bgremoval.cpp)
 set(LIBS -lX11 -lrt)
 
 find_package( OpenCV REQUIRED )
 
 add_executable(${PROJECT_NAME} ${SRC_FILES})
 
-target_link_libraries( ${PROJECT_NAME} ${LIBS}  -lopencv_core -lopencv_imgcodecs -lopencv_highgui -lopencv_features2d -lopencv_imgcodecs -lopencv_imgproc )
+target_link_libraries( ${PROJECT_NAME} ${LIBS}  -lopencv_core -lopencv_imgcodecs -lopencv_highgui -lopencv_features2d -lopencv_imgcodecs -lopencv_imgproc -lopencv_video)
 target_include_directories(${PROJECT_NAME} PRIVATE  "/usr/include/opencv4")
 add_definitions(" -std=c++17 -Wall -O2 -flto -fno-strict-aliasing")
 set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -s")
diff --git a/argpopt.h b/argpopt.h
index 9c1e83f..e4e1080 100644
--- a/argpopt.h
+++ b/argpopt.h
@@ -24,7 +24,9 @@
 struct Config
 {
 	char** inFileNames = NULL;
-	std::string outputFileName = "out.png";
+	std::string norm = "";
+	std::string maps = "";
+	std::string bg = "";
 	bool verbose = false;
 };
 
@@ -36,7 +38,9 @@ static char args_doc[] = "";
 static struct argp_option options[] = 
 {
 {"verbose",  'v', 0,      0,  "Enable verbose logging" },
-{"output", 'o', "File Name",      0,  "Output image file name" },
+{"map", 'm', "File Name",      0,  "remap maps file" },
+{"bg", 'b', "File Name",      0,  "background image file" },
+{"normalize", 'n', "File Name",      0,  "image to use as a normalization source" },
 { 0 }
 };
 
@@ -47,10 +51,16 @@ error_t parse_opt (int key, char *arg, struct argp_state *state)
 	switch (key)
 	{
 		case 'v':
-			config->verbose = false;
+			config->verbose = true;
 		break;
-		case 'o':
-			config->outputFileName.assign(arg);
+		case 'm':
+			config->maps.assign(arg);
+		break;
+		case 'b':
+			config->bg.assign(arg);
+		break;
+		case 'n':
+			config->norm.assign(arg);
 		break;
 		case ARGP_KEY_NO_ARGS:
 			argp_usage(state);
@@ -66,3 +76,4 @@ error_t parse_opt (int key, char *arg, struct argp_state *state)
 static struct argp argp = { options, parse_opt, args_doc, doc };
 
 
+
diff --git a/bgremoval.cpp b/bgremoval.cpp
new file mode 100644
index 0000000..02af5d9
--- /dev/null
+++ b/bgremoval.cpp
@@ -0,0 +1,21 @@
+#include "bgremoval.h"
+#include <iostream>
+#include <opencv2/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/bgsegm.hpp>
+
+bool createMask(const cv::Mat& in, cv::Mat& mask, const cv::Mat& bg)
+{
+	if(in.size != bg.size || in.type() != bg.type())
+	{
+		std::cerr<<"input image and backgournd image size and type needs to be the same\n";
+		return false;
+	}
+	
+	cv::Ptr<cv::BackgroundSubtractorMOG2> bgremv = cv::createBackgroundSubtractorMOG2(2,10,false);
+	bgremv->apply(bg, mask, 1);
+	bgremv->apply(in, mask, 0);
+	cv::GaussianBlur(mask,mask,cv::Size(49,49), 15);
+	cv::threshold(mask, mask, 70, 255, cv::THRESH_BINARY);
+	return true;
+}
diff --git a/bgremoval.h b/bgremoval.h
new file mode 100644
index 0000000..516791e
--- /dev/null
+++ b/bgremoval.h
@@ -0,0 +1,5 @@
+#pragma once
+
+#include <opencv2/core/ocl.hpp>
+
+bool createMask(const cv::Mat& in, cv::Mat& mask, const cv::Mat& bg);
diff --git a/drawing.cpp b/drawing.cpp
new file mode 100644
index 0000000..b8b0090
--- /dev/null
+++ b/drawing.cpp
@@ -0,0 +1,19 @@
+#include "drawing.h"
+#include <opencv2/viz/types.hpp>
+#include <opencv2/imgproc.hpp>
+
+void drawRows(cv::Mat& image, const std::vector< std::vector<cv::Point2f > >& rows)
+{
+	for(size_t i = 0; i < rows.size(); ++i)
+	{
+		for(size_t y = 0; y < rows[i].size(); ++y)
+		{
+			cv::circle(image, rows[i][y], 5, cv::viz::Color(128 * (i%3), 128 * ((i+1)%3), 128 * ((i+2)%3)));
+		}
+	}
+}
+
+void drawEllipses(cv::Mat& image, const std::vector<cv::RotatedRect>& ellipses )
+{
+	 for(const auto& ellipse : ellipses)cv::ellipse(image, ellipse, cv::viz::Color(128,128,128));
+}
diff --git a/drawing.h b/drawing.h
new file mode 100644
index 0000000..b06e0d0
--- /dev/null
+++ b/drawing.h
@@ -0,0 +1,6 @@
+#pragma once
+#include <opencv2/core/ocl.hpp>
+
+void drawRows(cv::Mat& image, const std::vector< std::vector<cv::Point2f > >& rows);
+
+void drawEllipses(cv::Mat& image, const std::vector<cv::RotatedRect>& ellipses );
diff --git a/main.cpp b/main.cpp
index 2a7b0af..be58467 100644
--- a/main.cpp
+++ b/main.cpp
@@ -1,19 +1,13 @@
 #include <iostream>
 #include <opencv2/highgui.hpp>
-#include <opencv2/imgproc.hpp>
 #include <opencv2/core/ocl.hpp>
-#include <opencv2/features2d.hpp>
-#include <opencv2/viz/types.hpp>
-#include <math.h>
 #include <unistd.h>
 #include <vector>
-#include <string>
-#include <sstream>
-#include <algorithm> 
-
+#include <string> 
 #include "argpopt.h"
-
-bool verbose = false;
+#include "unwrap.h"
+#include "bgremoval.h"
+#include "normalize.h"
 
 void cd_to_exe_dir( char *argv[] )
 {
@@ -39,403 +33,6 @@ std::vector<cv::Mat> loadImages(char** fileNames)
 	return images;
 }
 
-
-std::vector< std::vector<cv::Point2f > > sortPointsIntoRows(std::vector<cv::Point2f>& points)
-{
-	
-	if(points.size() < 6) return std::vector< std::vector<cv::Point2f> >();
-	
-	struct 
-	{
-		bool operator()(const cv::Point2f& a, const cv::Point2f& b) const
-		{   
-			return sqrt(a.y*a.y+a.x*a.x) < sqrt(b.y*b.y+b.x*b.x);
-		}   
-	} lessDist;
-	
-	std::vector<cv::Point2f>::iterator topLeftIt = std::min_element(points.begin(), points.end(), lessDist);
-	std::vector<cv::Point2f>::iterator bottomRightIt = std::max_element(points.begin(), points.end(), lessDist);
-	
-	cv::Point2f topLeft(topLeftIt[0]);
-	cv::Point2f bottomRight(bottomRightIt[0]);
-	
-	std::cout<<"topLeft "<<topLeft.x<<' '<<topLeft.y<<'\n';
-	std::cout<<"bottomRight "<<bottomRight.x<<' '<<bottomRight.y<<'\n';
-	
-	float fuzz = (bottomRight.x-topLeft.x)*0.01f;
-	
-	for(size_t i = 0; i < points.size(); ++i)
-	{
-		if(points[i].x < topLeft.x-fuzz || points[i].y < topLeft.y-fuzz || 
-			points[i].x > bottomRight.x+fuzz || points[i].y > bottomRight.y+fuzz)
-			points.erase(points.begin()+i);
-	}
-	
-	std::sort(points.begin(), points.end(), [](const cv::Point2f& a, const cv::Point2f& b){return a.y < b.y;});
-	
-	double accumulator = points[0].y+points[1].y;
-	size_t accuCount = 2;
-	float sigDist = (bottomRight.y-topLeft.y) / 20;
-	
-	std::vector< std::vector<cv::Point2f> > result(1);
-	result.back().push_back(points[0]);
-	result.back().push_back(points[1]);
-	
-	for(size_t i = 2; i < points.size(); ++i)
-	{
-		if( points[i].y - accumulator/accuCount > sigDist )
-		{
-			if(points.size() - i - 3 < 0) break;
-			else 
-			{
-				accumulator = points[i+1].y+points[i+2].y;
-				accuCount = 2;
-			}
-			result.push_back(std::vector<cv::Point2f>());
-		}
-		result.back().push_back(points[i]);
-		accumulator += points[i].y;
-		++accuCount;
-	}
-	
-	for(auto& row : result)  std::sort(row.begin(), row.end(), [](const cv::Point2f& a, const cv::Point2f& b){return a.x < b.x;});
-	
-	return result;
-}
-
-void drawRows(cv::Mat& image, const std::vector< std::vector<cv::Point2f > >& rows)
-{
-	for(size_t i = 0; i < rows.size(); ++i)
-	{
-		for(size_t y = 0; y < rows[i].size(); ++y)
-		{
-			cv::circle(image, rows[i][y], 5, cv::viz::Color(128 * (i%3), 128 * ((i+1)%3), 128 * ((i+2)%3)));
-		}
-	}
-}
-
-std::vector<cv::RotatedRect> fitEllipses(std::vector< std::vector<cv::Point2f > >& rows, bool remove = true)
-{
-	if(rows.empty()) return std::vector<cv::RotatedRect>();
-	
-	std::vector<cv::RotatedRect> ellipses;
-	ellipses.reserve(rows.size());
-	for(size_t i = 0; i < rows.size(); ++i) 
-	{
-		if(rows[i].size() > 4) ellipses.push_back(cv::fitEllipse(rows[i]));
-		else rows.erase(rows.begin()+i);
-	}
-	return ellipses;
-}
-
-void drawEllipses(cv::Mat& image, const std::vector<cv::RotatedRect>& ellipses )
-{
-	 for(const auto& ellipse : ellipses)cv::ellipse(image, ellipse, cv::viz::Color(128,128,128));
-}
-
-
-struct DisplacmentMap
-{
-	std::vector< std::vector<cv::Point2f> > destination;
-	std::vector< std::vector<cv::Point2f> > source;
-};
-
-DisplacmentMap calcDisplacementMap(const std::vector< std::vector<cv::Point2f > >& rows, 
-											   const std::vector<cv::RotatedRect>& elipses)
-{
-	if(rows.size() < 2 || rows.size() != elipses.size()) {
-		std::cerr<<__func__<<": rows < 2 or rows != elipses\n";
-		return DisplacmentMap();
-	}
-	
-	DisplacmentMap displacmentmap;
-	
-	displacmentmap.destination.assign(rows.size(), std::vector<cv::Point2f>());
-	displacmentmap.source = rows;
-	
-	for(int i = 0; i < displacmentmap.destination.size(); ++i)
-		displacmentmap.source[i].reserve(rows[i].size());
-	
-	
-	std::cout<<__func__<<": "<<elipses[1].center.y-elipses[0].center.y<<'\n';
-	
-	float meanYdist = 0;
-	size_t j = 0;
-	while(j < elipses.size()-1)
-	{
-		meanYdist += elipses[j+1].center.y-elipses[j].center.y;
-		++j;
-	}
-	meanYdist = meanYdist/elipses.size();
-	
-	std::cout<<__func__<<": meanYdist "<<meanYdist<<'\n';
-	
-	for(int rowCnt = 0; rowCnt < rows.size(); ++rowCnt)
-	{
-		const std::vector<cv::Point2f>& row = rows[rowCnt];
-		const cv::RotatedRect& elipse = elipses[rowCnt];
-				
-		cv::Rect_<float> boundingRect = elipse.boundingRect2f();
-		
-		std::cout<<__func__<<": Proc row "<<rowCnt<<'\n';
-		
-		for(size_t i = 0; i < row.size(); ++i)
-		{
-			float yDest = rowCnt*meanYdist;
-			
-			double normDist = ((row[i].x - boundingRect.x)/boundingRect.width-0.5)*2;
-			double tau = asin(normDist);
-			float xDest = (((2*tau)/M_PI)*500)+500;
-			std::cout<<__func__<<": normDist "<<normDist<<" tau "<<tau<<" xDest "<<xDest<<'\n';
-			displacmentmap.destination[rowCnt].push_back(cv::Point2f(xDest,yDest));
-		}
-	}
-	return displacmentmap;
-}
-
-float detimineXPitch(const std::vector<cv::Point2f>& row, float fuzz = 1.3f)
-{
-	std::vector<float> xRowDists;
-	for(size_t i = 0; i < row.size()-1; ++i)
-		xRowDists.push_back(abs(row[i+1].x-row[i].x));
-	float xMinDist = *std::min(xRowDists.begin(), xRowDists.end());
-	
-	std::cout<<__func__<<": xMinDist "<<xMinDist<<'\n';
-	
-	float meanXDistAccum = 0;
-	size_t validCount = 0;
-	for(size_t i = 0; i < xRowDists.size(); ++i)
-	{
-		if(xRowDists[i] < xMinDist*fuzz)
-		{
-			++validCount;
-			meanXDistAccum+=xRowDists[i];
-		}
-	}
-	return meanXDistAccum/validCount;
-}
-
-
-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][0].y < point.y) ++rowBelow;
-	
-	if(rowBelow == array.size()) rowBelow = array.size()-1;
-	
-	int rightAbove = -1;
-	int 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].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;
-	}
-	return abs(array[yIndex][xIndex].x - point.x) < xTolerance && abs(array[yIndex][xIndex].y - point.y) < yTolerance;
-}
-
-
-void interpolateMissing(cv::Mat& mat)
-{
-	for(size_t 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(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)
-{
-	for(size_t y = 0; y < mat.rows; y++)
-	{
-		float* col = mat.ptr<float>(y);
-		for(int x = 0; x < mat.cols; ++x)
-		{
-			if(col[x] < 0) 
-			{
-				if(y > 0 && mat.at<float>(x,y-1) >= 0)
-					col[x] = mat.at<float>(x,y-1);
-				else if(y < mat.rows && mat.at<float>(x,y+1) >= 0)
-					col[x] = mat.at<float>(x,y+1);
-				if((x+1 < mat.cols && col[x] > col[x+1]) || (x > 0 && col[x] < col[x-1]))
-					col[x] = -1;
-			}
-		}
-	}
-}
-
-void sanityCheckMap(cv::Mat& mat, const float min, const float max)
-{
-	for(size_t 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] = min;
-			else if(col[x] > max)
-				col[x] = max;
-		}
-	}
-}
-
-//dst(x,y) =  src(map_x(x,y),map_y(x,y))
-void generateRemapMaps(const DisplacmentMap& map, cv::Mat& xMat, cv::Mat& yMat, const cv::Size& size, float fuzz = 1.3f)
-{
-	if(map.destination.size() < 2) 
-	{
-		std::cerr<<__func__<<": at least 2 rows are needed"<<std::endl;
-		return;
-	}
-	
-	float yMeanDist = map.destination[1][0].y-map.destination[0][0].y;
-	float xMeanDist = 0;
-	for(size_t i = 0; i < map.destination.size(); ++i)
-	{
-		xMeanDist+=detimineXPitch(map.destination[i]);
-	}
-	xMeanDist/=map.destination.size();
-	std::cout<<__func__<<": xMeanDist "<<xMeanDist<<'\n';
-	
-	float xMin = std::numeric_limits<float>::max();
-	float xMax = std::numeric_limits<float>::min();
-	
-	for(auto& row: map.destination )
-	{
-		if(xMin > row.front().x )
-			xMin = row.front().x;
-		if(xMax < row.back().x)
-			xMax = row.back().x;
-	}
-	
-	std::cout<<__func__<<": Grid: xMin "<<xMin<<'\n';
-	std::cout<<__func__<<": Grid: grid xMax "<<xMax<<'\n';
-	
-	size_t xGridSize = static_cast<size_t>(std::lround(abs((xMax-xMin)/xMeanDist))+1);
-	xMat = cv::Mat::zeros(cv::Size(xGridSize, map.destination.size()), CV_32FC1);
-	yMat = cv::Mat::zeros(cv::Size(xGridSize, map.destination.size()), CV_32FC1);
-	
-	std::cout<<"Grid: "<<xGridSize<<'x'<<map.destination.size()<<'\n';
-	
-	for(size_t y = 0; y < xMat.rows; y++)
-	{
-		float* colx = xMat.ptr<float>(y);
-		float* coly = yMat.ptr<float>(y);
-		for(int x = 0; x < xMat.cols; x++)
-		{
-			size_t xIndex;
-			size_t yIndex;
-			bool found = findClosest(xIndex, yIndex, 
-									 cv::Point2f((x+1)*xMeanDist, (y)*yMeanDist), 
-									 map.destination, xMeanDist/2, yMeanDist/2);
-			std::cout<<__func__<<": found: "<<found<<' '<<xIndex<<"x"<<yIndex<<'\n';
-			colx[x] = found ? map.source[yIndex][xIndex].x : -1;
-			coly[x] = found ? map.source[yIndex][xIndex].y : -1;
-		}
-	}
-	fillMissing(xMat);
-	interpolateMissing(xMat);
-	interpolateMissing(yMat);
-	std::cout<<__func__<<": xMat \n"<<xMat<<'\n';
-	std::cout<<__func__<<": yMat \n"<<yMat<<'\n';
-	resize(xMat, xMat, size);
-	resize(yMat, yMat, size);
-}
-
-std::vector<cv::Point2f> detectPoints(cv::Mat& image)
-{
-		cv::Mat gray;
-		cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);
-		
-		//detect corners
-		cv::Mat corners;
-		cv::cornerHarris(gray, corners, 5, 5, 0.01);
-		cv::normalize(corners, corners, 0, 255, cv::NORM_MINMAX, CV_32FC1, cv::Mat());
-		cv::convertScaleAbs( corners, corners );
-		cv::threshold(corners, corners, 40, 255, cv::THRESH_BINARY);
-		
-		cv::waitKey(0);
-		cv::imshow( "Viewer", corners );
-		
-		//get middle of corners
-		cv::SimpleBlobDetector::Params blobParams;
-		blobParams.filterByArea = true;
-		blobParams.minArea = 4;
-		blobParams.maxArea = 50;
-		blobParams.filterByColor = false;
-		blobParams.blobColor = 255;
-		blobParams.filterByInertia = false;
-		blobParams.filterByConvexity = false;
-		cv::Ptr<cv::SimpleBlobDetector> blobDetector = cv::SimpleBlobDetector::create(blobParams);
-		std::vector<cv::KeyPoint> keypoints;
-		blobDetector->detect(corners, keypoints);
-		
-		std::vector<cv::Point2f> points;
-		cv::KeyPoint::convert(keypoints, points);
-		
-		return points;
-}
-
 int main(int argc, char* argv[])
 {
 	cv::ocl::setUseOpenCL(false);
@@ -453,47 +50,90 @@ int main(int argc, char* argv[])
 		return -1;
 	}
 	
-	for(auto& image : inImages)
+	if(config.verbose) 
 	{
 		cv::namedWindow( "Viewer", cv::WINDOW_NORMAL );
 		cv::resizeWindow("Viewer", 960, 500);
-		cv::imshow( "Viewer", image );
-		std::vector<cv::Point2f > points = detectPoints(image);
-		
-		std::vector< std::vector<cv::Point2f > > rows = sortPointsIntoRows(points);
-		std::vector< cv::RotatedRect > ellipses = fitEllipses(rows);
-		
-		cv::Mat pointsMat = cv::Mat::zeros(image.size(), CV_8UC3);
-		
-		drawEllipses(pointsMat, ellipses);
-		
-		std::cout<<"rows: "<<rows.size()<<'\n';
-		
-		
-		DisplacmentMap dispMap = calcDisplacementMap(rows, ellipses);
-		
-		drawRows(pointsMat, dispMap.source);
-		
-		cv::waitKey(0);
-		cv::imshow( "Viewer", pointsMat );
-		
-		cv::Mat dispPointsDest = cv::Mat::zeros(cv::Size(cv::Size(1000,1000)), CV_8UC3);
-		drawRows(dispPointsDest, dispMap.destination);
-		
-		cv::waitKey(0);
-		cv::imshow( "Viewer", dispPointsDest );
+	}
+	
+	
+	if(config.maps.empty())
+	{
+		cv::Mat mask;
+		if(config.verbose)
+		{
+			cv::imshow( "Viewer", inImages[0] );
+			cv::waitKey(0);
+		}
+		if(!config.bg.empty())
+		{
+			cv::Mat bg = cv::imread(config.bg);
 		
+			if(bg.data)
+			{
+				createMask(inImages[0], mask, bg);
+				if(config.verbose)
+				{
+					cv::Mat masked;
+					inImages[0].copyTo(masked, mask);
+					cv::imshow( "Viewer", masked );
+					cv::waitKey(0);
+				}
+			}
+			else std::cout<<"can not read background image from "<<config.bg<<'\n';
+		}
+		createRemapMap(inImages[0], config.inFileNames[0], mask, config.verbose);
+	}
+	else 
+	{
+		cv::FileStorage fs(config.maps, cv::FileStorage::READ);
+		if (!fs.isOpened())
+		{
+			std::cerr<<"could not open maps file " <<config.maps<<'\n';
+			return -1;
+		}
 		cv::Mat xMat;
 		cv::Mat yMat;
-		generateRemapMaps(dispMap, xMat, yMat, cv::Size(1000,1000));
+		fs["xmat"]>>xMat;
+		fs["ymat"]>>yMat;
 		
-		cv::Mat remaped;
-		cv::remap(image, remaped,  xMat, yMat, cv::INTER_LINEAR);
-		cv::waitKey(0);
-		cv::imshow( "Viewer", remaped );
-		cv::waitKey(0);
+		cv::Mat norm;
+		if(!config.norm.empty())
+		{
+			cv::Mat tmp = cv::imread(config.norm);
+			if(!tmp.data)
+			{
+				std::cerr<<"could not open normalize file " <<config.norm<<'\n';
+			}
+			applyRemap(tmp, norm, xMat, yMat, cv::Size(900,1000));
+			if(config.verbose)
+			{
+				cv::imshow("Viewer", norm );
+				cv::waitKey(0);
+			}
+		}
+		
+		for(auto& image : inImages)
+		{
+			if(config.verbose)
+			{
+				cv::imshow( "Viewer", image );
+				cv::waitKey(0);
+			}
+			cv::Mat remaped;
+			applyRemap(image, remaped, xMat, yMat, cv::Size(900,1000));
+			if(norm.data) normalize(remaped, norm);
+			cv::imshow( "Viewer", remaped );
+			cv::waitKey(0);
+		}
+	}
+	
+	if(config.verbose)
+	{
 		cv::destroyWindow("Viewer");  
+		cv::waitKey(0);
 	}
 	
 	return 0;
 }
+
diff --git a/matutils.cpp b/matutils.cpp
new file mode 100644
index 0000000..7f7ba19
--- /dev/null
+++ b/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);
+}
+
+
diff --git a/matutils.h b/matutils.h
new file mode 100644
index 0000000..200a79e
--- /dev/null
+++ b/matutils.h
@@ -0,0 +1,22 @@
+#pragma once
+#include <opencv2/core/ocl.hpp>
+#include <vector>
+
+void sanityCheckMap(cv::Mat& mat, const float min, const float max, const float minValue, const float maxValue);
+
+std::vector<cv::Point2f>::iterator getTopLeft(std::vector<cv::Point2f>& points);
+
+std::vector<cv::Point2f>::iterator getBottomRight(std::vector<cv::Point2f>& points);
+
+double distance(const cv::Point2f& a, const cv::Point2f& b);
+
+bool findClosest(size_t& xIndex, size_t& yIndex, 
+				 cv::Point2f point, const std::vector< std::vector<cv::Point2f> >& array, 
+				 float xTolerance, float yTolerance);
+
+
+void interpolateMissing(cv::Mat& mat);
+
+void fillMissing(cv::Mat& mat);
+
+
diff --git a/normalize.h b/normalize.h
new file mode 100644
index 0000000..013a29c
--- /dev/null
+++ b/normalize.h
@@ -0,0 +1,29 @@
+#pragma once
+#include <opencv2/core/ocl.hpp>
+#include <opencv2/imgproc.hpp>
+#include <stdint.h>
+
+inline bool normalize(cv::Mat& image, const cv::Mat& referance)
+{
+	cv::Mat labReferance;
+	cv::cvtColor(referance, labReferance, cv::COLOR_BGR2Lab);
+	std::vector<cv::Mat> labPlanesRef(3);
+	cv::split(labReferance, labPlanesRef);
+
+	cv::Scalar mean = cv::mean(labPlanesRef[0]);
+	labPlanesRef[0].convertTo(labPlanesRef[0], CV_16SC1);
+	
+	labPlanesRef[0] = labPlanesRef[0] - cv::Scalar(mean);
+	
+	
+	cv::Mat labImage;
+	cv::cvtColor(image, labImage, cv::COLOR_BGR2Lab);
+	
+	std::vector<cv::Mat> labPlanes(3);
+	cv::split(labImage, labPlanes);
+	
+	cv::add(labPlanes[0], labPlanesRef[0], labPlanes[0], cv::noArray(), CV_8UC1);
+	cv::merge(labPlanes, labImage);
+	cv::cvtColor(labImage, image,  cv::COLOR_Lab2BGR);
+	return true;
+}
diff --git a/unwrap.cpp b/unwrap.cpp
new file mode 100644
index 0000000..ec42ba6
--- /dev/null
+++ b/unwrap.cpp
@@ -0,0 +1,532 @@
+#include "unwrap.h"
+
+#include <opencv2/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+#include <math.h>
+#include <vector>
+#include <algorithm>
+#include <opencv2/features2d.hpp>
+#include <iostream>
+
+#include "matutils.h"
+#include "drawing.h"
+
+
+struct DisplacmentMap
+{
+	std::vector< std::vector<cv::Point2f> > destination;
+	std::vector< std::vector<cv::Point2f> > source;
+};
+
+static std::vector< std::vector<cv::Point2f > > sortPointsIntoRows(std::vector<cv::Point2f>& points)
+{
+	
+	if(points.size() < 6) return std::vector< std::vector<cv::Point2f> >();
+	
+	cv::Point2f topLeft(*getTopLeft(points));
+	cv::Point2f bottomRight(*getBottomRight(points));
+	
+	std::cout<<"topLeft "<<topLeft.x<<' '<<topLeft.y<<'\n';
+	std::cout<<"bottomRight "<<bottomRight.x<<' '<<bottomRight.y<<'\n';
+	
+	float fuzz = (bottomRight.x-topLeft.x)*0.01f;
+	
+	for(size_t i = 0; i < points.size(); ++i)
+	{
+		if(points[i].x < topLeft.x-fuzz || points[i].y < topLeft.y-fuzz || 
+			points[i].x > bottomRight.x+fuzz || points[i].y > bottomRight.y+fuzz)
+			points.erase(points.begin()+i);
+	}
+	
+	std::sort(points.begin(), points.end(), [](const cv::Point2f& a, const cv::Point2f& b){return a.y < b.y;});
+	
+	double accumulator = points[0].y+points[1].y;
+	size_t accuCount = 2;
+	float sigDist = (bottomRight.y-topLeft.y) / 20;
+	
+	std::vector< std::vector<cv::Point2f> > result(1);
+	result.back().push_back(points[0]);
+	result.back().push_back(points[1]);
+	
+	for(size_t i = 2; i < points.size(); ++i)
+	{
+		if( points[i].y - accumulator/accuCount > sigDist )
+		{
+			if(points.size() - i - 3 < 0) break;
+			else 
+			{
+				accumulator = points[i+1].y+points[i+2].y;
+				accuCount = 2;
+			}
+			result.push_back(std::vector<cv::Point2f>());
+		}
+		result.back().push_back(points[i]);
+		accumulator += points[i].y;
+		++accuCount;
+	}
+	
+	for(auto& row : result)  std::sort(row.begin(), row.end(), [](const cv::Point2f& a, const cv::Point2f& b){return a.x < b.x;});
+	
+	return result;
+}
+
+
+static float detimineXPitch(const std::vector<cv::Point2f>& row, float fuzz = 1.3f)
+{
+	std::vector<float> xRowDists;
+	for(size_t i = 0; i < row.size()-1; ++i)
+		xRowDists.push_back(abs(row[i+1].x-row[i].x));
+	float xMinDist = *std::min(xRowDists.begin(), xRowDists.end());
+	
+	std::cout<<__func__<<": xMinDist "<<xMinDist<<'\n';
+	
+	float meanXDistAccum = 0;
+	size_t validCount = 0;
+	for(size_t i = 0; i < xRowDists.size(); ++i)
+	{
+		if(xRowDists[i] < xMinDist*fuzz)
+		{
+			++validCount;
+			meanXDistAccum+=xRowDists[i];
+		}
+	}
+	return meanXDistAccum/validCount;
+}
+
+
+static std::vector<cv::RotatedRect> fitEllipses(std::vector< std::vector<cv::Point2f > >& rows, bool remove = true)
+{
+	if(rows.empty()) return std::vector<cv::RotatedRect>();
+	
+	std::vector<cv::RotatedRect> ellipses;
+	ellipses.reserve(rows.size());
+	for(size_t i = 0; i < rows.size(); ++i) 
+	{
+		if(rows[i].size() > 4) ellipses.push_back(cv::fitEllipse(rows[i]));
+		else 
+		{ 
+			rows.erase(rows.begin()+i);
+			i--;
+		}
+	}
+	return ellipses;
+}
+
+static void thompsonTauTest(const std::vector<float>& in, std::vector<size_t>& outliers, float criticalValue)
+{
+	float mean = 0;
+	size_t n = 0;
+	for(size_t i = 0; i < in.size(); ++i)
+	{
+		bool found = false;
+		for(size_t j = 0; j < outliers.size() && !found; ++j) 
+			if(outliers[j]==i) found = true;
+		if(!found)
+		{
+			mean+=in[i];
+			++n;
+		}
+	}
+	mean/=n;
+	
+	float sd = 0;
+	for(size_t i = 0; i < in.size(); ++i)
+	{
+		bool found = false;
+		for(size_t j = 0; j < outliers.size() && !found; ++j) 
+			if(outliers[j]==i) found = true;
+		if(!found) sd+=pow(in[i]-mean,2);
+	}
+	sd = sqrt(sd/(n-1));
+	float rej = (criticalValue*(n-1))/(sqrt(n)*sqrt(n-2+criticalValue));
+	bool removed = false;
+	for(size_t i = 0; i < in.size() && !removed; ++i)
+	{
+		bool found = false;
+		for(size_t j = 0; j < outliers.size() && !found; ++j) 
+			if(outliers[j]==i) found = true;
+		if(!found)
+		{
+			if(abs((in[i]-mean)/sd) > rej)
+			{
+				std::cout<<__func__<<": "<<i<<" is outlier mean: "<<mean<<" sd: "<<sd<<" n: "<<n<<'\n';
+				outliers.push_back(i);
+				removed = true;
+			}
+		}
+	}
+	if(removed) thompsonTauTest(in, outliers, criticalValue);
+}
+
+static bool sanityCheckElipses(std::vector< std::vector<cv::Point2f > >& rows, std::vector<cv::RotatedRect>& elipses)
+{
+	if(rows.size() != elipses.size() && elipses.size() > 1) return false;
+
+	for(size_t i = 0; i < elipses.size(); ++i)
+	{
+		float angDiff = fmod(elipses[i].angle,90);
+		if(angDiff < 90-5 && angDiff > 5)
+		{
+			elipses.erase(elipses.begin()+i);
+			rows.erase(rows.begin()+i);
+			--i;
+		}
+	}
+	
+	std::vector<float> widths;
+	std::vector<float> heights;
+	
+	for(auto& elipse : elipses)
+	{
+		widths.push_back(elipse.size.width);
+		heights.push_back(elipse.size.height);
+	}
+	
+	std::vector<size_t> outliersW;
+	std::vector<size_t> outliersH;
+	thompsonTauTest(widths, outliersW, 2);
+	thompsonTauTest(heights, outliersH, 2);
+	
+	std::vector< std::vector<cv::Point2f > > rowsReduced;
+	std::vector<cv::RotatedRect> elipsesReduced;
+
+	for(size_t i = 0; i < elipses.size(); ++i)
+	{
+		bool found = false;
+		for(size_t j = 0; j < outliersW.size() && !found; ++j) 
+			if(outliersW[j]==i) found = true;
+		for(size_t j = 0; j < outliersH.size() && !found; ++j) 
+			if(outliersH[j]==i) found = true;
+		if(!found)
+		{
+			rowsReduced.push_back(rows[i]);
+			elipsesReduced.push_back(elipses[i]);
+		}
+	}
+	elipses = elipsesReduced;
+	rows = rowsReduced;
+	return true;
+}
+
+static DisplacmentMap calcDisplacementMap(const std::vector< std::vector<cv::Point2f > >& rows, 
+											   const std::vector<cv::RotatedRect>& elipses)
+{
+	if(rows.size() < 2 || rows.size() != elipses.size()) {
+		std::cerr<<__func__<<": rows < 2 or rows != elipses\n";
+		return DisplacmentMap();
+	}
+	
+	DisplacmentMap displacmentmap;
+	
+	displacmentmap.destination.assign(rows.size(), std::vector<cv::Point2f>());
+	displacmentmap.source = rows;
+	
+	for(size_t i = 0; i < displacmentmap.destination.size(); ++i)
+	{
+		displacmentmap.destination[i].reserve(rows[i].size());
+	}
+	
+	float meanYdist = 0;
+	size_t j = 0;
+	while(j < elipses.size()-1)
+	{
+		meanYdist += elipses[j+1].center.y-elipses[j].center.y;
+		++j;
+	}
+	meanYdist = meanYdist/elipses.size();
+	
+	std::cout<<__func__<<": meanYdist "<<meanYdist<<'\n';
+	
+	for(size_t rowCnt = 0; rowCnt < rows.size(); ++rowCnt)
+	{
+		const std::vector<cv::Point2f>& row = rows[rowCnt];
+		const cv::RotatedRect& elipse = elipses[rowCnt];
+				
+		cv::Rect_<float> boundingRect = elipse.boundingRect2f();
+		
+		std::cout<<__func__<<": Proc row "<<rowCnt<<'\n';
+		
+		for(size_t i = 0; i < row.size(); ++i)
+		{
+			float yDest = rowCnt*meanYdist;
+			
+			double normDist = ((row[i].x - boundingRect.x)/boundingRect.width-0.5)*2;
+			double tau = asin(normDist);
+			float xDest = (((2*tau)/M_PI)*500)+500;
+			std::cout<<__func__<<": normDist "<<normDist<<" tau "<<tau<<" xDest "<<xDest<<'\n';
+			displacmentmap.destination[rowCnt].push_back(cv::Point2f(xDest,yDest));
+		}
+	}
+	return displacmentmap;
+}
+
+static void removeSparseCollums(cv::Mat& mat)
+{
+	int front = 0;
+	int back = 0;
+	for(int y = 0; y < mat.rows; ++y)
+	{
+		if(mat.at<float>(y,0) >= 0) ++front;
+		if(mat.at<float>(y,mat.cols-1) >= 0) ++back;
+	}
+	std::cout<<__func__<<" front: "<<front<<" back "<<back<<'\n';
+	cv::Rect roi;
+	bool frontRej = (front < mat.rows/2);
+	bool backRej  = (back < mat.rows/2);
+	roi.x=frontRej;
+	roi.y=0;
+	roi.width = mat.cols - backRej - frontRej;
+	roi.height = mat.rows;
+	mat = mat(roi);
+	if(frontRej || backRej) removeSparseCollums(mat);
+}
+
+
+static void generateRemapMaps(const DisplacmentMap& map, cv::Mat& xMat, cv::Mat& yMat, float fuzz = 1.3f)
+{
+	if(map.destination.size() < 2) 
+	{
+		std::cerr<<__func__<<": at least 2 rows are needed"<<std::endl;
+		return;
+	}
+	
+	float yMeanDist = map.destination[1][0].y-map.destination[0][0].y;
+	float xMeanDist = 0;
+	for(size_t i = 0; i < map.destination.size(); ++i)
+	{
+		xMeanDist+=detimineXPitch(map.destination[i]);
+	}
+	xMeanDist/=map.destination.size();
+	std::cout<<__func__<<": xMeanDist "<<xMeanDist<<'\n';
+	
+	float xMin = std::numeric_limits<float>::max();
+	float xMeanMin = 0;
+	float xMax = std::numeric_limits<float>::min();
+	
+	for(auto& row: map.destination )
+	{
+		xMeanMin+=row.front().x;
+		if(xMin > row.front().x )
+			xMin = row.front().x;
+		if(xMax < row.back().x)
+			xMax = row.back().x;
+	}
+	xMeanMin = xMeanMin / map.destination.size();
+	
+	std::cout<<__func__<<": Grid: xMin "<<xMin<<'\n';
+	std::cout<<__func__<<": Grid: grid xMax "<<xMax<<'\n';
+	std::cout<<__func__<<": Grid: grid xMeanMin "<<xMeanMin<<'\n';
+	/*if(abs(xMeanMin-xMin) > (xMeanDist)/2)
+	{
+		std::cout<<__func__<<": Grid: xMin is outlier\n";
+		xMin = xMeanMin;
+	}*/
+	
+	size_t xGridSize = static_cast<size_t>(std::lround(abs((xMax-xMin)/xMeanDist))+1);
+	xMat = cv::Mat::zeros(cv::Size(xGridSize, map.destination.size()), CV_32FC1);
+	yMat = cv::Mat::zeros(cv::Size(xGridSize, map.destination.size()), CV_32FC1);
+	
+	std::cout<<"Grid: "<<xGridSize<<'x'<<map.destination.size()<<'\n';
+	
+	for(int y = 0; y < xMat.rows; y++)
+	{
+		float* colx = xMat.ptr<float>(y);
+		float* coly = yMat.ptr<float>(y);
+		for(int x = 0; x < xMat.cols; x++)
+		{
+			size_t xIndex = 0;
+			size_t yIndex = 0;
+			bool found = findClosest(xIndex, yIndex, 
+									 cv::Point2f((x)*xMeanDist+xMin, (y)*yMeanDist), 
+									 map.destination, (2*xMeanDist)/5, (2*yMeanDist)/5);
+			std::cout<<__func__<<": found: "<<found<<' '<<xIndex<<'x'<<yIndex<<" at: "<<(x)*xMeanDist+xMin<<'x'<<(y)*yMeanDist<<'\n';
+			colx[x] = found ? map.source[yIndex][xIndex].x : -1;
+			coly[x] = found ? map.source[yIndex][xIndex].y : -1;
+		}
+	}
+	std::cout<<__func__<<": xMat raw\n"<<xMat<<'\n';
+	removeSparseCollums(xMat);
+	removeSparseCollums(yMat);
+	std::cout<<__func__<<": xMat rejcted\n"<<xMat<<'\n';
+	fillMissing(xMat);
+	std::cout<<__func__<<": xMat filled\n"<<xMat<<'\n';
+	interpolateMissing(xMat);
+	std::cout<<__func__<<": yMat raw \n"<<yMat<<'\n';
+	interpolateMissing(yMat);
+	std::cout<<__func__<<": xMat \n"<<xMat<<'\n';
+	std::cout<<__func__<<": yMat \n"<<yMat<<'\n';
+}
+
+static std::vector<cv::Point2f> detectPoints(cv::Mat& image, const cv::Mat& mask, bool verbose = false)
+{
+	cv::Mat gray;
+	cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);
+	
+	//detect corners
+	cv::Mat corners;
+	cv::cornerHarris(gray, corners, 5, 5, 0.01);
+	cv::normalize(corners, corners, 0, 255, cv::NORM_MINMAX, CV_32FC1, cv::Mat());
+	cv::convertScaleAbs( corners, corners );
+	cv::threshold(corners, corners, 50, 255, cv::THRESH_BINARY);
+	cv::Mat cornersMasked;
+	if(mask.size == corners.size) corners.copyTo(cornersMasked, mask);
+	else corners.copyTo(cornersMasked);
+	
+	if(verbose)
+	{
+		cv::imshow( "Viewer", cornersMasked );
+		cv::waitKey(0);
+	}
+	
+	//get middle of corners
+	cv::SimpleBlobDetector::Params blobParams;
+	blobParams.filterByArea = true;
+	blobParams.minArea = 7;
+	blobParams.maxArea = 500;
+	blobParams.filterByColor = false;
+	blobParams.blobColor = 255;
+	blobParams.filterByInertia = false;
+	blobParams.filterByConvexity = false;
+	cv::Ptr<cv::SimpleBlobDetector> blobDetector = cv::SimpleBlobDetector::create(blobParams);
+	std::vector<cv::KeyPoint> keypoints;
+	blobDetector->detect(cornersMasked, keypoints);
+	
+	std::vector<cv::Point2f> points;
+	cv::KeyPoint::convert(keypoints, points);
+	
+	return points;
+}
+
+bool createRemapMap(cv::Mat& image, const std::string& fileName, const cv::Mat& mask,  bool verbose)
+{
+	std::vector<cv::Point2f > points = detectPoints(image, mask, verbose);
+	if(verbose) std::cout<<"Found "<<points.size()<<" points\n";
+	if(points.size() < 8) 
+	{
+		std::cout<<"Error creating map, insufficant points detected\n";
+		return false;
+	}
+
+	std::vector< std::vector<cv::Point2f > > rows = sortPointsIntoRows(points);
+	if(verbose) std::cout<<"Found "<<rows.size()<<" rows\n";
+	if(rows.size() < 2)
+	{
+		std::cout<<"Error creating map, insufficant rows detected\n";
+		return false;
+	}
+
+	std::vector< cv::RotatedRect > ellipses = fitEllipses(rows);
+	if(verbose) std::cout<<"Found "<<ellipses.size()<<" ellipses. rows reduced to "<<rows.size()<<'\n';
+	if(ellipses.size() < 3)
+	{
+		std::cout<<"Error creating map, insufficant ellipses detected\n";
+		return false;
+	}
+	
+	if(verbose)
+	{
+		cv::Mat pointsMat = cv::Mat::zeros(image.size(), CV_8UC3);
+		drawRows(pointsMat, rows);
+		drawEllipses(pointsMat, ellipses);
+		cv::imshow( "Viewer", pointsMat );
+		cv::waitKey(0);
+	}
+	
+	sanityCheckElipses(rows, ellipses);
+	
+	if(verbose)
+	{
+		cv::Mat pointsMat = cv::Mat::zeros(image.size(), CV_8UC3);
+		drawRows(pointsMat, rows);
+		drawEllipses(pointsMat, ellipses);
+		cv::imshow( "Viewer", pointsMat );
+		cv::waitKey(0);
+	}
+
+	DisplacmentMap dispMap = calcDisplacementMap(rows, ellipses);
+	if(dispMap.destination.size() < 2)
+	{
+		std::cout<<"Error creating map, unable to calculate destination points";
+		return false;
+	}
+	
+	cv::Mat dispPointsDest = cv::Mat::zeros(cv::Size(cv::Size(1000,1000)), CV_8UC3);
+	drawRows(dispPointsDest, dispMap.destination);
+	
+	if(verbose)
+	{
+		cv::imshow( "Viewer", dispPointsDest );
+		cv::waitKey(0);
+	}
+	
+	cv::Mat xMat;
+	cv::Mat yMat;
+	generateRemapMaps(dispMap, xMat, yMat);
+	sanityCheckMap(xMat, 0, image.cols-1, -1, -1);
+	sanityCheckMap(yMat, 0, image.rows-1, -1, -1);
+	fillMissing(xMat);
+	interpolateMissing(xMat);
+	interpolateMissing(yMat);
+	sanityCheckMap(xMat, 0, image.cols-1, 0, image.cols-1);
+	sanityCheckMap(yMat, 0, image.rows-1, 0, image.rows-1);
+	
+	cv::FileStorage matf(fileName+".mat", cv::FileStorage::WRITE );
+	matf<<"xmat"<<xMat<<"ymat"<<yMat;
+	matf.release();
+	
+	if(verbose)
+	{
+		cv::Mat remaped;
+		applyRemap(image, remaped, xMat, yMat, cv::Size(900,1000));
+		cv::imshow( "Viewer", remaped );
+		cv::waitKey(0);
+	}
+	return true;
+}
+
+bool findDeadSpace(const cv::Mat& mat, cv::Rect& roi)
+{
+	if(mat.cols < 2 || mat.rows < 2) return false;
+	
+	float centerTop = mat.at<float>(0,mat.cols/2);
+	float centerLeft = mat.at<float>(mat.rows/2,0);
+	float centerBottom = mat.at<float>(mat.rows-1,mat.cols/2);
+	float centerRight = mat.at<float>(mat.rows/2,mat.cols-1);
+	
+	int left = 0;
+	int right = mat.cols-1;
+	int top = 0;
+	int bottom = mat.rows-1;
+	for(; left < mat.cols && mat.at<float>(mat.rows/2,left) == centerLeft; ++left);
+	for(; top < mat.rows && mat.at<float>(top,mat.cols/2) == centerTop; ++top);
+	for(; right > left && mat.at<float>(mat.rows/2,right) == centerRight; --right);
+	for(; bottom > top && mat.at<float>(bottom,mat.cols/2) == centerBottom; --bottom);
+	
+	roi.x=left;
+	roi.y=top;
+	roi.width = right-left;
+	roi.height = bottom-top;
+	return true;
+}
+
+void applyRemap(cv::Mat& image, cv::Mat& out, const cv::Mat& xmap, const cv::Mat& ymap, cv::Size size)
+{
+	cv::Mat xMapResized;
+	cv::Mat yMapResized;
+	cv::resize(xmap, xMapResized, cv::Size(800,900), cv::INTER_CUBIC);
+	cv::resize(ymap, yMapResized, cv::Size(800,900), cv::INTER_CUBIC);
+	cv::Rect roi;
+	cv::Mat xMapRed;
+	cv::Mat yMapRed;
+	if(findDeadSpace(xMapResized, roi))
+	{
+		xMapRed = xMapResized(roi);
+		yMapRed = yMapResized(roi);
+	}
+	else
+	{
+		xMapRed = xMapResized;
+		yMapRed = yMapResized;
+	}
+	cv::remap(image, out,  xMapRed, yMapRed, cv::INTER_LINEAR);
+}
diff --git a/unwrap.h b/unwrap.h
new file mode 100644
index 0000000..15f9793
--- /dev/null
+++ b/unwrap.h
@@ -0,0 +1,7 @@
+#pragma once
+#include <opencv2/core/ocl.hpp>
+#include <string>
+
+bool createRemapMap(cv::Mat& image, const std::string& fileName, const cv::Mat& mask, bool verbose = false);
+
+void applyRemap(cv::Mat& image, cv::Mat& out, const cv::Mat& xmap, const cv::Mat& ymap, cv::Size size);