[OpenCV] 摄像机标定 + 三维重建

        源码是《Learning Opencv》中的，自己在这个基础上加入了重建部分，其实就是读取了生成的相机外参：平移旋转矩阵，然后在绘制中调用一下这个矩阵。由于加了平移之后就不知道物体移动到哪里去了，demo中只加了旋转部分。开了一个线程跑opencv重建，主线程用Opengl绘制。

       识别效果很一般，经常会识别失败，所以这个算法已经被放弃，把demo丢在这里作为纪念。

       

        按p键可以切换到透视角度，视觉效果好一些=A= 不能上传太大的图所以只有很小的片段了

run.h

#pragma once
#include <cv.h>

class check
{
public:
	CvMat *Rot = nullptr;
	CvMat *Trans = nullptr;
	int found;
	void run();
};

check.cpp

#include "run.h"
#include <cv.h>
#include <highgui.h>
#include <stdio.h>
#include <stdlib.h>

void check::run() {
	int board_w = 12;
	int board_h = 12;
	int board_n = board_w * board_h;
	CvSize board_sz = cvSize(board_w, board_h);
	CvMat *intrinsic = (CvMat*)cvLoad("Intrinsics.xml");
	CvMat *distortion = (CvMat*)cvLoad("Distortion.xml");
	IplImage *image = 0, *gray_image = 0;
	CvCapture *cap = cvCaptureFromCAM(0);
	IplImage* mapx = cvCreateImage(cvSize(640,480), IPL_DEPTH_32F, 1);
	IplImage* mapy = cvCreateImage(cvSize(640, 480), IPL_DEPTH_32F, 1);
	CvPoint2D32f* corners = new CvPoint2D32f[board_n];
	CvMat* RotRodrigues = cvCreateMat(3, 1, CV_32F);

	CvMat* image_points = cvCreateMat(4, 1, CV_32FC2);
	CvMat* object_points = cvCreateMat(4, 1, CV_32FC3);
	char key;
	int count = 0;

	while (1)
	{
		image = cvQueryFrame(cap);
		key = cvWaitKey(10);
		cvShowImage("Camera", image);
		
		gray_image = cvCreateImage(cvGetSize(image), 8, 1);
		cvCvtColor(image, gray_image, CV_BGR2GRAY);


		cvInitUndistortMap(
			intrinsic,
			distortion,
			mapx,
			mapy
		);
		IplImage *t = cvCloneImage(image);
		cvRemap(t, image, mapx, mapy);
		
		int corner_count = 0;
	
		found = cvFindChessboardCorners(
			image,
			board_sz,
			corners,
			&corner_count,
			CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS
		);

		
		cvFindCornerSubPix(gray_image, corners, corner_count,
			cvSize(11, 11), cvSize(-1, -1),
			cvTermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.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];

		for (int i = 0; i < 4; ++i) {
			CV_MAT_ELEM(*image_points, CvPoint2D32f, i, 0) = imgPts[i];
			CV_MAT_ELEM(*object_points, CvPoint3D32f, i, 0) = cvPoint3D32f(objPts[i].x, objPts[i].y, 0);
		}

		
		Rot = cvCreateMat(3, 3, CV_32F);
		Trans = cvCreateMat(3, 1, CV_32F);

		cvFindExtrinsicCameraParams2(object_points, image_points,
			intrinsic, distortion,
			RotRodrigues, Trans);
		cvRodrigues2(RotRodrigues, Rot);
		//delete t;
	}
	cvReleaseCapture(&cap);
}

main.cpp

#include "run.h"
#include <iostream>
#include <stdlib.h>
#include <thread>
#include "gl/glut.h"  

float fRotate = 0.0f; //旋转因子
float fScale = 1.0f; //缩放因子  
float tRotate = 0.0f; //旋转因子

check* c;

bool bPersp = false; //是否为透视投影 （vs 正投影）  
bool bAnim = false; //   
bool bWire = false; // 
bool isRotate = false; //

float ax, ay, az;
float mx,my;
bool isDown = false;
float rotateMatrix[16] = { 0 };
float translateMatrix[16] = { 0 };
float wHeight, wWidth;
int min(int x, int y)
{
	return x < y ? x : y;
}

void initMat(CvMat* mat, float* dstMat)
{
	for (int i = 0; i < mat->rows; i++) {
		float* pData = (float*)(mat->data.ptr + i*mat->step);
		for (int j = 0; j < mat->cols; j++) {
			dstMat[4 * i + j] = *pData;
			pData++;//指向下一个元素
		}
	}
}

void mouse(int button, int state, int x, int y)
{
	if (button == GLUT_LEFT_BUTTON) {
		if (state == GLUT_DOWN) {
			isDown = true;
			mx = x;
			my = y;
		}
		else if (state == GLUT_UP) {
			isDown = false;
		}
	}
}

void mouseMotion(int x, int y)
{
	if (isDown) {
		ax += 1.0f*(y - my) / 10.0f;
		ay += 1.0f*(x - mx) / 10.0f;
		mx = x;
		my = y;
	}
	glutPostRedisplay();
}


void draw()
{
	int numx = 13;
	int numy = 13;
	const float size = 0.1f;
	glDisable(GL_COLOR_MATERIAL);
	
	GLfloat mat_ambient[] = { 0.247250, 0.199500, 0.074500, 1.000000, };
	GLfloat mat_diffuse[] = { 0.751640, 0.606480, 0.226480, 1.000000, };
	GLfloat mat_specular[] = { 0.628281, 0.555802, 0.366065, 1.000000, };
	GLfloat mat_shininess[] = { 83.199997, }; 
	glMaterialfv(GL_FRONT, GL_AMBIENT, mat_ambient);
	glMaterialfv(GL_FRONT, GL_DIFFUSE, mat_diffuse);
	glMaterialfv(GL_FRONT, GL_SPECULAR, mat_specular);
	glMaterialfv(GL_FRONT, GL_SHININESS, mat_shininess);
	
	glPushMatrix();
	glTranslatef(-5, -5, -10);
	glRectf(0,0, 10, 10);
	glPopMatrix();

	glPushMatrix();
	glTranslatef(-5,-5, -10);
	glRotatef(90, 1, 0, 0);
	glRectf(0,0, 10, 10);
	glPopMatrix();

	glPushMatrix();
	glTranslatef(-5,-5, 0);
	glRotatef(90, 0, 1, 0);
	glRectf(0,0, 10, 10);
	glPopMatrix();
	if (c->found == 0)return;

	CvMat* mat1 = c->Rot;
	CvMat* mat2 = c->Trans;
	if (mat1 == nullptr || mat2 == nullptr)return;
	initMat(mat1, rotateMatrix);
	//initMat(mat2, translateMatrix);
	//translateMatrix[8] = -translateMatrix[8];
	rotateMatrix[15] = 1;
	//translateMatrix[15] = 1;

	glEnable(GL_COLOR_MATERIAL);
	//设置颜色追踪的材料属性以及多边形的面  
	glColorMaterial(GL_FRONT, GL_AMBIENT_AND_DIFFUSE);
	glPushMatrix();
	glTranslatef(-numx*size / 2 + size/2, 0, -numy*size / 2 + size/2);
	//glTranslatef(translateMatrix[0], translateMatrix[4], translateMatrix[8]);
	//glMultMatrixf(translateMatrix);
	glMultMatrixf(rotateMatrix);

	for (int i = 0; i < numx; i++) {
		for (int j = 0; j < numy; j++) {
			if ((i + j) % 2 == 1) {
				glColor3f(0, 0, 0);
			}
			else {
				glColor3f(1, 1, 1);
			}
			glRectf(0, 0,size,size);
			glTranslatef(0, size,0);
		}
		glTranslatef(size, -size*numy,0);
	}
	glPopMatrix();
}
void updateView(int width, int height)
{
	glViewport(0, 0, width, height);//设置视窗大小  

	glMatrixMode(GL_PROJECTION);//设置矩阵模式为投影  
	glLoadIdentity();    //初始化矩阵为单位矩阵    

	float whRatio = (GLfloat)width / (GLfloat)height;//设置显示比例  

	if (bPersp) {
		gluPerspective(45, whRatio, 1, 100); 
	}
	else glOrtho(-3, 3, -3, 3, -100, 100); //正投影   

	glMatrixMode(GL_MODELVIEW); //设置矩阵模式为模型  
}

void reshape(int width, int height)
{
	if (height == 0)  //如果高度为0  
	{
		height = 1;   //让高度为1（避免出现分母为0的现象）  
	}

	//height = width = min(height, width);
	wHeight = height;
	wWidth = width;

	updateView(wHeight, wWidth); //更新视角  
}

void idle()
{
	glutPostRedisplay();//调用当前绘制函数   
}

float eye[] = { 0, 0, 30 };
float center[] = { 0, 0, 0 };
float place[] = { 0, 0, 5 };

//按键回调函数  
void key(unsigned char k, int x, int y)
{
	switch (k)
	{
	case 'q': {exit(0); break; } //退出  
	case 'p': {bPersp = !bPersp; updateView(wHeight, wWidth); break; } //切换正投影、透视投影  

	case ' ': {bAnim = !bAnim; break; } //旋转模式的切换  
	case 'o': {bWire = !bWire; break; } //渲染方式的切换  

										//整体操作  

	case 'a': { //向左移动  
		center[0] += 0.1f;
		eye[0] += 0.1f;
		break;
	}
	case 'd': { //向右移动  
		center[0] -= 0.1f;
		eye[0] -= 0.1f;
		break;
	}
	case 'w': { //向上移动  
		center[1] -= 0.1f;
		eye[1] -= 0.1f;
		break;
	}
	case 's': { //向下移动  
		center[1] += 0.1f;
		eye[1] += 0.1f;
		break;
	}
	case 'z': { //向前移动  
		center[2] -= 0.1f;
		eye[2] -= 0.1f;
		break;
	}
	case 'c': { //向后移动  
		center[2] += 0.1f;
		eye[2] += 0.1f;
		break;
	}

	case 'l': { //右移  
		place[0] += 0.1f;
		if (place[0] > 1.5f)place[0] = 1.5f; //不超出桌面范围  
		break;
	}
	case 'j': { //左移 
		place[0] -= 0.1f;
		if (place[0] < -1.5f)place[0] = -1.5f;
		break;
	}
	case 'i': { //后移  
		place[1] += 0.1f;
		if (place[1] > 1.5f)place[1] = 1.5f;
		break;
	}
	case 'k': { //前移
		place[1] -= 0.1f;
		if (place[1] < -1.5f)place[1] = -1.5f;
		break;
	}
	case 'e': { //旋转
		isRotate = !isRotate;
		break;
	}
	}
}

void redraw()
{


	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); //清除颜色缓存和深度缓存  
	glClearColor(0.3, 0.3, 0.3,1);
	glLoadIdentity(); //初始化矩阵为单位矩阵 

	gluLookAt(eye[0], eye[1], eye[2],
		center[0], center[1], center[2],
		0, 1, 0);
	
	glEnable(GL_LIGHTING);  //开启光照模式
	GLfloat light_pos[] = { 5.0  ,5.0,5.0,1 }; //定义环境光位置     
	GLfloat white[] = { 1.0f, 1.0f, 1.0f, 1.0f }; //定义白色  

	glLightfv(GL_LIGHT0, GL_POSITION, light_pos); //设置第0号光源的光照位置    
	glLightfv(GL_LIGHT0, GL_SPECULAR, white); //设置镜面反射光照颜色  
	glLightfv(GL_LIGHT0, GL_DIFFUSE, white);                     //设置漫射光成分  
	glLightfv(GL_LIGHT0, GL_AMBIENT, white);   //设置第0号光源多次反射后的光照颜色（环境光颜色）  

	glEnable(GL_LIGHT0); //开启第0号光源   
	if (bWire) {
		glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); //设置多边形绘制模式：正反面，线型    
	}
	else {
		glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); //设置多边形绘制模式：正反面，填充    
	}

	glEnable(GL_DEPTH_TEST);  //开启深度测试  

	glRotatef(ax, 1, 0, 0);
	glRotatef(ay, 0, 1, 0);
	draw();

	glutSwapBuffers(); //交换缓冲区    
}

void init()
{
	c->run();
}

int main(int argc, char *argv[])
{
	c = new check();
	std::thread* th = new std::thread(init);
	//init();
	glutInit(&argc, argv);//对glut的初始化   
	glutInitDisplayMode(GLUT_RGBA | GLUT_DEPTH | GLUT_DOUBLE);//初始化显示模式:RGB颜色模型，深度测试，双缓冲     
	glutInitWindowSize(600, 480);//设置窗口大小    
	int windowHandle = glutCreateWindow("三维重建");//设置窗口标题     
	glutMouseFunc(mouse);
	glutMotionFunc(mouseMotion);
	glutDisplayFunc(redraw);//注册绘制回调函数    
	glutReshapeFunc(reshape);//注册重绘回调函数   
	glutKeyboardFunc(key);//注册按键回调函数  
	glutIdleFunc(idle);//注册全局回调函数：空闲时调用    
	
	glutMainLoop();// glut事件处理循环  
	return 0;
}

生成棋盘图的代码

#include <cv.h>
#include <highgui.h>
int main()
{
	CvMat* mat = cvCreateMat(520,520, CV_32F);
	for (int i = 0; i < mat->rows; i++) {
		float* pData = (float*)(mat->data.ptr + i*mat->step);
		for (int j = 0; j < mat->cols; j++) {
			if (((i / 40))%2== (j/40)%2) {
				*pData = 255;
			}
			else *pData = 0;
			
			pData++;//指向下一个元素
		}
	}
	cvSaveImage("calibration.jpg",mat);
	cvWaitKey(0);
}

最后一段是要预先跑的一段代码，用于对输入的棋盘图片进行建模，代码可以在《learning opencv》一书的源码中找到，在这里还是贴出来，只不过并没有包含需要用的图片，要图片的话也许还是需要去那本书的源码文件夹找一找

#include <cv.h>
#include <highgui.h>
#include <stdio.h>
#include <stdlib.h>

int n_boards = 23; //Will be set by input list
const int board_dt = 10;
int board_w;
int board_h;


void help() {
	printf("Calibration from disk. Call convention:\n\n"
		"  ch11_ex11_1_fromdisk board_w board_h image_list\n\n"
		"Where: board_{w,h} are the # of internal corners in the checkerboard\n"
		"       width (board_w) and height (board_h)\n"
		"       image_list is space separated list of path/filename of checkerboard\n"
		"       images\n\n"
		"Hit 'p' to pause/unpause, ESC to quit.  After calibration, press any other key to step through the images\n\n");
}

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

	CvCapture* capture;
	help();
	board_w = 12;
	board_h = 12;
	int board_n = board_w * board_h;
	CvSize board_sz = cvSize(board_w, board_h);
	char names[2048];

	//cvNamedWindow("Calibration");
	//ALLOCATE STORAGE
	//图像点 对象点 点计数
	CvMat* image_points = cvCreateMat(n_boards*board_n, 2, CV_32FC1);
	CvMat* object_points = cvCreateMat(n_boards*board_n, 3, CV_32FC1);
	CvMat* point_counts = cvCreateMat(n_boards, 1, CV_32SC1);

	//本征方程
	//
	CvMat* intrinsic_matrix = cvCreateMat(3, 3, CV_32FC1);
	CvMat* distortion_coeffs = cvCreateMat(4, 1, CV_32FC1);


	IplImage* image = 0;// = cvQueryFrame( capture );
	IplImage* gray_image = 0; //for subpixel
	CvPoint2D32f* corners = new CvPoint2D32f[board_n];
	int corner_count;
	int successes = 0;
	int step;

	for (int frame = 0; frame<n_boards; frame++) {
		sprintf(names, "calibration/IMG_0%d.jpg", frame + 191);
		if (image) {
			cvReleaseImage(&image);
			image = 0;
		}
		image = cvLoadImage(names, 1);
		gray_image = cvLoadImage(names, 0);
		if (gray_image == 0 && image) { //We'll need this for subpixel accurate stuff
			printf("here\n");
			gray_image = cvCreateImage(cvGetSize(image), 8, 1);
		}
		if (!image)
			printf("null image\n");

		int found = cvFindChessboardCorners(
			image,
			board_sz,
			corners,
			&corner_count,
			CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS
		);
		//使用openCV中的标定板角点检测函数，检测出角点。
		//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));
		//Draw it
		//找到亚像素角度位置，函数：cvFindCornerSubPix
		cvDrawChessboardCorners(image, board_sz, corners, corner_count, found);
		char n[100];
		sprintf(n, "corner%d.jpg", frame + 1);
		cvSaveImage(n, image);
		cvShowImage("calibration", image);
		// If we got a good board, add it to our data
		//
		printf("%d %d\n", corner_count, board_n);
		if (corner_count == board_n) {
			step = successes*board_n;
			//	printf("Found = %d for %s\n",found,names);
			for (int i = step, j = 0; j<board_n; ++i, ++j) {
				///         CV_MAT_ELEM(*image_points, CvPoint2D32f,0,i) = cvPoint2D32f(corners[j].x,corners[j].y);
				///         CV_MAT_ELEM(*object_points,CvPoint3D32f,0,i) = cvPoint3D32f(j/board_w, j%board_w, 0.0f);
				CV_MAT_ELEM(*image_points, float, i, 0) = corners[j].x;
				CV_MAT_ELEM(*image_points, float, i, 1) = corners[j].y;
				CV_MAT_ELEM(*object_points, float, i, 0) = j / board_w;
				CV_MAT_ELEM(*object_points, float, i, 1) = j%board_w;
				CV_MAT_ELEM(*object_points, float, i, 2) = 0.0f;

			}
			//        CV_MAT_ELEM(*point_counts, int,0,successes) = board_n;
			CV_MAT_ELEM(*point_counts, int, successes, 0) = board_n;
			successes++;
		}

		//    if( successes == n_boards ) break;

		int c = cvWaitKey(15);
		if (c == 'p') {
			c = 0;
			while (c != 'p' && c != 27) {
				c = cvWaitKey(250);
			}
		}
		if (c == 27)
			return 0;
	}
	printf("successes = %d, n_boards=%d\n", successes, n_boards);
	//ALLOCATE MATRICES ACCORDING TO HOW MANY IMAGES WE FOUND CHESSBOARDS ON
	///  CvMat* image_points2      = cvCreateMat(1,successes*board_n,CV_32FC2);
	///  CvMat* object_points2     = cvCreateMat(1,successes*board_n,CV_32FC3);
	///  CvMat* point_counts2      = cvCreateMat(1,successes,CV_32SC1);
	CvMat* object_points2 = cvCreateMat(successes*board_n, 3, CV_32FC1);
	CvMat* image_points2 = cvCreateMat(successes*board_n, 2, CV_32FC1);
	CvMat* point_counts2 = cvCreateMat(successes, 1, CV_32SC1);
	//TRANSFER THE POINTS INTO THE CORRECT SIZE MATRICES
	for (int i = 0; i<successes*board_n; ++i) {
		///      CV_MAT_ELEM(*image_points2, CvPoint2D32f,0,i)  = CV_MAT_ELEM(*image_points, CvPoint2D32f,0,i);
		///      CV_MAT_ELEM(*object_points2,CvPoint3D32f,0,i)  = CV_MAT_ELEM(*object_points,CvPoint3D32f,0,i);
		CV_MAT_ELEM(*image_points2, float, i, 0) = CV_MAT_ELEM(*image_points, float, i, 0);
		CV_MAT_ELEM(*image_points2, float, i, 1) = CV_MAT_ELEM(*image_points, float, i, 1);
		CV_MAT_ELEM(*object_points2, float, i, 0) = CV_MAT_ELEM(*object_points, float, i, 0);
		CV_MAT_ELEM(*object_points2, float, i, 1) = CV_MAT_ELEM(*object_points, float, i, 1);
		CV_MAT_ELEM(*object_points2, float, i, 2) = CV_MAT_ELEM(*object_points, float, i, 2);

	}
	for (int i = 0; i<successes; ++i) {
		///		CV_MAT_ELEM(*point_counts2,int,0, i) = CV_MAT_ELEM(*point_counts, int,0,i);
		CV_MAT_ELEM(*point_counts2, int, i, 0) = CV_MAT_ELEM(*point_counts, int, i, 0);
	}
	cvReleaseMat(&object_points);
	cvReleaseMat(&image_points);
	cvReleaseMat(&point_counts);

	//cvWaitKey();//Now we have to reallocate the matrices
	// return 0;
	// At this point we have all of the chessboard corners we need.
	//

	// Initialize the intrinsic matrix such that the two focal
	// lengths have a ratio of 1.0
	//
	CV_MAT_ELEM(*intrinsic_matrix, float, 0, 0) = 1.0f;
	CV_MAT_ELEM(*intrinsic_matrix, float, 1, 1) = 1.0f;
	printf("cvCalibrateCamera2\n");
	cvCalibrateCamera2(
		object_points2,
		image_points2,
		point_counts2,
		cvGetSize(image),
		intrinsic_matrix,
		distortion_coeffs,
		NULL,
		NULL,
		0//CV_CALIB_FIX_ASPECT_RATIO
	);
	// Save our work
	cvSave("Intrinsics.xml", intrinsic_matrix);
	cvSave("Distortion.xml", distortion_coeffs);
	// Load test
	CvMat *intrinsic = (CvMat*)cvLoad("Intrinsics.xml");
	CvMat *distortion = (CvMat*)cvLoad("Distortion.xml");

	// Build the undistort map which we will use for all 
	// subsequent frames.
	//

	// Just run the camera to the screen, now only showing the undistorted
	// image.
	//
	//rewind(fptr);
	//cvNamedWindow("Undistort");
	printf("\n\nPress any key to step through the images, ESC to quit\n\n");
	for (int frame = 0; frame<n_boards; frame++) {
		sprintf(names, "calibration/IMG_0%d.jpg", frame + 191);
		if (image) {
			cvReleaseImage(&image);
			image = 0;
		}
		image = cvLoadImage(names);
		IplImage* mapx = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1);
		IplImage* mapy = cvCreateImage(cvGetSize(image), IPL_DEPTH_32F, 1);
		printf("cvInitUndistortMap\n");
		cvInitUndistortMap(
			intrinsic,
			distortion,
			mapx,
			mapy
		);

		char n[100];
		sprintf(n, "distort%d.jpg", frame + 1);
		IplImage *t = cvCloneImage(image);
		cvRemap(t, image, mapx, mapy);
		cvReleaseImage(&t);
		cvSaveImage(n, image);
	}
	system("pause");
	return 0;