结构光：投影仪的标定（OpenCV）

最近看了一下opencv3.0的源码，之前很苦恼投影仪的标定，opencv提供了一个demo，上传源码：

#include <opencv2/highgui.hpp>  
#include <vector>  
#include <iostream>  
#include <fstream>  
  
#include <opencv2/core.hpp>  
#include <opencv2/core/utility.hpp>  
#include <opencv2/imgproc.hpp>  
#include <opencv2/calib3d.hpp>  
  
using namespace std;  
using namespace cv;  
  
static const char* keys =  
{  
    "{@camSettingsPath | | Path of camera calibration file}"  
    "{@projSettingsPath | | Path of projector settings}"  
    "{@patternPath | | Path to checkerboard pattern}"  
    "{@outputName | | Base name for the calibration data}"  
};  
  
static void help()  
{  
    cout << "\nThis example calibrates a camera and a projector" << endl;  
    cout << "To call: ./example_structured_light_projectorcalibration <cam_settings_path> "  
        " <proj_settings_path> <chessboard_path> <calibration_basename>"  
        " cam settings are parameters about the chessboard that needs to be detected to"  
        " calibrate the camera and proj setting are the same kind of parameters about the chessboard"  
        " that needs to be detected to calibrate the projector" << endl;  
}  
enum calibrationPattern  
{     
    CHESSBOARD,   
    CIRCLES_GRID,  
    ASYMETRIC_CIRCLES_GRID   
};  
  
struct Settings  
{  
    Settings();  
    int patternType;  
    Size patternSize;  
    Size subpixelSize;  
    Size imageSize;  
    float squareSize;  
    int nbrOfFrames;  
};  
  
void loadSettings(String path, Settings &sttngs);  
void createObjectPoints(vector<Point3f> &patternCorners, Size patternSize, float squareSize,  
    int patternType);  
void createProjectorObjectPoints(vector<Point2f> &patternCorners, Size patternSize, float squareSize,  
    int patternType);  
double calibrate(vector< vector<Point3f> > objPoints, vector< vector<Point2f> > imgPoints,  
    Mat &cameraMatrix, Mat &distCoeffs, vector<Mat> &r, vector<Mat> &t, Size imgSize);  
void fromCamToWorld(Mat cameraMatrix, vector<Mat> rV, vector<Mat> tV,  
    vector< vector<Point2f> > imgPoints, vector< vector<Point3f> > &worldPoints);  
void saveCalibrationResults(String path, Mat camK, Mat camDistCoeffs, Mat projK, Mat projDistCoeffs,  
    Mat fundamental);  
void saveCalibrationData(String path, vector<Mat> T1, vector<Mat> T2, vector<Mat> ptsProjCam, vector<Mat> ptsProjProj, vector<Mat> ptsProjCamN, vector<Mat> ptsProjProjN);  
void normalize(const Mat &pts, const int& dim, Mat& normpts, Mat &T);  
void fromVectorToMat(vector<Point2f> v, Mat &pts);  
void fromMatToVector(Mat pts, vector<Point2f> &v);  
  
int main(int argc, char **argv)  
{  
    VideoCapture cap(CAP_PVAPI);  
    Mat frame;  
  
    int nbrOfValidFrames = 0;  
  
    vector< vector<Point2f> > imagePointsCam, imagePointsProj, PointsInProj, imagePointsProjN, pointsInProjN;  
    vector< vector<Point3f> > objectPointsCam, worldPointsProj;  
    vector<Point3f> tempCam;  
    vector<Point2f> tempProj;  
    vector<Mat> T1, T2;  
    vector<Mat> projInProj, projInCam;  
    vector<Mat> projInProjN, projInCamN;  
  
    vector<Mat> rVecs, tVecs, projectorRVecs, projectorTVecs;  
    Mat cameraMatrix, distCoeffs, projectorMatrix, projectorDistCoeffs;  
    Mat pattern;  
    vector<Mat> images;  
  
    Settings camSettings, projSettings;  
  
    //根据实际情况更改  
    CommandLineParser parser(argc, argv, keys);  
    String camSettingsPath = parser.get<String>(0);  
    String projSettingsPath = parser.get<String>(1);  
    String patternPath = parser.get<String>(2);  
    String outputName = parser.get<String>(3);  
  
    if (camSettingsPath.empty() || projSettingsPath.empty() || patternPath.empty() || outputName.empty())  
    {  
        //help();  
        return -1;  
    }  
  
    pattern = imread(patternPath);  
  
    loadSettings(camSettingsPath, camSettings);  
    loadSettings(projSettingsPath, projSettings);  
  
    projSettings.imageSize = Size(pattern.rows, pattern.cols);  
  
    createObjectPoints(tempCam, camSettings.patternSize,  
        camSettings.squareSize, camSettings.patternType);  
    createProjectorObjectPoints(tempProj, projSettings.patternSize,  
        projSettings.squareSize, projSettings.patternType);  
  
    if (!cap.isOpened())  
    {  
        cout << "Camera could not be opened" << endl;  
        return -1;  
    }  
    cap.set(CAP_PROP_PVAPI_PIXELFORMAT, CAP_PVAPI_PIXELFORMAT_BAYER8);  
  
    namedWindow("pattern", WINDOW_NORMAL);  
    setWindowProperty("pattern", WND_PROP_FULLSCREEN, WINDOW_FULLSCREEN);  
  
    namedWindow("camera view", WINDOW_NORMAL);  
  
    imshow("pattern", pattern);  
    cout << "Press any key when ready" << endl;  
    waitKey(0);  
  
    while (nbrOfValidFrames < camSettings.nbrOfFrames)  
    {  
        cap >> frame;  
        if (frame.data)  
        {  
            Mat color;  
            cvtColor(frame, color, COLOR_BayerBG2BGR);  
            if (camSettings.imageSize.height == 0 || camSettings.imageSize.width == 0)  
            {  
                camSettings.imageSize = Size(frame.rows, frame.cols);  
            }  
  
            bool foundProj, foundCam;  
  
            vector<Point2f> projPointBuf;  
            vector<Point2f> camPointBuf;  
  
            imshow("camera view", color);  
            if (camSettings.patternType == CHESSBOARD && projSettings.patternType == CHESSBOARD)  
            {  
                int calibFlags = CALIB_CB_ADAPTIVE_THRESH;  
  
                foundCam = findChessboardCorners(color, camSettings.patternSize,  
                    camPointBuf, calibFlags);  
  
                foundProj = findChessboardCorners(color, projSettings.patternSize,  
                    projPointBuf, calibFlags);  
  
                if (foundCam && foundProj)  
                {  
                    Mat gray;  
                    cvtColor(color, gray, COLOR_BGR2GRAY);  
                    cout << "found pattern" << endl;  
                    Mat projCorners, camCorners;  
                    cornerSubPix(gray, camPointBuf, camSettings.subpixelSize, Size(-1, -1),  
                        TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 30, 0.1));  
  
                    cornerSubPix(gray, projPointBuf, projSettings.subpixelSize, Size(-1, -1),  
                        TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 30, 0.1));  
  
                    drawChessboardCorners(gray, camSettings.patternSize, camPointBuf, foundCam);  
                    drawChessboardCorners(gray, projSettings.patternSize, projPointBuf, foundProj);  
  
                    imshow("camera view", gray);  
                    char c = (char)waitKey(0);  
                    if (c == 10)  
                    {  
                        cout << "saving pattern #" << nbrOfValidFrames << " for calibration" << endl;  
                        ostringstream name;  
                        name << nbrOfValidFrames;  
                        nbrOfValidFrames += 1;  
  
                        imagePointsCam.push_back(camPointBuf);  
                        imagePointsProj.push_back(projPointBuf);  
                        objectPointsCam.push_back(tempCam);  
                        PointsInProj.push_back(tempProj);  
                        images.push_back(frame);  
  
                        Mat ptsProjProj, ptsProjCam;  
                        Mat ptsProjProjN, ptsProjCamN;  
                        Mat TProjProj, TProjCam;  
                        vector<Point2f> ptsProjProjVec;  
                        vector<Point2f> ptsProjCamVec;  
  
                        fromVectorToMat(tempProj, ptsProjProj);  
                        normalize(ptsProjProj, 2, ptsProjProjN, TProjProj);  
                        fromMatToVector(ptsProjProjN, ptsProjProjVec);  
                        pointsInProjN.push_back(ptsProjProjVec);  
                        T2.push_back(TProjProj);  
                        projInProj.push_back(ptsProjProj);  
                        projInProjN.push_back(ptsProjProjN);  
  
                        fromVectorToMat(projPointBuf, ptsProjCam);  
                        normalize(ptsProjCam, 2, ptsProjCamN, TProjCam);  
                        fromMatToVector(ptsProjCamN, ptsProjCamVec);  
                        imagePointsProjN.push_back(ptsProjCamVec);  
                        T1.push_back(TProjCam);  
                        projInCam.push_back(ptsProjCam);  
                        projInCamN.push_back(ptsProjCamN);  
  
                    }  
                    else if (c == 32)  
                    {  
                        cout << "capture discarded" << endl;  
                    }  
                    else if (c == 27)  
                    {  
                        cout << "closing program" << endl;  
                        return -1;  
                    }  
                }  
                else  
                {  
                    cout << "no pattern found, move board and press any key" << endl;  
                    imshow("camera view", frame);  
                    waitKey(0);  
                }  
            }  
        }  
    }  
  
    saveCalibrationData(outputName + "_points.yml", T1, T2, projInCam, projInProj, projInCamN, projInProjN);  
  
    double rms = calibrate(objectPointsCam, imagePointsCam, cameraMatrix, distCoeffs,  
        rVecs, tVecs, camSettings.imageSize);  
  
    cout << "rms = " << rms << endl;  
    cout << "camera matrix = \n" << cameraMatrix << endl;  
    cout << "dist coeffs = \n" << distCoeffs << endl;  
  
    fromCamToWorld(cameraMatrix, rVecs, tVecs, imagePointsProj, worldPointsProj);  
  
    rms = calibrate(worldPointsProj, PointsInProj, projectorMatrix, projectorDistCoeffs,  
        projectorRVecs, projectorTVecs, projSettings.imageSize);  
  
    cout << "rms = " << rms << endl;  
    cout << "projector matrix = \n" << projectorMatrix << endl;  
    cout << "projector dist coeffs = \n" << distCoeffs << endl;  
  
    Mat stereoR, stereoT, essential, fundamental;  
    Mat RCam, RProj, PCam, PProj, Q;  
    rms = stereoCalibrate(worldPointsProj, imagePointsProj, PointsInProj, cameraMatrix, distCoeffs,  
        projectorMatrix, projectorDistCoeffs, camSettings.imageSize, stereoR, stereoT,  
        essential, fundamental);  
  
    cout << "stereo calibrate: \n" << fundamental << endl;  
  
    saveCalibrationResults(outputName, cameraMatrix, distCoeffs, projectorMatrix, projectorDistCoeffs, fundamental);  
    return 0;  
}  
  
Settings::Settings()  
{  
    patternType = CHESSBOARD;  
    patternSize = Size(13, 9);  
    subpixelSize = Size(11, 11);  
    squareSize = 50;  
    nbrOfFrames = 25;  
}  
  
void loadSettings(String path, Settings &sttngs)  
{  
    FileStorage fsInput(path, FileStorage::READ);  
  
    fsInput["PatternWidth"] >> sttngs.patternSize.width;  
    fsInput["PatternHeight"] >> sttngs.patternSize.height;  
    fsInput["SubPixelWidth"] >> sttngs.subpixelSize.width;  
    fsInput["SubPixelHeight"] >> sttngs.subpixelSize.height;  
    fsInput["SquareSize"] >> sttngs.squareSize;  
    fsInput["NbrOfFrames"] >> sttngs.nbrOfFrames;  
    fsInput["PatternType"] >> sttngs.patternType;  
    fsInput.release();  
}  
  
double calibrate(vector< vector<Point3f> > objPoints, vector< vector<Point2f> > imgPoints,  
    Mat &cameraMatrix, Mat &distCoeffs, vector<Mat> &r, vector<Mat> &t, Size imgSize)  
{  
    int calibFlags = 0;  
  
    double rms = calibrateCamera(objPoints, imgPoints, imgSize, cameraMatrix,  
        distCoeffs, r, t, calibFlags);  
  
    return rms;  
}  
  
void createObjectPoints(vector<Point3f> &patternCorners, Size patternSize, float squareSize,  
    int patternType)  
{  
    switch (patternType)  
    {  
    case CHESSBOARD:  
    case CIRCLES_GRID:  
        for (int i = 0; i < patternSize.height; ++i)  
        {  
            for (int j = 0; j < patternSize.width; ++j)  
            {  
                patternCorners.push_back(Point3f(float(i*squareSize), float(j*squareSize), 0));  
            }  
        }  
        break;  
    case ASYMETRIC_CIRCLES_GRID:  
        break;  
    }  
}  
  
void createProjectorObjectPoints(vector<Point2f> &patternCorners, Size patternSize, float squareSize,  
    int patternType)  
{  
    switch (patternType)  
    {  
    case CHESSBOARD:  
    case CIRCLES_GRID:  
        for (int i = 1; i <= patternSize.height; ++i)  
        {  
            for (int j = 1; j <= patternSize.width; ++j)  
            {  
                patternCorners.push_back(Point2f(float(j*squareSize), float(i*squareSize)));  
            }  
        }  
        break;  
    case ASYMETRIC_CIRCLES_GRID:  
        break;  
    }  
}  
  
void fromCamToWorld(Mat cameraMatrix, vector<Mat> rV, vector<Mat> tV,  
    vector< vector<Point2f> > imgPoints, vector< vector<Point3f> > &worldPoints)  
{  
    int s = (int)rV.size();  
    Mat invK64, invK;  
    invK64 = cameraMatrix.inv();  
    invK64.convertTo(invK, CV_32F);  
  
    for (int i = 0; i < s; ++i)  
    {  
        Mat r, t, rMat;  
        rV[i].convertTo(r, CV_32F);  
        tV[i].convertTo(t, CV_32F);  
  
        Rodrigues(r, rMat);  
        Mat transPlaneToCam = rMat.inv()*t;  
  
        vector<Point3f> wpTemp;  
        int s2 = (int)imgPoints[i].size();  
        for (int j = 0; j < s2; ++j){  
            Mat coords(3, 1, CV_32F);  
            coords.at<float>(0, 0) = imgPoints[i][j].x;  
            coords.at<float>(1, 0) = imgPoints[i][j].y;  
            coords.at<float>(2, 0) = 1.0f;  
  
            Mat worldPtCam = invK*coords;  
            Mat worldPtPlane = rMat.inv()*worldPtCam;  
  
            float scale = transPlaneToCam.at<float>(2) / worldPtPlane.at<float>(2);  
            Mat worldPtPlaneReproject = scale*worldPtPlane - transPlaneToCam;  
  
            Point3f pt;  
            pt.x = worldPtPlaneReproject.at<float>(0);  
            pt.y = worldPtPlaneReproject.at<float>(1);  
            pt.z = 0;  
            wpTemp.push_back(pt);  
        }  
        worldPoints.push_back(wpTemp);  
    }  
}  
  
void saveCalibrationResults(String path, Mat camK, Mat camDistCoeffs, Mat projK, Mat projDistCoeffs,  
    Mat fundamental)  
{  
    FileStorage fs(path + ".yml", FileStorage::WRITE);  
    fs << "camIntrinsics" << camK;  
    fs << "camDistCoeffs" << camDistCoeffs;  
    fs << "projIntrinsics" << projK;  
    fs << "projDistCoeffs" << projDistCoeffs;  
    fs << "fundamental" << fundamental;  
    fs.release();  
}  
  
void saveCalibrationData(String path, vector<Mat> T1, vector<Mat> T2, vector<Mat> ptsProjCam, vector<Mat> ptsProjProj, vector<Mat> ptsProjCamN, vector<Mat> ptsProjProjN)  
{  
    FileStorage fs(path + ".yml", FileStorage::WRITE);  
  
    int size = (int)T1.size();  
    fs << "size" << size;  
    for (int i = 0; i < (int)T1.size(); ++i)  
    {  
        ostringstream nbr;  
        nbr << i;  
        fs << "TprojCam" + nbr.str() << T1[i];  
        fs << "TProjProj" + nbr.str() << T2[i];  
        fs << "ptsProjCam" + nbr.str() << ptsProjCam[i];  
        fs << "ptsProjProj" + nbr.str() << ptsProjProj[i];  
        fs << "ptsProjCamN" + nbr.str() << ptsProjCamN[i];  
        fs << "ptsProjProjN" + nbr.str() << ptsProjProjN[i];  
    }  
    fs.release();  
  
}  
  
void normalize(const Mat &pts, const int& dim, Mat& normpts, Mat &T)  
{  
    float averagedist = 0;  
    float scale = 0;  
  
    //centroid  
    Mat centroid(dim, 1, CV_32F);  
    Scalar tmp;  
  
    if (normpts.empty())  
    {  
        normpts = Mat(pts.rows, pts.cols, CV_32F);  
    }  
  
    for (int i = 0; i < dim; ++i)  
    {  
        tmp = mean(pts.row(i));  
        centroid.at<float>(i, 0) = (float)tmp[0];  
        subtract(pts.row(i), centroid.at<float>(i, 0), normpts.row(i));  
    }  
  
    //average distance  
    Mat ptstmp;  
    for (int i = 0; i < normpts.cols; ++i)  
    {  
        ptstmp = normpts.col(i);  
        averagedist = averagedist + (float)norm(ptstmp);  
    }  
    averagedist = averagedist / normpts.cols;  
    scale = (float)(sqrt(dim) / averagedist);  
  
    normpts = normpts * scale;  
  
    T = cv::Mat::eye(dim + 1, dim + 1, CV_32F);  
    for (int i = 0; i < dim; ++i)  
    {  
        T.at<float>(i, i) = scale;  
        T.at<float>(i, dim) = -scale*centroid.at<float>(i, 0);  
    }  
}  
  
void fromVectorToMat(vector<Point2f> v, Mat &pts)  
{  
    int nbrOfPoints = (int)v.size();  
  
    if (pts.empty())  
        pts.create(2, nbrOfPoints, CV_32F);  
  
    for (int i = 0; i < nbrOfPoints; ++i)  
    {  
        pts.at<float>(0, i) = v[i].x;  
        pts.at<float>(1, i) = v[i].y;  
    }  
}  
  
void fromMatToVector(Mat pts, vector<Point2f> &v)  
{  
    int nbrOfPoints = pts.cols;  
  
    for (int i = 0; i < nbrOfPoints; ++i)  
    {  
        Point2f temp;  
        temp.x = pts.at<float>(0, i);  
        temp.y = pts.at<float>(1, i);  
        v.push_back(temp);  
    }  
}