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

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

① 内参intrinsics.txt：

 ② 外参extrinsics.txt：

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

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 进行计算，这是一个标准的立体视觉技术，用于通过两个相机视角中的匹配点估算其三维坐标。

    步骤如下代码所示：

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

        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);

调用的函数代码：

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） 进行计算，提高准确性。

三角测量计算的流程

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

2. 构造相机投影矩阵

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

     P1=[I∣0]

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

     P2=[R∣T]

3. 使用 OpenCV 的 triangulatePoints 进行三角测量

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

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