OpenCV联合C++/Qt 学习笔记(二十三)----图像校正及单目位姿估计

一、图像校正

图像校正（Image Undistortion）是指：

利用相机标定得到的内参与畸变参数，对畸变图像进行几何恢复，使图像尽可能接近真实成像效果。

1、去畸变函数

cpp 复制代码

/* 用途：用于根据相机标定得到的内参矩阵和畸变系数，对图像进行镜头畸变校正。
   该函数能够消除由于镜头造成的：桶形畸变、枕形畸变、切向畸变，
   从而恢复真实场景中的几何形状。 */
void cv::undistort( InputArray src, OutputArray dst,
                             InputArray cameraMatrix,
                             InputArray distCoeffs,
                             InputArray newCameraMatrix = noArray() );
/*
src：输入待去畸变的原始图像
dst：输出去畸变后的图像，图像尺寸与数据类型通常与输入图像相同
cameraMatrix：相机内参矩阵，即相机标定得到的K矩阵
distCoeffs：相机畸变系数，根据近似模型不同，参数数量可以为4、5、8、12或者14，如果
            是空矩阵表示没有畸变
newCameraMatrix：畸变图像的相机内参矩阵，一般情况下与第三个参数相同或者使用默认值
*/

2、示例代码

cpp 复制代码

void undist(vector<Mat> imgs,/*所有原图像向量*/
    Mat cameraMatrix,/*计算得到的相机内参*/
    Mat distCoeffs,/*计算得到的相机畸变系数*/
    vector<Mat>& undistImgs/*校正后的输出图像*/)
{
    for (int i = 0; i < imgs.size(); i++)
    {
        Mat undistImg;
        undistort(imgs[i], undistImg, cameraMatrix, distCoeffs);
        undistImgs.push_back(undistImg);
    }
}
/*******************************************************************************/
    QString imgPath = QApplication::applicationDirPath() + "/Images";
    cv::String s_imgPath = imgPath.toLocal8Bit().data();
    /*读取所有图像*/
    vector<Mat> imgs;
    string imageName;
    ifstream fin(s_imgPath + "/calibdata.txt");

    while (getline(fin, imageName))
    {
        string fullPath = s_imgPath + "/" + imageName;

        cv::Mat img = cv::imread(fullPath);/*left01.jpg left02.jpg left03.jpg left04.jpg*/

        if (img.empty())
        {
            qDebug() << "读取失败:" << QString::fromStdString(fullPath);
            continue;
        }
        imgs.push_back(img);
    }
    /*输入内参矩阵*/
    Mat cameraMatrix = (Mat_<float>(3, 3) << 532.016297, 0, 332.172519,
        0, 531.565159, 233.388075,
        0, 0, 1);
    /*输入畸变系数*/
    Mat distCoeffs = (Mat_<float>(1, 5) << -0.285188, 0.080097, 0.001274, -0.002415, 0.106579);

    vector<Mat> undistImgs;

    //Size imageSize;
    //imageSize.width = imgs[0].cols;
    //imageSize.height = imgs[0].rows;

    /*用undistort()函数直接计算校正图像*/
    undist(imgs, cameraMatrix, distCoeffs, undistImgs);
    /*显示校正前后的图像*/
    for (int i = 0; i < imgs.size(); i++)
    {
        string windowNmber = to_string(i);
        imshow("before Image" + windowNmber, imgs[i]);
        imshow("after Image" + windowNmber, undistImgs[i]);
        waitKey(0);
        destroyWindow("before Image" + windowNmber);
        destroyWindow("after Image" + windowNmber);
    }

二、单目位姿估计

1、单目位姿估计介绍

在计算机视觉中，单目位姿估计通常指：

使用一台普通摄像机，通过图像信息计算目标相对于相机的位置和旋转状态。

其中：

位置：目标在三维空间中的坐标：平移向量 t = [tx, ty, tz]

姿态：目标绕 X、Y、Z 轴的旋转角度：旋转矩阵 R

因此，位姿估计本质上就是求解：

其中：

：世界坐标系中的点

：相机坐标系中的点

R：旋转矩阵

t：平移向量

对比项	相机标定	单目位姿估计
核心目标	求解相机内部参数	求解目标相对于相机的位置与姿态
本质	建立相机成像模型	建立目标与相机空间关系
求解内容	内参矩阵 K、畸变参数 D	外参 R、t
是否涉及空间位置	不直接求目标位置	直接求三维空间位姿
输入数据	多张标定板图像	目标3D点与图像2D点
主要依赖	标定板角点	2D-3D特征对应
输出结果	焦距、主点、畸变系数	旋转矩阵、平移向量
坐标关系	像素坐标 ↔ 相机坐标	世界坐标 ↔ 相机坐标
数学核心	相机投影模型	PnP空间变换
是否必须	位姿估计前通常必须完成	依赖标定结果
OpenCV核心函数	`calibrateCamera()`	`solvePnP()`
输出矩阵	相机内参矩阵	外参矩阵
是否需要已知尺寸	需要标定板真实尺寸	需要目标真实三维坐标
精度影响因素	角点精度、标定图像质量	特征匹配、PnP算法
是否需要去畸变	标定负责求畸变参数	位姿估计通常需先去畸变
结果是否固定	同一相机基本固定	每帧图像都会变化
典型应用	相机矫正、测量	AR、机器人定位、目标跟踪
二者关系	提供内参与畸变参数	基于标定结果计算位姿
[单目位姿估计与相机标定关系]

2、相关函数

cpp 复制代码

/* 返回值：bool类型，true表示位姿求解成功，false表示求解失败
   用途：用于根据："三维世界点 ↔ 二维图像点"的对应关系，计算目标相对于相机的空间位姿。
   即求解：目标的旋转姿态、目标的空间位置 */
bool cv::solvePnP( InputArray objectPoints, InputArray imagePoints,
                InputArray cameraMatrix, InputArray distCoeffs,
                OutputArray rvec, OutputArray tvec,
                bool useExtrinsicGuess = false, int flags = SOLVEPNP_ITERATIVE );
/*
objectPoints：世界坐标系中的三维点集合，通常为目标物体上的3D特征点坐标
imagePoints：图像中的二维像素点集合，与objectPoints中的点一一对应
cameraMatrix：相机内参矩阵，通常由calibrateCamera()获得
distCoeffs：相机畸变系数，通常由calibrateCamera()获得
rvec：输出旋转向量，表示目标相对于相机的旋转姿态
tvec：输出平移向量，表示目标相对于相机的位置
useExtrinsicGuess：是否使用输入的旋转向量初值和平移向量初值的标志，
        true表示使用已有rvec和tvec作为初始值进行优化
flags：PnP求解算法类型标志，常见方法包括：
        SOLVEPNP_ITERATIVE：迭代优化方法（最常用）
        SOLVEPNP_P3P：P3P方法，至少需要4个点
        SOLVEPNP_AP3P：改进P3P算法
        SOLVEPNP_EPNP：EPnP高效求解算法
        SOLVEPNP_IPPE：平面目标专用方法
*/

3、示例代码

cpp 复制代码

    QString imgPath = QApplication::applicationDirPath() + "/Images";
    cv::String s_imgPath = imgPath.toLocal8Bit().data();
    Mat img = imread(s_imgPath + "/left01.jpg");
    if (img.empty())
    {
        qDebug() << "图片加载失败, 请确认图像文件名称是否正确";
        return;
    }
    Mat gray;
    cvtColor(img, gray, COLOR_BGR2GRAY);
    vector<Point2f> imgPoints;
    Size boardSize = Size(9, 6);
    findChessboardCorners(gray, boardSize, imgPoints);/*计算方格标定板角点*/
    find4QuadCornerSubpix(gray, imgPoints, Size(5, 5));/*细化方格标定版角点坐标*/

    /*生成棋盘格每个内角点的空间三维坐标*/
    Size squareSize = Size(10, 10);/*棋盘格每个方格的真实尺寸*/
    vector<Point3f> PointSets;
    for (int j = 0; j < boardSize.height; j++)
    {
        for (int k = 0; k < boardSize.width; k++)
        {
            Point3f realPoint;
            /*假设标定板为世界坐标系的z平面，即z=0*/
            realPoint.x = j * squareSize.width;
            realPoint.y = k * squareSize.height;
            realPoint.z = 0;
            PointSets.push_back(realPoint);
        }
    }
    /*输入前文计算得到的内参矩阵和畸变矩阵*/
    Mat cameraMatrix = (Mat_<float>(3, 3) << 532.016297, 0, 332.172519,
        0, 531.565159, 233.388075,
        0, 0, 1);
    Mat distCoeffs = (Mat_<float>(1, 5) << -0.285188, 0.080097, 0.001274, -0.002415, 0.106579);
    /*用PnP算法计算旋转和平移量*/
    Mat rvec, tvec;
    solvePnP(PointSets, imgPoints, cameraMatrix, distCoeffs, rvec, tvec);
    cout << "rvec: " << rvec << endl;
    /*旋转向量转换旋转矩阵*/
    Mat R;
    Rodrigues(rvec, R);
    cout << "R: " << endl << R << endl;

    /*使用PnP+Ransac算法计算旋转向量和平移向量*/
    Mat rvecRansac, tvecRansac;
    solvePnPRansac(PointSets, imgPoints, cameraMatrix, distCoeffs, rvecRansac, tvecRansac);
    Mat RRansac;
    Rodrigues(rvecRansac, RRansac);
    cout << "RRansac: " << endl << RRansac << endl;
    waitKey(0);