TopView Transformation (鳥瞰図変換) 2

TopView Transformation 1 のエントリで論文まとめを書いた鳥瞰図変換ですが,ここでは実際に鳥瞰図変換にトライしていきます.

実験設定

 自宅実験室(笑) にて,以前紹介した USB カメラを使って写真を撮ります.鳥瞰図変換用にカメラ三脚と角度計を購入.

実験の様子(笑)

f:id:rkoichi2001:20160529025845j:plain f:id:rkoichi2001:20160529025842j:plain

結果

 結果ですが,カメラのキャリブレーションが甘いのか,カメラ設置があまいのか,直線が少し曲がってしまっているのですが,それでも鳥瞰図は出来上がりました.画像の端に行くに連れて対応するピクセルが疎になっている様子がわかります.線形補間等の処理を加えてあげる必要がありそうですが,それはまた今度....

変換前写真(歪み補正前)

f:id:rkoichi2001:20160529133210p:plain

変換前写真(歪み補正後)

f:id:rkoichi2001:20160529133136p:plain

変換後写真

f:id:rkoichi2001:20160529134517p:plain

実装

 下記,実験に使ったコードです.

#include <iostream>
#include <math.h>
#include "opencv2/opencv.hpp"
//#include "opencv2/calib3d.hpp"

using namespace std;
using namespace cv;

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

	// 1. Load Raw Iamge
	string imagepath = "/home/koichi/my_photo-1.jpg";
	Mat rawImage = imread(imagepath, CV_LOAD_IMAGE_COLOR);
	namedWindow("Raw Image", WINDOW_AUTOSIZE);
	imshow("Raw Image", rawImage);

	// 2. Undistort Image Based on Calibration Matrix.
	Mat undistortImage(rawImage.rows, rawImage.cols, CV_8UC3);
	Matx33d K(	627.235434, 0,			654.957574,
				0,			630.482585,	494.346943,
				0,			0,			1			);
	cv::Vec4d D(-0.210146, 0.030563, 0.001172, -0.001306);
	cv::undistort(rawImage, undistortImage, K, D);
	namedWindow("Undistorted Image", WINDOW_AUTOSIZE);
	//line(undistortImage, Point(640, 0), Point(640, 960), Scalar(255, 255, 255), 10);
	//line(undistortImage, Point(0, 460), Point(1280, 460), Scalar(255, 255, 255), 10);
	imshow("Undistorted Image", undistortImage);

	// 3. Convert Image to Gray
	Mat grayImage(rawImage.rows, rawImage.cols, CV_8UC1);
	cvtColor(undistortImage, grayImage, COLOR_RGB2GRAY);
	namedWindow("Gray Image", WINDOW_AUTOSIZE);
	imshow("Gray Image", grayImage);

	// 4. Top View Conversion
	Mat topImage(rawImage.rows, rawImage.cols, CV_8UC3);
	topImage.setTo(Scalar(0));

	Mat topImageGray(rawImage.rows, rawImage.cols, CV_8UC1);
	topImageGray.setTo(Scalar(0));
	{
		double Hvc = 2;
		double Hc = 0.7;
		double Dvc = 1.7;
		double f = 630;
		double fp = f;
		double theta = 30.0 / 180.0 * M_PI;
		double s = sin(theta);
		double c = cos(theta);
		int cx = 640;
		int cy = 480;
		int cxp = 640;
		int cyp = 480;

		for (int y = 0; y < topImage.rows; y++) {
			for (int x = 0; x < topImage.cols; x++) {

				int xOrg = x - cx;
				int yOrg = - y + cy;

				int newX = fp / Hvc * Hc * xOrg / (f * s - yOrg * c);
				int newY = fp / Hvc * (Hc * (f * c + yOrg * s) / (f * s - yOrg * c) - Dvc);

				newX = newX + cxp;
				newY = -newY + cyp;

				if (newX < 0 || topImage.cols - 1 < newX || newY < 0 || topImage.rows - 1 < newY ) {
					continue;
				}

				topImageGray.data[newY * topImageGray.cols + newX] = grayImage.data[y * grayImage.cols + x];

				topImage.data[(newY * topImage.cols + newX) * topImage.channels()] = undistortImage.data[(y * topImage.cols + x) * topImage.channels()];
				topImage.data[(newY * topImage.cols + newX) * topImage.channels() + 1] = undistortImage.data[(y * topImage.cols + x) * topImage.channels() + 1];
				topImage.data[(newY * topImage.cols + newX) * topImage.channels() + 2] = undistortImage.data[(y * topImage.cols + x) * topImage.channels() + 2];

			}
		}
	}

	namedWindow("Top Image", WINDOW_AUTOSIZE);
	imshow("Top Image", topImage);
	namedWindow("Top Image Gray", WINDOW_AUTOSIZE);
	imshow("Top Image Gray", topImageGray);

	while(true) {
		waitKey(0);
	}
	return 0;
}