add charuco point detection method

CMakeLists.txt

@@ -2,14 +2,14 @@ cmake_minimum_required(VERSION 2.4)
 
 project(unwap)
 
-set(SRC_FILES main.cpp unwrap.cpp drawing.cpp matutils.cpp bgremoval.cpp)
+set(SRC_FILES main.cpp unwrap.cpp drawing.cpp matutils.cpp bgremoval.cpp charuco.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 -lopencv_video)
+target_link_libraries( ${PROJECT_NAME} ${LIBS}  -lopencv_core -lopencv_aruco -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")

argpopt.h

@@ -27,10 +27,10 @@ struct Config
 	std::string norm = "";
 	std::string maps = "";
 	std::string bg = "";
-	int blockSize = 5;
+	std::string charuco = "";
-	int apature = 5; 
+	bool harris = false;
-	float detectorParameter = 0.01; 
 	float minSize = 7; 
+	unsigned int size = 600;
 	bool verbose = false;
 	bool quiet = false;
 };
@@ -47,10 +47,10 @@ static struct argp_option options[] =
 {"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" },
-{"apature", 'a', "Value",      0,  "Sobel size" },
-{"block-size", 't', "Value",      0,  "Harris neighborhood size " },
-{"detector-arameter", 'd', "Value",      0,  "Harris detector parameter" },
 {"min-size", 's', "Value",      0,  "Smallest feature accepted as a corner" },
+{"create", 'c', "File Name",      0,  "Create charuco board" },
+{"x-size", 'x', "Value",      0,  "Output image width" },
+{"harris", 'r', 0,      0,  "Use harris to detect points" },
 { 0 }
 };
 
@@ -75,22 +75,18 @@ error_t parse_opt (int key, char *arg, struct argp_state *state)
 		case 'n':
 			config->norm.assign(arg);
 		break;
-		case 'a':
-			config->apature=atol(arg);
-			if(config->apature % 2 == 0) ++config->apature;
-		break;
-		case 't':
-			config->blockSize=atol(arg);
-			if(config->blockSize % 2 == 0) ++config->blockSize;
-		break;
-		case 'd':
-			config->detectorParameter=atol(arg);
-		break;
 		case 's':
 			config->minSize=atol(arg);
 		break;
-		case ARGP_KEY_NO_ARGS:
+		case 'r':
-			argp_usage(state);
+			config->harris = true;
+		break;
+		case 'c':
+			config->charuco.assign(arg);
+		break;
+		case 'x':
+			config->size=atol(arg);
+		break;
 		case ARGP_KEY_ARG:
 			config->inFileNames = &state->argv[state->next-1];
 			state->next = state->argc;

charuco.cpp

@@ -0,0 +1,67 @@
+#include "charuco.h"
+#include <opencv2/aruco/charuco.hpp>
+#include <opencv2/highgui.hpp>
+
+static constexpr unsigned int X_BOARD_SIZE = 18;
+static constexpr unsigned int Y_BOARD_SIZE = 10;
+
+void createCharucoBoard(unsigned int size, const std::string& fileName)
+{
+    cv::Ptr<cv::aruco::CharucoBoard> board = 
+			cv::aruco::CharucoBoard::create(X_BOARD_SIZE, Y_BOARD_SIZE, 0.03f, 0.02f, 
+											cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_250 ));
+    cv::Mat charucoImage;
+    board->draw(cv::Size((size*18)/10, size), charucoImage, 0, 1);
+    cv::imwrite(fileName, charucoImage);
+}
+
+std::vector<cv::Point2f> detectCharucoPoints(cv::Mat image, std::vector<cv::Point2i>* coordiantes, bool verbose)
+{
+	cv::Ptr<cv::aruco::CharucoBoard> board = cv::aruco::CharucoBoard::create(X_BOARD_SIZE, Y_BOARD_SIZE, 0.03f, 0.02f, 
+												cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_250 ));
+	cv::Ptr<cv::aruco::DetectorParameters> params = cv::aruco::DetectorParameters::create();
+	params->adaptiveThreshWinSizeMin = 3;
+	params->adaptiveThreshWinSizeMax = 41;
+	params->adaptiveThreshWinSizeStep = 4;
+	params->perspectiveRemovePixelPerCell = 16;
+	params->perspectiveRemoveIgnoredMarginPerCell = 0.1;
+	params->polygonalApproxAccuracyRate = 0.05;
+	params->perspectiveRemoveIgnoredMarginPerCell = 0.1;
+	params->aprilTagMinClusterPixels = 3;
+	
+	std::vector<int> arucoIds;
+	std::vector<std::vector<cv::Point2f> > arucoCorners;
+	cv::aruco::detectMarkers(image, board->dictionary, arucoCorners, arucoIds, params);
+
+	
+	if(verbose)
+	{
+		cv::Mat debugImage;
+		image.copyTo(debugImage);
+		cv::aruco::drawDetectedMarkers(debugImage, arucoCorners, arucoIds);
+		cv::imshow( "Viewer", debugImage );
+		cv::waitKey(0);
+	}
+	
+	if (arucoIds.size() > 0) 
+	{
+		std::vector<cv::Point2f> charucoCorners;
+		std::vector<int> charucoIds;
+		cv::aruco::interpolateCornersCharuco(arucoCorners, arucoIds, image, board, charucoCorners, 
+												charucoIds, cv::noArray(), cv::noArray(), 0);
+		if(verbose) 
+		{
+			cv::Mat debugImage;
+			image.copyTo(debugImage);
+			cv::aruco::drawDetectedCornersCharuco(debugImage, charucoCorners, charucoIds, cv::Scalar(0, 255, 0));
+			cv::imshow("Viewer", debugImage);
+			cv::waitKey(0);
+		}
+		
+		if(coordiantes) for( auto id : charucoIds)
+			coordiantes->push_back(cv::Point2i(id % (X_BOARD_SIZE-1), (Y_BOARD_SIZE-2) - id/(X_BOARD_SIZE-1)));
+		
+		return charucoCorners;
+	}
+	return std::vector<cv::Point2f>();
+}

charuco.h

@@ -0,0 +1,8 @@
+#pragma once
+
+#include <string>
+#include <opencv2/core/ocl.hpp>
+
+void createCharucoBoard(unsigned int size, const std::string& fileName);
+
+std::vector<cv::Point2f> detectCharucoPoints(cv::Mat image, std::vector<cv::Point2i>* coordiantes = nullptr, bool verbose = true);

harris.h

@@ -0,0 +1,44 @@
+#include <opencv2/imgproc.hpp>
+#include <opencv2/features2d.hpp>
+
+static std::vector<cv::Point2f> harrisDetectPoints(cv::Mat& image, const cv::Mat& mask, 
+											 int blockSize = 5, int apature = 5, float detectorParameter = 0.01,
+											 float minSize = 7, bool verbose = false)
+{
+	cv::Mat gray;
+	cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);
+	
+	//detect corners
+	cv::Mat corners;
+	cv::cornerHarris(gray, corners, blockSize, apature, detectorParameter);
+	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.data && 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 = minSize;
+	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;
+}

main.cpp

@@ -9,6 +9,8 @@
 #include "bgremoval.h"
 #include "normalize.h"
 #include "log.h"
+#include "charuco.h"
+#include "harris.h"
 
 void cd_to_exe_dir( char *argv[] )
 {
@@ -37,7 +39,7 @@ std::vector<cv::Mat> loadImages(char** fileNames)
 int main(int argc, char* argv[])
 {
 	cv::ocl::setUseOpenCL(false);
-	std::cout<<"UVOS Optical lubricant thikness mapper "<<argp_program_version<<std::endl;
+	
 	cd_to_exe_dir(argv);
 	
 	Config config;
@@ -45,6 +47,15 @@ int main(int argc, char* argv[])
 	
 	Log::level = config.quiet ? Log::WARN : config.verbose ? Log::DEBUG : Log::INFO;
 	
+	Log(Log::INFO)<<"UVOS Optical lubricant thikness mapper "<<argp_program_version;
+	
+	if(!config.charuco.empty())
+	{
+		createCharucoBoard(config.size, config.charuco);
+		Log(Log::INFO)<<"exporting charuco map";
+		return 0;
+	}
+	
 	std::vector<cv::Mat> inImages = loadImages(config.inFileNames);
 	
 	if(inImages.empty()) 
@@ -59,7 +70,6 @@ int main(int argc, char* argv[])
 		cv::resizeWindow("Viewer", 960, 500);
 	}
 	
-	
 	if(config.maps.empty())
 	{
 		cv::Mat mask;
@@ -85,9 +95,26 @@ int main(int argc, char* argv[])
 			}
 			else Log(Log::WARN)<<"can not read background image from "<<config.bg;
 		}
-		createRemapMap(inImages[0], config.inFileNames[0], mask, 
+		std::vector<cv::Point2f > points;
-					   config.blockSize, config.apature, config.detectorParameter,
+		std::vector<cv::Point2i> coordiantes;
-					   config.minSize, config.verbose);
+		
+		if(config.harris)
+		{
+			points = harrisDetectPoints(inImages[0], mask);
+		}
+		else
+		{
+			points = detectCharucoPoints(inImages[0], &coordiantes);
+		}
+		
+		Log(Log::INFO)<<"Found "<<points.size()<<" points";
+		if(points.size() < 8) 
+		{
+			Log(Log::ERROR)<<"Error creating map, insufficant points detected";
+			return -1;
+		}
+		
+		createRemapMap(inImages[0], points, coordiantes, config.inFileNames[0], config.verbose);
 	}
 	else 
 	{
@@ -110,7 +137,7 @@ int main(int argc, char* argv[])
 			{
 				Log(Log::WARN)<<"could not open normalize file " <<config.norm;
 			}
-			applyRemap(tmp, norm, xMat, yMat, 500);
+			applyRemap(tmp, norm, xMat, yMat, config.size);
 			if(config.verbose)
 			{
 				cv::imshow("Viewer", norm );
@@ -126,7 +153,7 @@ int main(int argc, char* argv[])
 				cv::waitKey(0);
 			}
 			cv::Mat remaped;
-			applyRemap(image, remaped, xMat, yMat, 500);
+			applyRemap(image, remaped, xMat, yMat, config.size);
 			if(norm.data) normalize(remaped, norm);
 			cv::imshow( "Viewer", remaped );
 			cv::waitKey(0);

unwrap.cpp

@@ -5,7 +5,6 @@
 #include <math.h>
 #include <vector>
 #include <algorithm>
-#include <opencv2/features2d.hpp>
 #include <iostream>
 
 #include "matutils.h"
@@ -29,7 +28,7 @@ static std::vector< std::vector<cv::Point2f > > sortPointsIntoRows(std::vector<c
 	Log(Log::DEBUG)<<"topLeft "<<topLeft.x<<' '<<topLeft.y;
 	Log(Log::DEBUG)<<"bottomRight "<<bottomRight.x<<' '<<bottomRight.y;
 	
-	float fuzz = (bottomRight.x-topLeft.x)*0.01f;
+	float fuzz = (bottomRight.x-topLeft.x)*0.05f;
 	
 	for(size_t i = 0; i < points.size(); ++i)
 	{
@@ -352,115 +351,145 @@ static void generateRemapMaps(const DisplacmentMap& map, cv::Mat& xMat, cv::Mat&
 	Log(Log::INFO)<<__func__<<": yMat \n"<<yMat;
 }
 
-static std::vector<cv::Point2f> detectPoints(cv::Mat& image, const cv::Mat& mask, 
+static void generateRemapMaps(const std::vector<cv::Point2f>& points, const std::vector<cv::Point2i>& coordinates, cv::Mat& xMat, cv::Mat& yMat)
-											 int blockSize = 5, int apature = 5, float detectorParameter = 0.01,
-											 float minSize = 7, bool verbose = false)
 {
-	cv::Mat gray;
+	if(points.size() < 6 || coordinates.size() != points.size()) 
-	cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);
-	
-	//detect corners
-	cv::Mat corners;
-	cv::cornerHarris(gray, corners, blockSize, apature, detectorParameter);
-	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 );
+		Log(Log::ERROR)<<__func__<<": at least 6 points are needed";
-		cv::waitKey(0);
+		return;
 	}
 	
-	//get middle of corners
+	int xMin = std::numeric_limits<int>::max();
-	cv::SimpleBlobDetector::Params blobParams;
+	int xMax = std::numeric_limits<int>::min();
-	blobParams.filterByArea = true;
+	int yMin = std::numeric_limits<int>::max();
-	blobParams.minArea = minSize;
+	int yMax = std::numeric_limits<int>::min();
-	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;
+	for(auto& point : coordinates)
-	cv::KeyPoint::convert(keypoints, points);
+	{
+		Log(Log::DEBUG)<<"point: "<<point.x<<'x'<<point.y;
 		
-	return points;
+		if(point.x > xMax) xMax = point.x;
+		else if(point.x < xMin) xMin = point.x;
+		if(point.y > yMax) yMax = point.y;
+		else if(point.y < yMin) yMin = point.y;
+	}
+	
+	size_t xGridSize = xMax-xMin+1;
+	size_t yGridSize = yMax-yMin+1;
+	xMat = cv::Mat::zeros(cv::Size(xGridSize, yGridSize), CV_32FC1);
+	yMat = cv::Mat::zeros(cv::Size(xGridSize, yGridSize), CV_32FC1);
+	
+	Log(Log::DEBUG)<<"Grid: "<<xGridSize<<'x'<<yGridSize;
+	
+	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++)
+		{
+			colx[x] = -1;
+			coly[x] = -1;
+			for(size_t i = 0; i < coordinates.size(); ++i)
+			{
+				if(coordinates[i] == cv::Point2i(x+xMin,y+yMin))
+				{
+					colx[x] = points[i].x;
+					coly[x] = points[i].y;
+					break;
+				}
+			}
+				
+			
+		}
+	}
+	Log(Log::DEBUG)<<__func__<<": xMat raw\n"<<xMat;
+	removeSparseCollums(xMat);
+	removeSparseCollums(yMat);
+	Log(Log::DEBUG)<<__func__<<": xMat rejcted\n"<<xMat;
+	fillMissing(xMat);
+	Log(Log::DEBUG)<<__func__<<": xMat filled\n"<<xMat;
+	interpolateMissing(xMat);
+	Log(Log::DEBUG)<<__func__<<": yMat raw \n"<<yMat;
+	interpolateMissing(yMat);
+	Log(Log::INFO)<<__func__<<": xMat \n"<<xMat;
+	Log(Log::INFO)<<__func__<<": yMat \n"<<yMat;
 }
 
-bool createRemapMap(cv::Mat& image, const std::string& fileName, const cv::Mat& mask, 
+bool createRemapMap(cv::Mat& image, std::vector<cv::Point2f> points, const std::vector<cv::Point2i>& coordinates, 
-					int blockSize, int apature, float detectorParameter, float minSize,  
+					const std::string& fileName, bool verbose)
-					bool verbose)
 {
-	std::vector<cv::Point2f > points = detectPoints(image, mask, blockSize, apature, detectorParameter, minSize, verbose);
-	if(verbose) std::cout<<"Found "<<points.size()<<" points\n";
-	if(points.size() < 8) 
-	{
-		Log(Log::ERROR)<<"Error creating map, insufficant points detected";
-		return false;
-	}
-
-	std::vector< std::vector<cv::Point2f > > rows = sortPointsIntoRows(points);
-	if(verbose) std::cout<<"Found "<<rows.size()<<" rows\n";
-	if(rows.size() < 2)
-	{
-		Log(Log::ERROR)<<"Error creating map, insufficant rows detected";
-		return false;
-	}
-
-	std::vector< cv::RotatedRect > ellipses = fitEllipses(rows);
-	if(verbose) Log(Log::INFO)<<"Found "<<ellipses.size()<<" ellipses. rows reduced to "<<rows.size();
-	if(ellipses.size() < 3)
-	{
-		Log(Log::ERROR)<<"Error creating map, insufficant ellipses detected";
-		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)
-	{
-		Log(Log::ERROR)<<"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);
+	
+	if(coordinates.empty())
+	{
+		std::vector< std::vector<cv::Point2f > > rows = sortPointsIntoRows(points);
+		Log(Log::INFO)<<"Found "<<rows.size()<<" rows";
+		if(rows.size() < 2)
+		{
+			Log(Log::ERROR)<<"Error creating map, insufficant rows detected";
+			return false;
+		}
+		
+		if(verbose)
+		{
+			cv::Mat pointsMat = cv::Mat::zeros(image.size(), CV_8UC3);
+			drawRows(pointsMat, rows);
+			cv::imshow( "Viewer", pointsMat );
+			cv::waitKey(0);
+		}
+
+		std::vector< cv::RotatedRect > ellipses = fitEllipses(rows);
+		Log(Log::INFO)<<"Found "<<ellipses.size()<<" ellipses. rows reduced to "<<rows.size();
+		if(ellipses.size() < 3)
+		{
+			Log(Log::ERROR)<<"Error creating map, insufficant ellipses detected";
+			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)
+		{
+			Log(Log::ERROR)<<"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);
+		}
+		generateRemapMaps(dispMap, xMat, yMat);
+	}
+	else
+	{
+		generateRemapMaps(points, coordinates, xMat, yMat);
+	}
+	
 	sanityCheckMap(xMat, 0, image.cols-1, -1, -1);
 	sanityCheckMap(yMat, 0, image.rows-1, -1, -1);
 	fillMissing(xMat);
@@ -525,7 +554,7 @@ void applyRemap(cv::Mat& image, cv::Mat& out, const cv::Mat& xmap, const cv::Mat
 	cv::Rect roi;
 	cv::Mat xMapRed;
 	cv::Mat yMapRed;
-	if(findDeadSpace(xMapResized, roi))
+	if(!findDeadSpace(xMapResized, roi))
 	{
 		xMapRed = xMapResized(roi);
 		yMapRed = yMapResized(roi);

unwrap.h

@@ -2,8 +2,6 @@
 #include <opencv2/core/ocl.hpp>
 #include <string>
 
-bool createRemapMap(cv::Mat& image, const std::string& fileName, const cv::Mat& mask, 
+bool createRemapMap(cv::Mat& image, std::vector<cv::Point2f> points, const std::vector<cv::Point2i>& coordinates, const std::string& fileName, bool verbose = false);
-					int blockSize = 5, int apature = 5, float detectorParameter = 0.01, float minSize = 7,  
-					bool verbose = false);
 
 void applyRemap(cv::Mat& image, cv::Mat& out, const cv::Mat& xmap, const cv::Mat& ymap, unsigned int size);