三维重建代码实现(三)

写在开头

最近在学习三维重建的相关知识,打算将三维重建SFM的整个过程用代码的形式梳理一下,本章节主要实现相机标定代码。
这里我们假定你有一定的三维重建相关的基本知识,作者在这里推荐高翔博士的《视觉SLAM十四讲:从理论到实践》,在B站上有高翔博士的讲解视频。

程序流程

  1. 使用相机拍摄一系列的棋盘格的图片。
  2. 对每一个图片提取角点信息,对于角点信息不够精确,进一步提取亚像素角点信息;
  3. 在图片中提取出角点。
  4. 相机标定。
  5. 对标定结果评价,计算误差。
  6. 使用标定结果对原图片进行矫正。

步骤一:拍摄图片

标定图片一般来说需要我们在不同位置、不同角度、不同姿态下拍摄。在之前我们的数学推到中我们得知,一般来说我们需要至少3张图片才可以确定为一解。但是,通常我们使用10~20张图片来标定。

步骤二:提取角点信息和提取亚像素角点信息

OPENCV中提取棋盘格中内角点的函数:findChessboardCorners()

函数原型

CV_EXPORTS_W bool findChessboardCorners( InputArray image, Size patternSize, OutputArray corners,
                                         int flags = CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE );

函数说明:

这个函数的功能是确定输入图像中是否有棋盘格图案,并检测棋盘格的内角点。如果所有的内角点都找到了,那么函数返回一个非0值;如果没有找到所有的内角点,就会返回0。

参数说明:

  • image:输入的棋盘格图像,必须是8位的灰度或彩色图像。
  • patternSize:每一个图片中,每行和每列角点的个数。注意,每行和每列的角点个数不能相同,因为这样不易辨别方向。
  • corners:输入角点的坐标。通常用 cv:Point2f 向量来保存。
  • flags:默认为0。可选参数如下:
    • CALIB_CB_ADAPTIVE_THRESH 使用自适应阈值(通过平均图像亮度计算得到)将图像转换为黑白图,而不是一个固定的阈值。
    • CALIB_CB_NORMALIZE_IMAGE 在使用固定阈值或者自适应阈值进行二值化之前,先使用equalizeHist()来均衡化图像亮度。
    • CALIB_CB_FILTER_QUADS 使用其他的准则(如轮廓面积,周长,方形形状)来去除在轮廓检测阶段检测到的错误方块。

有两个函数可以实现提取亚像素角点信息:cornerSubPixfind4QuadCornerSubpix。在提取棋盘格角点时两者的效果差不多,随便使用哪一个都行。

函数原型

void cornerSubPix(InputArray image, InputOutputArray corners, Size winSize, Size zeroZone, TermCriteria criteria)

参数说明

  • image:输入图像。
  • corners:初始的角点坐标,同时也会作为亚像素角点坐标的输出;通常用cv::Point2f/Point2d 向量来保存,vector points
  • winSize:大小为搜索窗口的一半。
  • zeroZone:死区的一般尺寸,死区为不对搜索区的中央位置做求和运算的区域。
  • criteria:迭代的终止条件。

函数原型

CV_EXPORTS bool find4QuadCornerSubpix( InputArray img, InputOutputArray corners, Size region_size )

参数说明

  • img:输入图像,最好是8位灰度图像,检测效率更高;
  • corners:初始的角点坐标,同时也会作为亚像素角点坐标的输出;通常用cv::Point2f/Point2d 向量来保存,vector points
  • region_size:角点搜索窗口的大小。

步骤三:画出角点

函数原型

CV_EXPORTS_W void drawChessboardCorners( InputOutputArray image, Size patternSize, InputArray corners, bool patternWasFound )

函数说明

画出检测的角点。

参数说明

  • image:图像,8位灰度或彩色图像。
  • patternSize:每一幅棋盘格图片中,每行和每列角点的个数。
  • corners:初始的角点坐标,同时也会作为亚像素角点坐标的输出;通常用cv::Point2f/Point2d 向量来保存,vector points
  • patternWasFound:标志位,用来只是是否检测倒所有的棋盘内角点。true表示完整地检测到了所有内角点,函数会用直线将角点依次连接起来;false表示没有完整检测到所有内角点,函数会用红色圆圈标出检测到的内角点。

步骤四:相机标定

函数原型

CV_EXPORTS_W double calibrateCamera( InputArrayOfArrays objectPoints,
                                     InputArrayOfArrays imagePoints, Size imageSize,
                                     InputOutputArray cameraMatrix, InputOutputArray distCoeffs,
                                     OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
                                     int flags = 0, TermCriteria criteria = TermCriteria(
                                        TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) );

参数说明

  • objectPoints:一系列点的三维坐标,即若干张图片中对应的若干个点的三维坐标。在使用时应该建立一个二维的vector,即vector> objectPoints。我们需要根据棋盘格每个黑白格子的长宽,计算出各个内角点的三维坐标。通常我们会取z=0,而只计算x和y 坐标。
  • imagePoints:若干张图片对应的若干的内角点的坐标,通常采用vector> image_points表示。
  • imageSize:图像的像素尺寸大小。
  • cameraMatrix:相机的内参矩阵,对应推导时的内参矩阵,大小为。
  • distCoeffs:相机的畸变参数矩阵,有5个畸变参数:,矩阵大小为。
  • rvecs:旋转向量,罗德里格旋转向量,是相机外参;因为有若干张图片,所以通常使用Mat类型的vector 表示,vector rvecs
  • tvecs:位移向量,与旋转向量一样,也是相机外参,通常使用Mat类型的vector 表示,vector tvecs
  • flags:表示标定时采用的算法。默认为0,其他有:
    • CV_CALIB_USE_INTRINSIC_GUESS:使用该参数时,在cameraMatrix矩阵中应该有fx,fy,u0,v0的估计值。否则的话,将初始化(u0,v0)图像的中心点,使用最小二乘估算出fx,fy。
    • CV_CALIB_FIX_PRINCIPAL_POINT:在进行优化时会固定光轴点。当CV_CALIB_USE_INTRINSIC_GUESS参数被设置,光轴点将保持在中心或者某个输入的值。
    • CV_CALIB_FIX_ASPECT_RATIO:固定fx/fy的比值,只将fy作为可变量,进行优化计算。当CV_CALIB_USE_INTRINSIC_GUESS没有被设置,fx和fy将会被忽略。只有fx/fy的比值在计算中会被用到。
    • CV_CALIB_ZERO_TANGENT_DIST:设定切向畸变参数(p1,p2)为零。
    • CV_CALIB_FIX_K1,…,CV_CALIB_FIX_K6:对应的径向畸变在优化中保持不变。
    • CV_CALIB_RATIONAL_MODEL:计算k4,k5,k6三个畸变参数。如果没有设置,则只计算其它5个畸变参数。
  • criteria:迭代的终止条件。
    这个函数解决的就是我们以前推导的极大似然优化问题:

    其中:表示的是点在第个图像上的投影。

步骤五:评价标定结果

我们在进行相机标定时,本身要解决的是一个优化问题,而优化的对象就是角点与三维点投影到图像点坐标之间的差值,通过不断迭代,尽可能地最小化这个差值。我们对标定结果评价时,就是计算投影点与检测到的亚像素角点坐标的差值。由于是二维的,所以分别对x和y坐标求差值,再求平方根,即求L2范数。

函数原型

CV_EXPORTS_W void projectPoints( InputArray objectPoints,
                                 InputArray rvec, InputArray tvec,
                                 InputArray cameraMatrix, InputArray distCoeffs,
                                 OutputArray imagePoints,
                                 OutputArray jacobian = noArray(),
                                 double aspectRatio = 0 );

参数说明

  • objectPoints:一系列点的三维坐标,即若干张图片中对应的若干个点的三维坐标。在使用时应该建立一个二维的vector,即vector> objectPoints。我们需要根据棋盘格每个黑白格子的长宽,计算出各个内角点的三维坐标。通常我们会取z=0,而只计算x和y 坐标。
  • rvecs:旋转向量,罗德里格旋转向量,是相机外参;因为有若干张图片,所以通常使用Mat类型的vector 表示,vector rvecs
  • tvecs:位移向量,与旋转向量一样,也是相机外参,通常使用Mat类型的vector 表示,vector tvecs。
  • cameraMatrix:相机的内参矩阵,对应推导时的内参矩阵A,大小为。
  • distCoeffs:相机的畸变参数矩阵,有5个畸变参数:
    ,矩阵大小为。
  • imagePoints:若干张图片对应的若干的内角点的坐标,通常采用vector> image_points表示。

步骤六:矫正图像

使用前面求得的内参和外参以及畸变参数数据,可以对图像进行畸变矫正。使用initUndistortRectifyMapremap两个函数来实现。initUndistortRectifyMap用来计算畸变映射,remap把求得的映射应用到图像上。

函数原型

CV_EXPORTS_W void initUndistortRectifyMap( InputArray cameraMatrix, InputArray distCoeffs,
                           InputArray R, InputArray newCameraMatrix,
                           Size size, int m1type, OutputArray map1, OutputArray map2 );

参数说明

  • cameraMatrix:相机的内参矩阵;
  • distCoeffs:相机的畸变参数构成的矩阵;
  • R:可选的输入,是第一和第二相机坐标之间的旋转矩阵;
  • newCameraMatrix:校正后的内参矩阵;
  • size:摄像机采集的无失真的图像尺寸;
  • m1type:定义map1的数据类型,可以是CV_32FC1或者CV_16SC2;
  • map1map2:分别对应X和Y坐标的重映射参数。

函数原型

CV_EXPORTS_W void remap( InputArray src, OutputArray dst,
                         InputArray map1, InputArray map2,
                         int interpolation, int borderMode = BORDER_CONSTANT,
                         const Scalar& borderValue = Scalar());

参数说明

  • src:输入图像,原始有畸变的图像;
  • dst:输出图像,校正后的图像;
  • map1:X坐标的映射;
  • map2:Y坐标的映射;
  • interpolation:图像的插值方式;
  • borderMode:边界填充方式;

Code

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;
using namespace cv;

int main()
{
    ifstream inImgPath("calibdata.txt");    //标定所用图像文件的路径
    vector imgList;
    vector::iterator p;
    string temp;
    if (!inImgPath.is_open())
    {
        cout << "没有找到文件" << endl;
    }
    //读取文件中保存的图片文件路径,并存放在数组中
    while (getline(inImgPath, temp))
    {
        imgList.push_back(temp);
    }

    ofstream fout("caliberation_result.txt");   //保存标定结果的文件

    cout << "开始提取角点......" << endl;
    cv::Size image_size;//保存图片大小
    cv::Size pattern_size = cv::Size(4, 6);//标定板上每行、每列的角点数;测试图片中的标定板上内角点数为4*6
    vector corner_points_buf;//建一个数组缓存检测到的角点,通常采用Point2f形式
    vector::iterator corner_points_buf_ptr;
    vector> corner_points_of_all_imgs;
    int image_num = 0;
    string filename;
    while(image_num < imgList.size())
    {
        filename = imgList[image_num++];
        cout << "image_num = " << image_num << endl;
        cout << filename.c_str() << endl;
        cv::Mat imageInput = cv::imread(filename.c_str());
        if (image_num == 1)
        {
            image_size.width = imageInput.cols;
            image_size.height = imageInput.rows;
            cout << "image_size.width = " << image_size.width << endl;
            cout << "image_size.height = " << image_size.height << endl;
        }

        if (findChessboardCorners(imageInput, pattern_size, corner_points_buf) == 0)
        {
            cout << "can not find chessboard corners!\n";   //找不到角点
            exit(1);
        }
        else
        {
            cv::Mat gray;
            cv::cvtColor(imageInput, gray, CV_RGB2GRAY);
            cv::find4QuadCornerSubpix(gray, corner_points_buf, cv::Size(5, 5));
            corner_points_of_all_imgs.push_back(corner_points_buf);
            cv::drawChessboardCorners(gray, pattern_size, corner_points_buf, true);
            cv::imshow("camera calibration", gray);
            cv::waitKey(100);
        }
    }

    int total = corner_points_of_all_imgs.size();
    cout << "total=" << total << endl;
    int cornerNum = pattern_size.width * pattern_size.height;//每张图片上的总的角点数
    for (int i = 0; i < total;i++)
    {
        cout << "--> 第" << i + 1 << "幅图片的数据 -->:" << endl;
        for (int j = 0;j < cornerNum;j++)
        {
            cout << "-->" << corner_points_of_all_imgs[i][j].x;
            cout << "-->" << corner_points_of_all_imgs[i][j].y;
            if ((j + 1) % 3 == 0)
            {
                cout << endl;
            }
            else
            {
                cout.width(10);
            }
        }
        cout << endl;
    }

    cout << endl << "角点提取完成" << endl;

    //摄像机标定
    cout << "开始标定………………" << endl;
    cv::Mat cameraMatrix = cv::Mat(3, 3, CV_32FC1, cv::Scalar::all(0));//内外参矩阵,H——单应性矩阵
    cv::Mat distCoefficients = cv::Mat(1, 5, CV_32FC1, cv::Scalar::all(0));//摄像机的5个畸变系数:k1,k2,p1,p2,k3
    vector tvecsMat;//每幅图像的平移向量,t
    vector rvecsMat;//每幅图像的旋转向量(罗德里格旋转向量)
    vector> objectPoints;//保存所有图片的角点的三维坐标
                                             //初始化每一张图片中标定板上角点的三维坐标
    int i, j, k;
    for (k = 0;k < image_num;k++)//遍历每一张图片
    {
        vector tempCornerPoints;//每一幅图片对应的角点数组
        //遍历所有的角点
        for (i = 0;i < pattern_size.height;i++)
        {
            for (j = 0;j < pattern_size.width;j++)
            {
                cv::Point3f singleRealPoint;//一个角点的坐标
                singleRealPoint.x = i * 10;
                singleRealPoint.y = j * 10;
                singleRealPoint.z = 0;//假设z=0
                tempCornerPoints.push_back(singleRealPoint);
            }
        }
        objectPoints.push_back(tempCornerPoints);
    }

    cv::calibrateCamera(objectPoints, corner_points_of_all_imgs, image_size, cameraMatrix, distCoefficients, rvecsMat, tvecsMat, 0);
    cout << "标定完成" << endl;

    //开始保存标定结果
    cout << "开始保存标定结果" << endl;

    cout << endl << "相机相关参数:" << endl;
    fout << "相机相关参数:" << endl;
    cout << "1.内外参数矩阵:" << endl;
    fout << "1.内外参数矩阵:" << endl;
    cout << "大小:" << cameraMatrix.size() << endl;
    fout << "大小:" << cameraMatrix.size() << endl;
    cout << cameraMatrix << endl;
    fout << cameraMatrix << endl;

    cout << "2.畸变系数:" << endl;
    fout << "2.畸变系数:" << endl;
    cout << "大小:" << distCoefficients.size() << endl;
    fout << "大小:" << distCoefficients.size() << endl;
    cout << distCoefficients << endl;
    fout << distCoefficients << endl;

    cout << endl << "图像相关参数:" << endl;
    fout << endl << "图像相关参数:" << endl;
    cv::Mat rotation_Matrix = cv::Mat(3, 3, CV_32FC1, cv::Scalar::all(0));//旋转矩阵
    for (i = 0;i < image_num;i++)
    {
        cout << "第" << i + 1 << "幅图像的旋转向量:" << endl;
        fout << "第" << i + 1 << "幅图像的旋转向量:" << endl;
        cout << rvecsMat[i] << endl;
        fout << rvecsMat[i] << endl;
        cout << "第" << i + 1 << "幅图像的旋转矩阵:" << endl;
        fout << "第" << i + 1 << "幅图像的旋转矩阵:" << endl;
        cv::Rodrigues(rvecsMat[i], rotation_Matrix);//将旋转向量转换为相对应的旋转矩阵
        cout << rotation_Matrix << endl;
        fout << rotation_Matrix << endl;
        cout << "第" << i + 1 << "幅图像的平移向量:" << endl;
        fout << "第" << i + 1 << "幅图像的平移向量:" << endl;
        cout << tvecsMat[i] << endl;
        fout << tvecsMat[i] << endl;
    }

    cout << "结果保存完毕" << endl;

    //对标定结果进行评价
    cout << "开始评价标定结果......" << endl;

    //计算每幅图像中的角点数量,假设全部角点都检测到了
    int corner_points_counts;
    corner_points_counts = pattern_size.width * pattern_size.height;

    cout << "每幅图像的标定误差:" << endl;
    fout << "每幅图像的标定误差:" << endl;
    double err = 0;//单张图像的误差
    double total_err = 0;//所有图像的平均误差
    for (i = 0;i < image_num;i++)
    {
        vector image_points_calculated;//存放新计算出的投影点的坐标
        vector tempPointSet = objectPoints[i];
        cv::projectPoints(tempPointSet, rvecsMat[i], tvecsMat[i], cameraMatrix, distCoefficients, image_points_calculated);

        //计算新的投影点与旧的投影点之间的误差
        vector image_points_old = corner_points_of_all_imgs[i];
        //将两组数据换成Mat格式
        cv::Mat image_points_calculated_mat = cv::Mat(1, image_points_calculated.size(), CV_32FC2);
        cv::Mat image_points_old_mat = cv::Mat(1, image_points_old.size(), CV_32FC2);
        for (j = 0;j < tempPointSet.size();j++)
        {
            image_points_calculated_mat.at(0, j) = cv::Vec2f(image_points_calculated[j].x, image_points_calculated[j].y);
            image_points_old_mat.at(0, j) = cv::Vec2f(image_points_old[j].x, image_points_old[j].y);
        }
        err = cv::norm(image_points_calculated_mat, image_points_old_mat, cv::NORM_L2);
        err /= corner_points_counts;
        total_err += err;
        cout << "第" << i + 1 << "幅图像的平均误差:" << err << "像素" << endl;
        fout << "第" << i + 1 << "幅图像的平均误差:" << err << "像素" << endl;
    }
    cout << "总体平均误差:" << total_err / image_num << "像素" << endl;
    fout << "总体平均误差:" << total_err / image_num << "像素" << endl;
    cout << "评价完成" << endl;

    fout.close();

    cv::Mat mapx = cv::Mat(image_size, CV_32FC1);
    cv::Mat mapy = cv::Mat(image_size, CV_32FC1);
    cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
    cout << "保存矫正图像" << endl;
    string imageFileName;
    std::stringstream StrStm;
    for (int i = 0;i < image_num;i++)
    {
        cout << "Frame #" << i + 1 << endl;
        cv::initUndistortRectifyMap(cameraMatrix, distCoefficients, R, cameraMatrix, image_size, CV_32FC1, mapx, mapy);
        cv::Mat src_image = cv::imread(imgList[i].c_str(), 1);
        cv::Mat new_image = src_image.clone();
        cv::remap(src_image, new_image, mapx, mapy, cv::INTER_LINEAR);
        imshow("原始图像", src_image);
        imshow("矫正后图像", new_image);

        StrStm.clear();
        imageFileName.clear();
        StrStm << i + 1;
        StrStm >> imageFileName;
        imageFileName += "_d.jpg";
        cv::imwrite(imageFileName, new_image);

        cv::waitKey(200);
    }
    cout << "保存结束" << endl;

    cv::waitKey(0);

    return 0;
}

你可能感兴趣的:(三维重建代码实现(三))