OpenCV三维解算常用方法C++

如果标定过程是通过OpenCV张正友标定法实现的，得到的内参外参保存在.txt文件中是这样的形式：

① 内参intrinsics.txt：

② 外参extrinsics.txt：

那么可以通过如下方法读取.txt文件获取左右相机内外参，主要包括三维解算需要用到的左右相机内参矩阵、畸变系数，以及左右相机构成双目系统的旋转矩阵和平移矩阵，具体代码如下：

cpp 复制代码

std::string intrinsicsPath{ "D:\\Program Files\\edge下载文件\\030716.16\\030716.16\\intrinsics.txt" }; // 左右相机内参数文件路径
std::string extrinsicsPath{ "D:\\Program Files\\edge下载文件\\030716.16\\030716.16\\extrinsics.txt" }; // 左右相机外参数文件路径

// 加载左右相机内参数
cv::Mat cameraMatrixL;  // 左相机内参矩阵
cv::Mat distCoeffsL;    // 左相机畸变参数
cv::Mat cameraMatrixR;  // 右相机内参矩阵
cv::Mat distCoeffsR;    // 右相机畸变参数

cv::FileStorage fs(intrinsicsPath, cv::FileStorage::READ);
fs["cameraMatrixL"] >> cameraMatrixL;
fs["cameraDistcoeffL"] >> distCoeffsL;
fs["cameraMatrixR"] >> cameraMatrixR;
fs["cameraDistcoeffR"] >> distCoeffsR;
fs.release();

//std::cout << "左相机内参矩阵......" << std::endl;
//std::cout << cameraMatrixL << std::endl;
//std::cout << "左相机畸变参数......" << std::endl;
//std::cout << distCoeffsL << std::endl;
//std::cout << std::endl;
//std::cout << "右相机内参矩阵......" << std::endl;
//std::cout << cameraMatrixR << std::endl;
//std::cout << "右相机畸变参数......" << std::endl;
//std::cout << distCoeffsR << std::endl;
//std::cout << std::endl;

// 加载相机外参数
cv::Mat R; // 旋转矩阵
cv::Mat T; // 平移向量

fs.open(extrinsicsPath, cv::FileStorage::READ);
fs["R"] >> R;
fs["T"] >> T;
fs.release();

在得到二维像素坐标之后可以通过畸变校正，三角测量法（Triangulation） 来计算三维点坐标。通常是基于 OpenCV 提供的 **cv::triangulatePoints**进行计算，这是一个标准的立体视觉技术，用于通过两个相机视角中的匹配点估算其三维坐标。

步骤如下代码所示：

二维坐标→畸变校正→转换到相机坐标系→三维解算。

cpp 复制代码

        std::vector<cv::Point2f> leftPointsUndistort, rightPointsUndistort;
        cv::undistortPoints(targetsL, leftPointsUndistort, cameraMatrixL, distCoeffsL, cv::Mat(), cameraMatrixL);// targetsL是未做畸变矫正前处理得到的二维中心点坐标（左相机）
        cv::undistortPoints(targetsR, rightPointsUndistort, cameraMatrixR, distCoeffsR, cv::Mat(), cameraMatrixR);// targetsR是未做畸变矫正前处理得到的二维中心点坐标（右相机）
        // 转换到相机坐标系
        std::vector<cv::Point2f> leftPointsCam = pixel2cam(leftPointsUndistort, cameraMatrixL);
        std::vector<cv::Point2f> rightPointsCam = pixel2cam(rightPointsUndistort, cameraMatrixR);
        // 求解三维坐标
        std::vector<cv::Point3f> points3d = triangulation(leftPointsCam, rightPointsCam, R, T);

调用的函数代码：

cpp 复制代码

std::vector<cv::Point3f> triangulation(const std::vector<cv::Point2f>& pts1, const std::vector<cv::Point2f>& pts2, cv::Mat& R, cv::Mat& T)
{
    cv::Mat T1 = (cv::Mat_<float>(3, 4) << 1, 0, 0, 0,
        0, 1, 0, 0,
        0, 0, 1, 0);
    R.convertTo(R, CV_64FC1);
    T.convertTo(T, CV_64FC1);

    cv::Mat T2 = (cv::Mat_<float>(3, 4) <<
        R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), T.at<double>(0, 0),
        R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), T.at<double>(1, 0),
        R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2), T.at<double>(2, 0));

    cv::Mat pts4d;
    cv::triangulatePoints(T1, T2, pts1, pts2, pts4d);

    std::vector<cv::Point3f> pts3d;
    for (int i = 0; i < pts4d.cols; ++i)
    {
        float x = pts4d.at< float >(0, i) / pts4d.at< float >(3, i);
        float y = pts4d.at< float >(1, i) / pts4d.at< float >(3, i);
        float z = pts4d.at< float >(2, i) / pts4d.at< float >(3, i);

        pts3d.emplace_back(cv::Point3f(x, y, z));
    }

    return pts3d;
}

std::vector<cv::Point2f> pixel2cam(const std::vector<cv::Point2f>& pts, const cv::Mat& cameraMatrix)
{
    std::vector<cv::Point2f> ptsCam;
    for (const auto& p : pts)
    {
        cv::Point2f c(
            (p.x - cameraMatrix.at<double>(0, 2)) / cameraMatrix.at<double>(0, 0),
            (p.y - cameraMatrix.at<double>(1, 2)) / cameraMatrix.at<double>(1, 1)
        );
        ptsCam.emplace_back(c);
    }

    return ptsCam;
}

注：pixel2cam 函数的作用是将像素坐标转换为归一化相机坐标（normalized camera coordinates）。这是必要的，因为 三角测量法 计算三维点的位置时，假设输入的点是在无畸变的相机坐标系下。

这里的 (xc,yc) 是归一化相机坐标，表示 光学中心归一化后的坐标，它们不再依赖于摄像机的焦距和像素比例，因此可以用于三角测量。

三角测量的数学原理

三角测量基于 两个不同视角的相机投影矩阵（Projection Matrix）。
如果使用 像素坐标，那么投影矩阵应该是 P=K $R∣T$ （包含相机内参）。
但如果使用 归一化相机坐标，那么投影矩阵可以简化为 P= $R∣T$ （去除了相机内参）。
这样可以直接利用 相机外参（R, T） 进行计算，提高准确性。

三角测量计算的流程

将像素坐标转换为归一化相机坐标 （pixel2cam）

构造相机投影矩阵

第一个相机位于世界坐标系的原点（通常以左相机为原点）：

P1= $I∣0$

第二个相机的投影矩阵由外参 旋转矩阵 R 和 平移向量 T 给出：

P2= $R∣T$

使用 OpenCV 的 triangulatePoints 进行三角测量

通过求解一组线性方程，得到齐次坐标 (X,Y,Z,W)

通过 X′=X/W,Y′=Y/W,Z′=Z/W 得到真实的三维坐标