瞰视图代码详解

/*
瞰视图技术也可以应用于将任意平面(如墙或天花板)透视变换到前向平行视图。
机器人导航的一项常见工作就是将机器人场景的摄像机视图转换到从上到下的"俯视"视图。场景的机器人视图转换成了鸟瞰图，使得视图就可以被随后的
由扫描激光测距仪创建的世界场景覆盖。接下来，我们将详细地来说明如何利用标定得摄像机计算这样的一个鸟瞰图。
为了获得鸟瞰图，我们需要从摄像机标定程序中得到摄像机内参数和畸变参数矩阵。仅仅为了程序的多选择性，我们选择从硬盘读取文件。在地面上放置一个棋盘，
从机器人车上获得它的地平面图像，然后将图像重新映射为鸟瞰图。具体算法流程如下：
1.读取摄像机的内参数和畸变参数模型
2.查找地平面上的已知物体(如本例中的棋盘)，获得最少四个亚像素精度上的点。
3.将找到的点输入到函数cvGetPerseectiveTransform()中，计算地平面视图的单应矩阵H。
4.使用函数cvWarpPerspective()，设置标志CV_INTER_LINEAR+CV_WARP_INVERSE_MAP+CV_WARP_FILL_OUTLIERS,获得地平面的前向平行视图(鸟瞰图)。
*/


#include<opencv2/opencv.hpp>
#include<opencv2/highgui.hpp>


int main(int argc, char* argv[])
{
if (argc != 6)  return -1;


//Input Parameters
int board_w = atoi(argv[1]);//棋盘宽度
int board_h = atoi(argv[2]);//棋盘高度
int board_n = board_w*board_h;//棋盘内方格数目
CvSize board_sz = CvSize(board_w, board_h);
CvMat* intrinsic = (CvMat*)cvLoad(argv[3]);//前提是摄像机已经标定好了(即摄像机内参数和畸变参数已经求出来了)
CvMat* distortion = (CvMat*)cvLoad(argv[4]);
IplImage* image = 0;
IplImage* gray_image = 0;
if ((image = cvLoadImage(argv[5])) == 0)
{
printf("Error:Could not load %s\n", argv[5]);
return -1;
}
gray_image = cvCreateImage(cvGetSize(image), 8, 1);
cvCvtColor(image, gray_image, CV_BGR2GRAY);


/*OpenCV提供一个直接使用的校正算法，即输入原始图像和由函数cvCalibrateCamera2()得到的畸变系数，
生成校正后的图像。我们既可以一次性通过函数cvUndistort2()使用该算法完成所有事项，
也可以通过一对函数cvInitUndistortMap()和cvRemap()来更有效率的处理此事，这通常适合视频或者从同一摄像机中得到多个图像的应用。
基本方法是先计算畸变映射，再矫正图像。函数cvInitUndistortMap()用于计算畸变映射，而函数
cvRemap()表示在任意图像应用该映射*/
//Undistort our image
IplImage* mapx = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1);
IplImage* mapy = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1);


//1.This initializes rectification matrices(计算畸变映射)
cvInitUndistortMap(intrinsic, distortion, mapx, mapy);
IplImage *t = cvCloneImage(image);


//Rectify our image(矫正图像)
cvRemap(t, image, mapx, mapy);


//GET THE CHESSBOARD ON THE PLANE
cvNamedWindow("Chessboard");
CvPoint2D32f* corners = new CvPoint2D32f[board_n];
int corner_count = 0;
//2.发现棋盘角点
int found = cvFindChessboardCorners(image, board_sz, corners, &corner_count,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
if (!found)
{
printf("Could not aquire chessboard on %s, "
"only found %d of %d corners\n", argv[5], corner_count, board_n);
return -1;
}


//Get Subpixel accuracy on those corners:查找亚像素级角点
cvFindCornerSubPix(gray_image, corners, corner_count, cvSize(11, 11), cvSize(-1, -1),
cvTermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1));


//Get the image and object points: we will choose chessboard object points as (r,c):
//(0,0),(board_w-1,0),(0,board_h-1),(board_w-1,board_h-1)
CvPoint2D32f objPts[4], imgPts[4];//获取单应性矩阵至少需要四个点
objPts[0].x = 0; objPts[0].y = 0;
objPts[1].x = board_w - 1; objPts[1].y = 0;
objPts[2].x = 0; objPts[2].y = board_h - 1;
objPts[3].x = board_w - 1; objPts[3].y = board_h - 1;
imgPts[0] = corners[0];
imgPts[1] = corners[board_w - 1];
imgPts[2] = corners[(board_h - 1)*board_w];
imgPts[3] = corners[(board_h - 1)*board_w + board_w - 1];


//Draw the points in order:B,G,R,yellow
cvCircle(image, cvPointFrom32f(imgPts[0]), 9, CV_RGB(0, 0, 255), 3);
cvCircle(image, cvPointFrom32f(imgPts[1]), 9, CV_RGB(0, 255, 0), 3);
cvCircle(image, cvPointFrom32f(imgPts[2]), 9, CV_RGB(255, 0, 0), 3);
cvCircle(image, cvPointFrom32f(imgPts[3]), 9, CV_RGB(255, 255, 0), 3);


//Draw the found chessboard
cvDrawChessboardCorners(image, board_sz, corners, corner_count, found);
cvShowImage("Chessboard", image);


//3.Find the homography(单应性矩阵)--获得单应性矩阵至少需要四个点
CvMat* H = cvCreateMat(3, 3, CV_32F);
cvGetPerspectiveTransform(objPts, imgPts, H);


//Let the user adjust the Z height of the view
float Z = 25;
int key = 0;
IplImage* birds_image = cvCloneImage(image);
cvNamedWindow("Birlds_Eye");


//Loop to allow user to play with height   ESC key stops
while (key!=27)
{
//Set the height
CV_MAT_ELEM(*H, float, 2, 2);


//Compute the frontal parallel or bird-Eye view
//4.Using Homography to remap the view--获得地平面的前向平行视图(鸟瞰图)
cvWarpPerspective(image, birds_image, H, CV_INTER_LINEAR | CV_WARP_INVERSE_MAP | CV_WARP_FILL_OUTLIERS);
cvShowImage("Birds_Eye", birds_image);

key = cvWaitKey();
if (key == 'u')Z += 0.5;
if (key == 'd')Z -= 0.5;
}


cvSave("H.xml", H);//we can reuse H for the same camera mounting
return 0;


}