5.结构光：单目标定（OpenCV）

参考相关代码，基于OpenCV的单目视觉的标定的代码如下：

#include "stdafx.h"#include<iostream>// opencv头文件#include <cv.h>#include <highgui.h>#include <cxcore.h>using namespace std;using namespace cv;//设置一 图像尺寸int image_width =2592;int image_height = 1944;//待标定图片的大小//设置二 图像中X方向上的角点个数const int ChessBoardSize_w = 9;//设置三 图像中Y方向上的角点个数const int ChessBoardSize_h = 6;//图片中可标定的角点区域和个数const CvSize  ChessBoardSize = cvSize(ChessBoardSize_w,ChessBoardSize_h);const int NPoints = ChessBoardSize_w*ChessBoardSize_h;//单张图像中所有的角点个数//设置四 待标定的图片数目const int NImages=37;//设置五 设置棋盘格子的边长，单位为mmfloat    SquareWidth = 10; //标定的结果保存int corner_count[NImages] = {0};CvMat *intrinsics;//内参数CvMat *distortion_coeff;//畸变参数CvMat *rotation_vectors;//旋转向量CvMat *translation_vectors;//平移向量CvMat *object_points;CvMat *point_counts;CvMat *image_points;CvMat *image_points2;void InitCorners3D(CvMat *Corners3D, CvSize ChessBoardSize, int Nimages, float SquareSize);//求取棋盘格上点的世界坐标void main(void){IplImage     *current_frame_rgb; //连接棋盘格角点图像IplImage     *currentsamall_frame_rgb; //连接棋盘格角点图像（缩小大小后的，便于显示）IplImage     *current_frame_gray;//灰度图像，用来提取角点IplImage     *chessBoard_Img;//原始载入图像CvPoint2D32f corners[NPoints*NImages];//所有的角点坐标//创建图像chessBoard_Img =cvCreateImage(cvSize(image_width, image_height), IPL_DEPTH_8U, 3);current_frame_gray = cvCreateImage(cvSize(image_width, image_height), IPL_DEPTH_8U, 1);current_frame_rgb = cvCreateImage(cvSize(image_width, image_height), IPL_DEPTH_8U, 3);currentsamall_frame_rgb = cvCreateImage(cvSize(image_width/2, image_height/2), IPL_DEPTH_8U, 3);//当前图像序号int captured_frames=0;for(captured_frames=0;captured_frames<NImages;captured_frames++){// 类型转换string img_name;char s[12];_itoa(captured_frames + 1, s, 10);img_name = s;//img_name = "./cam1_gray/pattern_cam1_im" + img_name + ".png";img_name = "./cam2_gray/pattern_cam2_im" + img_name + ".png";const char *filename = img_name.c_str();chessBoard_Img = cvLoadImage(filename, 1);if (chessBoard_Img==NULL){printf("load image failed!");}cvCvtColor(chessBoard_Img, current_frame_gray, CV_BGR2GRAY);cvCopy(chessBoard_Img,current_frame_rgb,0);        //查找角点int find_corners_result;find_corners_result = cvFindChessboardCorners(current_frame_gray,//当前图像ChessBoardSize,//角度区域 棋盘图中每行和每列角点的个数&corners[captured_frames*NPoints],// 检测到的角点&corner_count[captured_frames],//int corner_count[NImages] = {0};输出，角点的个数。如果不是NULL，函数将检测到的角点的个数存储于此变量。CV_CALIB_CB_ADAPTIVE_THRESH );//使用自适应阈值//通过迭代来发现具有子象素精度的角点位置cvFindCornerSubPix( current_frame_gray, //当前图像&corners[captured_frames*NPoints],//检测到的角点NPoints, //角点的个数cvSize(5,5),cvSize(-1,-1), //不忽略corner临近的像素进行精确估计，cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,30,0.01) );//迭代次数（iteration）或者最小精度（epsilon）cvDrawChessboardCorners(current_frame_rgb, ChessBoardSize, //绘制检测到的棋盘角点&corners[captured_frames*NPoints], NPoints, find_corners_result);cvSaveImage("角点绘图.bmp",current_frame_rgb);//cvResize(current_frame_rgb,currentsamall_frame_rgb);//cvNamedWindow("result", CV_WINDOW_AUTOSIZE); //cvShowImage("result",currentsamall_frame_rgb);cvWaitKey(100);}intrinsics         = cvCreateMat(3,3,CV_32FC1);distortion_coeff     = cvCreateMat(1,5,CV_32FC1);rotation_vectors     = cvCreateMat(NImages,3,CV_32FC1);translation_vectors     = cvCreateMat(NImages,3,CV_32FC1);point_counts         = cvCreateMat(NImages,1,CV_32SC1);object_points     = cvCreateMat(NImages*NPoints,3,CV_32FC1);image_points         = cvCreateMat(NImages*NPoints,2,CV_32FC1);    image_points2         = cvCreateMat(NImages*NPoints,2,CV_32FC1);//把2维点转化成三维点（object_points输出量），InitCorners3D(object_points, ChessBoardSize, NImages, SquareWidth);//把corners 复制到 image_points中cvSetData( image_points, corners, sizeof(CvPoint2D32f));    //把corner_count 复制到 point_counts中 每个图像中多少个角点cvSetData( point_counts, &corner_count, sizeof(int));//计算内参cvCalibrateCamera2( object_points,image_points,point_counts,//指定不同视图里点的数目cvSize(image_width,image_height),intrinsics,distortion_coeff,rotation_vectors,translation_vectors,0);float intr[3][3] = {0.0};float dist[4] = {0.0};float tranv[3] = {0.0};float rotv[3] = {0.0};for ( int i = 0; i < 3; i++){for ( int j = 0; j < 3; j++){intr[i][j] = ((float*)(intrinsics->data.ptr + intrinsics->step*i))[j];}dist[i] = ((float*)(distortion_coeff->data.ptr))[i];tranv[i] = ((float*)(translation_vectors->data.ptr))[i];rotv[i] = ((float*)(rotation_vectors->data.ptr))[i];}dist[3] = ((float*)(distortion_coeff->data.ptr))[3];dist[4] = ((float*)(distortion_coeff->data.ptr))[4];//旋转向量转换为旋转矩阵double R_matrix[9];CvMat pr_vec;CvMat pR_matrix;cvInitMatHeader(&pr_vec,1,3,CV_64FC1,rotv,CV_AUTOSTEP);cvInitMatHeader(&pR_matrix,3,3,CV_64FC1,R_matrix,CV_AUTOSTEP);cvRodrigues2(&pr_vec, &pR_matrix,0);   //输出标定结果printf("-----------------------------------------\n ");printf("INTRINSIC MATRIX:  \n");printf("[ %6.4f %6.4f %6.4f ]  \n", intr[0][0], intr[0][1], intr[0][2]);printf("[ %6.4f %6.4f %6.4f ]  \n", intr[1][0], intr[1][1], intr[1][2]);printf("[ %6.4f %6.4f %6.4f ]  \n", intr[2][0], intr[2][1], intr[2][2]);printf("----------------------------------------- \n");printf("DISTORTION VECTOR:  \n");printf("[ %6.4f %6.4f %6.4f %6.4f %6.4f]  \n", dist[0], dist[1], dist[2], dist[3], dist[4]);printf("----------------------------------------- \n");printf("ROTATION VECTOR(IMAGE1):  \n");printf("[ %6.4f %6.4f %6.4f ]  \n", rotv[0], rotv[1], rotv[2]);printf("TRANSLATION VECTOR(IMAGE1):  \n");printf("[ %6.4f %6.4f %6.4f ]  \n", tranv[0], tranv[1], tranv[2]);printf("----------------------------------------- \n");printf("ROTATION MATRIX(IMAGE1):  \n");printf("[ %6.4f %6.4f %6.4f ]  \n", R_matrix[0], R_matrix[1], R_matrix[2]);printf("[ %6.4f %6.4f %6.4f ]  \n", R_matrix[3], R_matrix[4], R_matrix[5]);printf("[ %6.4f %6.4f %6.4f ]  \n", R_matrix[6], R_matrix[7], R_matrix[8]);printf("----------------------------------------- \n");//评价标定结果double total_err = 0.0;double err = 0.0;double point_cousum=0;cout<<"\t每幅图像的定标误差：\n";for (int i=0;i<NImages;i++){//提取单张图像的参数 object_matrix rotation_matrix translation_matrixCvMat *object_matrix= cvCreateMat(NPoints,3,CV_32FC1);CvMat *image_matrix= cvCreateMat(NPoints,2,CV_32FC1);CvMat *project_image_matrix= cvCreateMat(NPoints,2,CV_32FC1);CvMat *rotation_matrix_1= cvCreateMat(1,3,CV_32FC1);CvMat *translation_matrix_1= cvCreateMat(1,3,CV_32FC1);CvMat *rotation_matrix= cvCreateMat(3,1,CV_32FC1);CvMat *translation_matrix= cvCreateMat(3,1,CV_32FC1);double err=0;cvGetRows(object_points,object_matrix,i*NPoints,(i+1)*NPoints,1);cvGetRow(rotation_vectors,rotation_matrix_1,i);cvReshape(rotation_matrix_1,rotation_matrix,0,3); cvGetRow(translation_vectors,translation_matrix_1,i);cvReshape(translation_matrix_1,translation_matrix,0,3); cvGetRows(image_points,project_image_matrix,i*NPoints,(i+1)*NPoints,1);     //反向投影，投影到的点保存在image_matrix中cvProjectPoints2(object_matrix,rotation_matrix,translation_matrix,intrinsics,distortion_coeff,image_matrix,0,0,0,0,0);err=cvNorm(image_matrix,project_image_matrix,CV_L2,0);        total_err=err*err;point_cousum=point_cousum+(point_counts->data.ptr + point_counts->step*i)[0];printf("第%d幅图像的误差为%f\n",i+1,sqrt(err*err/(point_counts->data.ptr + point_counts->step*i)[0])); }   printf("总的图像的误差为%f\n",sqrt(total_err/point_cousum)); system("pause");cvReleaseMat(&intrinsics);       cvReleaseMat(&distortion_coeff); cvReleaseMat(&rotation_vectors);cvReleaseMat(&translation_vectors);   cvReleaseMat(&point_counts);cvReleaseMat(&object_points);cvReleaseMat(&image_points);cvDestroyAllWindows();}void InitCorners3D(CvMat *Corners3D, CvSize ChessBoardSize, int NImages, float SquareSize){int CurrentImage = 0;int CurrentRow = 0;int CurrentColumn = 0;int NPoints = ChessBoardSize.height*ChessBoardSize.width;float * temppoints = new float[NImages*NPoints*3];for (CurrentImage = 0 ; CurrentImage < NImages ; CurrentImage++){for (CurrentRow = 0; CurrentRow < ChessBoardSize.height; CurrentRow++){for (CurrentColumn = 0; CurrentColumn < ChessBoardSize.width; CurrentColumn++){temppoints[(CurrentImage*NPoints*3)+(CurrentRow*ChessBoardSize.width + CurrentColumn)*3]=(float)CurrentRow*SquareSize;temppoints[(CurrentImage*NPoints*3)+(CurrentRow*ChessBoardSize.width + CurrentColumn)*3+1]=(float)CurrentColumn*SquareSize;temppoints[(CurrentImage*NPoints*3)+(CurrentRow*ChessBoardSize.width + CurrentColumn)*3+2]=0.f;}}}(*Corners3D) = cvMat(NImages*NPoints,3,CV_32FC1, temppoints);}