OPENCV之从calibrateCamera到solvePnP（一）

写这篇文章其实主要是给入门者参考的，没有技术含量。主要是看到网上流传有各种疑问和答案，这里用自己的理解总结一下，一步一步从单目标定到重建写一下。

开发环境VS2013+OPENCV2.4.9

文章围绕OPENCV原函数C++版本的调用展开，构造输入和输出参数。

首先看标定

原理不再说明。
第一个参数物点是由自己构造的一系列点三维坐标。
以下图为例

vector<vector<Point3f> > calcBoardCornerPositions(int gridW, int gridH, float squareSize, int imagesCount)
{
    vector<vector<Point3f> > objectPoints(imagesCount);
    for (int k = 0; k <imagesCount; k++) {
        objectPoints[k] = vector<Point3f>(0);
        for (int i = 0; i < gridH; i++)
            for (int j = 0; j < gridW; j++)
                objectPoints[k].push_back(Point3f(float(j*squareSize), float(i*squareSize), 0));
    }
    return objectPoints;
}
vector<vector<Point3f> > objectPoints = calcBoardCornerPositions(grids.width, grids.height, 260, imagePoints.size());

这里 Size grids(9, 6);//代表棋盘点的个数
int Grids_Size = 260;//代表棋盘格子宽度（0.1mm)
Z坐标设为0.
棋盘的长宽可以不同，构造方法类似。
以上得到了第一个参数。

第二个参数像点，代表在图上检测到的角点像素坐标。

vector<vector<Point2f> > imagePoints;
vector<int> usefulImgIndeces;
for (int i = 0; i <20; i++) 
{
    vector<Point2f> corners;
    bool found = findChessboardCorners(images[i], grids, corners, CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE + CALIB_CB_FAST_CHECK);

    if (found) {
        usefulImgIndeces.push_back(i);
        Mat gray;
        cvtColor(images[i], gray, CV_BGR2GRAY);
        cornerSubPix(gray, corners, Size(11, 11), Size(-1, -1),
            TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));//0.1为精度
        imagePoints.push_back(corners);
        if (true) {
            drawChessboardCorners(images[i], grids, Mat(corners), found);
            //imshow("chessboard" + to_string(i), images[i]);
            //waitKey(200);
        }
    }
}

以上得到第二个参数。
对于findChessboardCorners函数，OPENCV内部使用cvCheckChessboard先判断是否有棋盘，算法阈值由低到高，决定平均亮度较低的时候检测棋盘较快。
在findChessboardCorners中同样调用了

        cvFindCornerSubPix( gray, out_corners, pattern_size.width*pattern_size.height,
            cvSize(wsize, wsize), cvSize(-1,-1), cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 15, 0.1));

精确寻找角点。再外部再调用其实意义不大，不过耗时也不多，可把精度提高再迭代几次。

以上得到两个参数了，注意两个参数的类型vector

vector<Mat> rvecs, tvecs; Mat  cameraMatrix; Mat distCoeffs;

再后面的参数可以不用管了，网上很多解释，有特殊需求的可以查一查其含义。

最后使用FileStorage保存参数。

FileStorage fs;
fs.open("result.xml", FileStorage::WRITE);
fs << "cameraMatrix" << cameraMatrix;
fs << "distCoeffs" << distCoeffs;
fs.release();

完整代码

#include <cv.h>
#include <highgui.h>
#include <iostream>
using namespace std;
using namespace cv;
vector<vector<Point3f> > calcBoardCornerPositions(int gridW, int gridH, float squareSize, int imagesCount)
{
    vector<vector<Point3f> > objectPoints(imagesCount);
    for (int k = 0; k <imagesCount; k++) {
        objectPoints[k] = vector<Point3f>(0);
        for (int i = 0; i < gridH; i++)
            for (int j = 0; j < gridW; j++)
                objectPoints[k].push_back(Point3f(float(j*squareSize), float(i*squareSize), 0));
    }
    return objectPoints;
}
int i = 0; Mat frame;
void onMouse(int event, int x, int y, int flags, void* userdata)
{
    if (event == 1)
        imwrite(to_string(i++) + ".jpg", frame);

}
int main()
{

    VideoCapture cap(0);
    //cap.set(CV_CAP_PROP_FRAME_WIDTH, 1920);
    //cap.set(CV_CAP_PROP_FRAME_HEIGHT, 1080);
    char key;
    if (!cap.isOpened()) return -1;

    namedWindow("frame", 0);
    cap >> frame;//该镜头第一帧可能为空白
    for (;i<20;)
    {
        cap >> frame;
        setMouseCallback("frame", onMouse);
        key = waitKey(20);
        if (key == ' ')
        {
            imwrite(to_string(i++) + ".jpg", frame);
        }
        if (key == 'q')break;
        imshow("frame", frame);
    }
vector<Mat> images;
for (int i = 0; i <20; i++) {
    images.push_back(imread(to_string(i)+".jpg"));
}

//Size grids(7, 5);
Size grids(9, 6);
int Grids_Size = 260;
vector<vector<Point2f> > imagePoints;
vector<int> usefulImgIndeces;
for (int i = 0; i <20; i++) 
{
    vector<Point2f> corners;
    bool found = findChessboardCorners(images[i], grids, corners, CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE + CALIB_CB_FAST_CHECK);

    if (found) {
        usefulImgIndeces.push_back(i);
        Mat gray;
        cvtColor(images[i], gray, CV_BGR2GRAY);
        cornerSubPix(gray, corners, Size(11, 11), Size(-1, -1),
            TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));//0.1为精度
        imagePoints.push_back(corners);
        if (true) {
            drawChessboardCorners(images[i], grids, Mat(corners), found);
            //imshow("chessboard" + to_string(i), images[i]);
            //waitKey(200);
        }
    }
}

if (usefulImgIndeces.empty()) {
    cout << "no chessboard found." << endl;
    return false;
}

vector<vector<Point3f> > objectPoints = calcBoardCornerPositions(grids.width, grids.height, Grids_Size, imagePoints.size());
Size imageSize = images[0].size(); /*cout << images.size() << endl; cout << imagePoints.size() << endl; waitKey(100000);*/
vector<Mat> rvecs, tvecs; Mat  cameraMatrix; Mat distCoeffs; Mat Output;
double rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs);
FileStorage fs;
fs.open("result.xml", FileStorage::WRITE);
fs << "cameraMatrix" << cameraMatrix;
fs << "distCoeffs" << distCoeffs;
fs.release();
for (int i = 0; i < 20; i++)
{
    undistort(images[i], Output, cameraMatrix, distCoeffs);
    namedWindow("out" + to_string(i),0);
    imshow("out"+to_string(i), images[i]);
    //imshow("qqq", Output);
    waitKey(200);
}
waitKey();
cout << cameraMatrix << endl;
cout << distCoeffs << endl;



    waitKey();
    return 0;
}

下篇进入下一阶段。