opencv-相机标定步骤、评估标定误差以及标定之后图像坐标到世界坐标的转换

前一段时间，研究了下相机标定。关于opencv相机标定，包括标定完后，世界坐标到 图像坐标的转换，以评估图像的标定误差，这些网上有很多资源和源代码。

可是，相机标定完之后，我们想要的是，知道了图像坐标，想要得到它的世界坐标，或者我们已知图像上两个点之间的像素距离，现在我们想知道两个点之间的实际距离。

楼主在网上搜了很多资源，问了很多人，都没有相关的代码，可以得到这样的结论：opencv没有提供现成的函数，满足从图像坐标到世界坐标的转换。

所以，我们最想要的这一步，是需要自己写的。（如果我理解的不对，希望看到这篇博文的人，能够批评指正）

相机标定的主要思路为：

一、获取十几张不同角度拍摄的图片，角点检测，得到每个角点的坐标；

二、分别定义十几张照片中，世界坐标系下的角点坐标，一般x、y为等间距，z为0 ；

三、开始标定，主要函数为calibrateCamera；

四、得到了相机内参和畸变系数，这是标定完后相机的属性，还会得到外参，外参代表着每张图片所在的平面；

五、opencv提供了世界坐标到图像坐标的转换函数，主要用来评估标定的误差；

六、我们最想要的，根据图像坐标到世界坐标的转换，本质上就是矩阵的运算，需要自己写；

下面贴出代码，开发环境opencv2.4.9+vs2013

#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include
#include

using namespace std;
using namespace cv;

const int imageWidth = 1600;                             //摄像头的分辨率
const int imageHeight = 1200;
const int boardWidth = 39;                               //横向的角点数目
const int boardHeight = 39;                              //纵向的角点数据
const int boardCorner = boardWidth * boardHeight;       //总的角点数据
const int frameNumber =7;                             //相机标定时需要采用的图像帧数
const int squareSize = 10;                              //标定板黑白格子的大小 单位mm
const Size boardSize = Size(boardWidth, boardHeight);   //

Mat intrinsic;                                          //相机内参数
Mat distortion_coeff;                                   //相机畸变参数
vector rvecs;                                        //旋转向量
vector tvecs;                                        //平移向量
vector> corners;                        //各个图像找到的角点的集合 和objRealPoint 一一对应
vector> objRealPoint;                   //各副图像的角点的实际物理坐标集合


vector corner;                                   //某一副图像找到的角点

Mat rgbImage, grayImage;

/*计算标定板上模块的实际物理坐标*/
void calRealPoint(vector>& obj, int boardwidth, int boardheight, int imgNumber, int squaresize)
{
    //  Mat imgpoint(boardheight, boardwidth, CV_32FC3,Scalar(0,0,0));
    vector imgpoint;
    for (int rowIndex = 0; rowIndex < boardheight; rowIndex++)
    {
        for (int colIndex = 0; colIndex < boardwidth; colIndex++)
        {
            //  imgpoint.at(rowIndex, colIndex) = Vec3f(rowIndex * squaresize, colIndex*squaresize, 0);
            imgpoint.push_back(Point3f(colIndex * squaresize, rowIndex * squaresize, 0));
        }
    }
    for (int imgIndex = 0; imgIndex < imgNumber; imgIndex++)
    {
        obj.push_back(imgpoint);
    }
}

/*设置相机的初始参数 也可以不估计*/

void CalibrationEvaluate(void)//标定结束后进行评价
{
    double err=0;
    double total_err=0;
    //calibrateCamera(objRealPoint, corners, Size(imageWidth, imageHeight), intrinsic, distortion_coeff, rvecs, tvecs, 0);
    cout << "每幅图像的定标误差：" << endl;
    for (int i = 0; i < corners.size(); i++)
    {
        vector image_points2;
        vector tempPointSet = objRealPoint[i];
        projectPoints(tempPointSet, rvecs[i], tvecs[i], intrinsic, distortion_coeff, image_points2);




        vector tempImagePoint = corners[i];
        Mat tempImagePointMat = Mat(1, tempImagePoint.size(), CV_32FC2);
        Mat image_points2Mat = Mat(1, image_points2.size(), CV_32FC2);
        for (int j = 0; j < tempImagePoint.size(); j++)
        {
            image_points2Mat.at(0, j) = Vec2f(image_points2[j].x, image_points2[j].y);
            tempImagePointMat.at(0, j) = Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);
        }
        err = norm(image_points2Mat, tempImagePointMat, NORM_L2);
        total_err = err + total_err;
        cout << "第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;
    }
    cout << "总体平均误差：" << total_err / (corners.size() + 1) << "像素" << endl;
}

void guessCameraParam(void)
{
    /*分配内存*/
    intrinsic.create(3, 3, CV_64FC1);
    distortion_coeff.create(5, 1, CV_64FC1);

    /*
    fx 0 cx
    0 fy cy
    0 0  1
    */
    intrinsic.at<double>(0, 0) = 256.8093262;   //fx
    intrinsic.at<double>(0, 2) = 160.2826538;   //cx
    intrinsic.at<double>(1, 1) = 254.7511139;   //fy
    intrinsic.at<double>(1, 2) = 127.6264572;   //cy

    intrinsic.at<double>(0, 1) = 0;
    intrinsic.at<double>(1, 0) = 0;
    intrinsic.at<double>(2, 0) = 0;
    intrinsic.at<double>(2, 1) = 0;
    intrinsic.at<double>(2, 2) = 1;

    /*
    k1 k2 p1 p2 p3
    */
    distortion_coeff.at<double>(0, 0) = -0.193740;  //k1
    distortion_coeff.at<double>(1, 0) = -0.378588;  //k2
    distortion_coeff.at<double>(2, 0) = 0.028980;   //p1
    distortion_coeff.at<double>(3, 0) = 0.008136;   //p2
    distortion_coeff.at<double>(4, 0) = 0;          //p3
}

void outputCameraParam(void)
{
    /*保存数据*/
    //cvSave("cameraMatrix.xml", &intrinsic);
    //cvSave("cameraDistoration.xml", &distortion_coeff);
    //cvSave("rotatoVector.xml", &rvecs);
    //cvSave("translationVector.xml", &tvecs);
    /*输出数据*/
    cout << "fx :" << intrinsic.at<double>(0, 0) << endl << "fy :" << intrinsic.at<double>(1, 1) << endl;
    cout << "cx :" << intrinsic.at<double>(0, 2) << endl << "cy :" << intrinsic.at<double>(1, 2) << endl;

    cout << "k1 :" << distortion_coeff.at<double>(0, 0) << endl;
    cout << "k2 :" << distortion_coeff.at<double>(1, 0) << endl;
    cout << "p1 :" << distortion_coeff.at<double>(2, 0) << endl;
    cout << "p2 :" << distortion_coeff.at<double>(3, 0) << endl;
    cout << "p3 :" << distortion_coeff.at<double>(4, 0) << endl;
}

//int _tmain(int argc, _TCHAR* argv[])
int main()
{
    Mat img;
    int goodFrameCount = 0;
    namedWindow("chessboard");
    cout << "按Q退出 ..." << endl;
    while (goodFrameCount < frameNumber)
    {

        char filename[100];
        sprintf_s(filename, "chao%d.bmp", goodFrameCount);
        //sprintf_s(filename, "chess%d.jpg", goodFrameCount);
        goodFrameCount++;

        rgbImage = imread(filename, 1);
        cvtColor(rgbImage, grayImage, CV_BGR2GRAY);
        imshow("Camera", grayImage);

        bool isFind = findChessboardCorners(rgbImage, boardSize, corner, 0);
        //bool isFind = findChessboardCorners(rgbImage, boardSize, corner, CV_CALIB_CB_NORMALIZE_IMAGE);
        if (isFind == true) //所有角点都被找到 说明这幅图像是可行的
        {
            /*
            Size(5,5) 搜索窗口的一半大小
            Size(-1,-1) 死区的一半尺寸
            TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 20, 0.1)迭代终止条件
            */
            cornerSubPix(grayImage, corner, Size(5, 5), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 20, 0.1));
            drawChessboardCorners(rgbImage, boardSize, corner, isFind);
            imshow("chessboard", rgbImage);
            corners.push_back(corner);
            //string filename = "res\\image\\calibration";
            //filename += goodFrameCount + ".jpg";
            //cvSaveImage(filename.c_str(), &IplImage(rgbImage));       //把合格的图片保存起来

            cout << "The image is good" << endl;
        }
        else
        {
            cout << "The image is bad please try again" << endl;
        }
        //  cout << "Press any key to continue..." << endl;
        //  waitKey(0);

        if (waitKey(10) == 'q')
        {
            break;
        }
        //  imshow("chessboard", rgbImage);
    }

    /*
    图像采集完毕 接下来开始摄像头的校正
    calibrateCamera()
    输入参数 objectPoints  角点的实际物理坐标
    imagePoints   角点的图像坐标
    imageSize     图像的大小
    输出参数
    cameraMatrix  相机的内参矩阵
    distCoeffs    相机的畸变参数
    rvecs         旋转矢量(外参数)
    tvecs         平移矢量(外参数）
    */

    /*设置实际初始参数 根据calibrateCamera来 如果flag = 0 也可以不进行设置*/
    guessCameraParam();
    cout << "guess successful" << endl;
    /*计算实际的校正点的三维坐标*/
    calRealPoint(objRealPoint, boardWidth, boardHeight, frameNumber, squareSize);
    cout << "cal real successful" << endl;
    /*标定摄像头*/
    calibrateCamera(objRealPoint, corners, Size(imageWidth, imageHeight), intrinsic, distortion_coeff, rvecs, tvecs, 0);
    cout << "calibration successful" << endl;
    /*保存并输出参数*/
    outputCameraParam();
    CalibrationEvaluate();
    cout << "out successful" << endl;

    /*显示畸变校正效果*/
    Mat cImage;
    undistort(rgbImage, cImage, intrinsic, distortion_coeff);
    imshow("Corret Image", cImage);
    cout << "Correct Image" << endl;
    cout << "Wait for Key" << endl;

    waitKey(0);
    system("pause");
    return 0;
}

以上只贴出了部分代码，完整的C++代码，可到 Hust平凡之路相机标定工程 下载。