Homography matrix를 이용한 planar rectification를 구현 예제

4개의 대응점을 이용하여 Homography matrix를 구한 후,  planar rectification를 구현해본 결과입니다.

1. 구현 알고리즘

2. 실행 결과

3. 전체 소스 코드

1. 구현 알고리즘

리차드 하틀리의 Multiple view geometry in computer vision의 4.1에 소개된 알고리즘을 구현했습니다.

여기에서 풀어야 하는 문제는 4개의 대응되는 점이 주어질 때, 

를 만족하는 매트릭스 H를 구하는 것입니다.

하나의 대응점에 대한 관계식인  를 다음처럼 풀어 적을 수 있습니다.

4개의 대응점에 관계식을 이용하면 Homograpy matrix H를 구할 수 있습니다.

이를 위해 대응점 4개에 대한 관계식을 다음처럼 하나로 모아야합니다.

H를 구하려면  매트릭스 A에 대한  SVD를 구해야 합니다. 

매트릭스 VT를 전치(Transpose)하여 매트릭스 V 를 구한 후..   

매트릭스 V의 오른쪽 끝에 있는 열백터를 Homograpy matrix로 사용하되 

이 부분에 대한 증명은 아래 블로그의 글을 참고하세요.

http://beanexpert.tistory.com/entry/Singular-value-decompositionSVD

원소 값들을 열백터의 마지막 값으로 나누어 주어야 합니다.

그러면 아래와 같이 h33=1인  Homograpy matrix를 구할 수 있습니다.

다음처럼 forward mapping을 적용하여 결과 이미지를 구하면 

(원본 이미지의 좌표를 입력으로 사용하는 것을 의미합니다.)

매핑이 되지 못한 빈공간이 생겨서  오른쪽 영상처럼 검은색 부분들이 생기깁니다. 

backward mapping 또는 inverse mapping이라고 부르는 매핑을 적용해주어야 합니다.

원본 이미지 상의 좌표에 대응하는 결과 이미지 상의 좌표를 구하여 입력으로 사용합니다.

추가로 nomalized DLT 알고리즘 적용했는데 결과에 얼마나 영향을 주는지는 모르겠네요...

(원본과 결과 이미지가 동일 크기라고 가정했습니다.)

알고리즘 출처는 home.deib.polimi.it/caglioti/StimaParametriGeometrici.ppt

사용시 주의점은 왼쪽 위, 오른쪽 위, 왼쪽 아래, 오른쪽 아래 순으로 직사각형 영역을 클릭해야 한다는 점입니다... 

네 점을 클릭하고 나면 잠시 후 결과 이미지가 보이게 됩니다.   

제가 구현한 결과와 opencv에서 구현한 결과를 같이 보여줍니다. 

2. 실행 결과

2-1. 첫번째 테스트 이미지

구현 결과

OpenCV와 달리 결과 이미지의 일부만 보여주기 때문에 

결과 창의 크기를 얼마로 할지 고민이 필요한 듯합니다. 

OpenCV 결과

2-2. 두번째 테스트 이미지

구현 결과

OpenCV 결과

3. 전체 소스 코드

#include <opencv2/highgui.hpp>  
#include <opencv2/imgproc.hpp>  
#include <opencv2/calib3d/calib3d.hpp>
#include <iostream>  


using namespace cv;
using namespace std;



int count_mouse_click = 0;
double pointX[4], pointY[4];
int caculate_start = 0;
Mat img_gray;

//https://helloacm.com/cc-function-to-compute-the-bilinear-interpolation/
float BilinearInterpolation(float q11, float q12, float q21, float q22, float x1, float x2, float y1, float y2, float x, float y)
{
	float x2x1, y2y1, x2x, y2y, yy1, xx1;
	x2x1 = x2 - x1;
	y2y1 = y2 - y1;
	x2x = x2 - x;
	y2y = y2 - y;
	yy1 = y - y1;
	xx1 = x - x1;
	return 1.0 / (x2x1 * y2y1) * (
		q11 * x2x * y2y +
		q21 * xx1 * y2y +
		q12 * x2x * yy1 +
		q22 * xx1 * yy1
		);
}




void CallBackFunc(int event, int x, int y, int flags, void* userdata)
{
	if (event == EVENT_LBUTTONDOWN)
	{
		cout << count_mouse_click << "번째 왼쪽 마우스 버튼 클릭.. 좌표 = (" << x << ", " << y << ")" << endl;
		pointX[count_mouse_click] = x;
		pointY[count_mouse_click] = y;

		count_mouse_click++;
	}

	if (count_mouse_click == 4 && caculate_start == 0)
	{
		caculate_start = 1;

		cout << "#######################################################" << endl;
		cout << "H계산에 필요한 4개의 점을 모두 클릭했습니다." << endl << endl;


		double width = ((pointX[1] - pointX[0]) + (pointX[3] - pointX[2]))*0.5;
		double height = ((pointY[2] - pointY[0]) + (pointY[3] - pointY[1]))*0.5;

		double newpointX[4] = { pointX[3] - width, pointX[3], pointX[3] - width, pointX[3] };
		double newpointY[4] = { pointY[3] - height, pointY[3] - height, pointY[3], pointY[3] };

		for (int i = 0; i < 4; i++)
			cout << newpointX[i] << " " << newpointY[i] << endl;

		//inverse mapping
		Mat img_result = Mat::zeros(img_gray.size(), CV_8UC1);

		rectangle(img_result, Point(newpointX[0], newpointY[0]), Point(newpointX[3], newpointY[3]), Scalar(255), 1);
		imshow("img_gray image1", img_result);


		vector<Point2f> pts_src;
		vector<Point2f> pts_dst;

		for (int i = 0; i < 4; i++) {
			pts_src.push_back(Point2f(pointX[i], pointY[i]));
			pts_dst.push_back(Point2f(newpointX[i], newpointY[i]));
		}
		Mat h222 = findHomography(pts_src, pts_dst);

		cout << pts_src << endl;
		cout << pts_dst << endl;
		cout << h222 << endl;



		//nomalized DLT 알고리즘 적용.. 1/2  시작
		//여기에서는 원본과 결과 이미지가 동일 크기인경우임.
		//home.deib.polimi.it/caglioti/StimaParametriGeometrici.ppt
		int image_width = img_gray.cols;
		int image_height = img_gray.rows;

		Mat T_norm_old = Mat::zeros(3, 3, CV_64FC1);
		T_norm_old.at<double>(0, 0) = image_width + image_height;
		T_norm_old.at<double>(1, 1) = image_width + image_height;
		T_norm_old.at<double>(0, 2) = image_width * 0.5;
		T_norm_old.at<double>(1, 2) = image_height * 0.5;
		T_norm_old.at<double>(2, 2) = 1;

		Mat T_norm_new = Mat::zeros(3, 3, CV_64FC1);
		T_norm_new.at<double>(0, 0) = image_width + image_height;
		T_norm_new.at<double>(1, 1) = image_width + image_height;
		T_norm_new.at<double>(0, 2) = image_width * 0.5;
		T_norm_new.at<double>(1, 2) = image_height * 0.5;
		T_norm_new.at<double>(2, 2) = 1;

		for (int i = 0; i < 4; i++)
		{
			pointX[i] = (image_width + image_height)*pointX[i] + image_width*0.5;
			pointY[i] = (image_width + image_height)*pointY[i] + image_height*0.5;

			newpointX[i] = (image_width + image_height)*newpointX[i] + image_width*0.5;
			newpointY[i] = (image_width + image_height)*newpointY[i] + image_height*0.5;
		}
		///////////////////nomalized DLT 알고리즘 적용.. 1/2 끝


		double data[8][9] = {
			{ -1 * pointX[0], -1 * pointY[0], -1, 0, 0, 0,     pointX[0] * newpointX[0], pointY[0] * newpointX[0], newpointX[0] },
			{ 0, 0, 0, -1 * pointX[0], -1 * pointY[0], -1,   pointX[0] * newpointY[0], pointY[0] * newpointY[0], newpointY[0] },
			{ -1 * pointX[1], -1 * pointY[1], -1, 0, 0, 0,pointX[1] * newpointX[1], pointY[1] * newpointX[1], newpointX[1] },
			{ 0, 0, 0, -1 * pointX[1], -1 * pointY[1], -1,pointX[1] * newpointY[1], pointY[1] * newpointY[1], newpointY[1] },
			{ -1 * pointX[2], -1 * pointY[2], -1, 0, 0, 0,pointX[2] * newpointX[2], pointY[2] * newpointX[2], newpointX[2] },
			{ 0, 0, 0, -1 * pointX[2], -1 * pointY[2], -1,pointX[2] * newpointY[2], pointY[2] * newpointY[2], newpointY[2] },
			{ -1 * pointX[3], -1 * pointY[3], -1, 0, 0, 0,pointX[3] * newpointX[3], pointY[3] * newpointX[3], newpointX[3] },
			{ 0, 0, 0, -1 * pointX[3], -1 * pointY[3], -1,pointX[3] * newpointY[3], pointY[3] * newpointY[3], newpointY[3] },
		};
		Mat A(8, 9, CV_64FC1, data);
		cout << "Matrix A" << endl;
		cout << A << endl;

		Mat d, u, vt, v;
		SVD::compute(A, d, u, vt, SVD::FULL_UV);
		transpose(vt, v);

		cout << "Matrix V" << endl;
		cout << v << endl;


		Mat h(3, 3, CV_64FC1);

		//마지막 컬럼값을 H로 취한다. 
		int lrow = 0;
		int lcols = v.cols - 1;
		for (int i = 0; i < 3; i++) {
			for (int j = 0; j < 3; j++) {
				h.at<double>(i, j) = v.at<double>(lrow, lcols);
				lrow++;
			}
		}

		//h_33을 1로 만든다.
		double dw = h.at<double>(2, 2);
		h = h / dw;



		//nomalized DLT 알고리즘 적용.. 2/2 시작
		Mat T_norm_new_invert = T_norm_new.inv();
		h = T_norm_new_invert*h*T_norm_old;
		/////////////nomalized DLT 알고리즘 적용.. 2/2  끝



		for (int y = 0; y<img_result.rows; y++)
		{
			for (int x = 0; x<img_result.cols; x++)
			{
				double data[3] = { x, y,1 };
				Mat oldpoint(3, 1, CV_64FC1);
				Mat newpoint(3, 1, CV_64FC1, data);

				Mat h2 = h.inv();
				oldpoint = h2*newpoint;

				int oldX, oldY;

				oldX = (int)((oldpoint.at<double>(0, 0)) / (oldpoint.at<double>(2, 0)) + 0.5);
				oldY = (int)((oldpoint.at<double>(1, 0)) / (oldpoint.at<double>(2, 0)) + 0.5);


				if ((oldX >= 0 && oldY >= 0) && (oldX < img_result.cols && oldY < img_result.rows))
					img_result.at<uchar>(y, x) = img_gray.at<uchar>(oldY, oldX);
			}
		}



		//보간법 적용
		Mat img_result2 = Mat::zeros(img_gray.size(), CV_8UC1);

		for (int y = 1; y<img_result.rows - 1; y++)
		{
			for (int x = 1; x < img_result.cols - 1; x++)
			{
				int q11 = img_result.at<uchar>(y - 1, x - 1);
				int q12 = img_result.at<uchar>(y + 1, x - 1);
				int q21 = img_result.at<uchar>(y + 1, x + 1);
				int q22 = img_result.at<uchar>(y - 1, x + 1);

				if (img_result.at<uchar>(y, x) == 0)
				{
					int p = BilinearInterpolation(q11, q12, q21, q22, x - 1, x + 1, y - 1, y + 1, x, y);
					if (p > 255) p = 255;
					if (p < 0)  p = 0;

					img_result2.at<uchar>(y, x) = p;
				}
				else img_result2.at<uchar>(y, x) = img_result.at<uchar>(y, x);

			}
		}

		//rectangle(img_result, Point(newpointX[0], newpointY[0]), Point(newpointX[3], newpointY[3]), Scalar(255), 1);
		imshow("my result", img_result2);

		Size size(600, 600);
		Mat im_dst = Mat::zeros(size, CV_8UC1);
		warpPerspective(img_gray, im_dst, h222, size);

		imshow("opencv result", im_dst);
	}
}


int main(int argc, char** argv)
{

	Mat img_original;

	//이미지파일을 로드하여 image에 저장  
	img_original = imread("PIC6A1E.PNG", IMREAD_COLOR);
	if (img_original.empty())
	{
		cout << "Could not open or find the image" << std::endl;
		return -1;
	}


	//그레이스케일 이미지로 변환  
	cvtColor(img_original, img_gray, COLOR_BGR2GRAY);


	//윈도우에 출력  
	imshow("gray image", img_gray);

	//윈도우에 콜백함수를 등록
	setMouseCallback("gray image", CallBackFunc, NULL);

	cout << "왼쪽 위 - 오른쪽 위 - 왼쪽 아래, 오른쪽 아래 순으로 클릭해주세요" << endl;

	//키보드 입력이 될때까지 대기  
	waitKey(0);

	return 0;
}