内参标定中三角测距原理 C++

三角测距原理是通过测量物体在不同位置或角度下的视角来计算物体距离的方法。在内参标定中，我们需要使用相机拍摄不同位置或角度下的标定板图像，并根据相机内参计算出相机在不同位置或角度下的旋转矩阵R和平移向量T，从而得到相机的外参。接下来，我们可以使用三角测距原理计算标定板上某个点的三维坐标。

下面是一个基于OpenCV库实现的三角测距示例代码：

#include <opencv2/opencv.hpp>
#include <iostream>

using namespace std;
using namespace cv;

// 根据相机内参和相机在世界坐标系中的位置计算相机的外参
void calcExtrinsic(Mat K, Mat distCoeffs, vector<Point2f> imagePoints, vector<Point3f> objectPoints, Mat& R, Mat& T) {
    Mat rvec;
    solvePnP(objectPoints, imagePoints, K, distCoeffs, rvec, T);
    Rodrigues(rvec, R);
}

// 根据相机内参和相机的外参计算某个点在世界坐标系中的坐标
Point3f triangulatePoint(Mat K, Mat distCoeffs, Mat R, Mat T, Point2f p2d, Point2f p3d) {
    Mat P(3, 4, CV_32FC1);
    Mat K_R(3, 3, CV_32FC1), K_T(3, 1, CV_32FC1);
    K.copyTo(K_R);
    T.copyTo(K_T);
    R.col(0).copyTo(P.col(0));
    R.col(1).copyTo(P.col(1));
    R.col(2).copyTo(P.col(2));
    T.copyTo(P.col(3));

    Mat p1(2, 1, CV_32FC1), p2(2, 1, CV_32FC1);
    p1.at<float>(0, 0) = p2d.x;
    p1.at<float>(1, 0) = p2d.y;
    p2.at<float>(0, 0) = p3d.x;
    p2.at<float>(1, 0) = p3d.y;

    Mat X(4, 1, CV_32FC1);
    triangulatePoints(K_P, P, p1, p2, X);

    Point3f p3d_;
    p3d_.x = X.at<float>(0, 0) / X.at<float>(3, 0);
    p3d_.y = X.at<float>(1, 0) / X.at<float>(3, 0);
    p3d_.z = X.at<float>(2, 0) / X.at<float>(3, 0);

    return p3d_;
}

int main() {
    Mat image = imread("calibration.jpg");
    Mat K = Mat::eye(3, 3, CV_32FC1); // 相机内参
    K.at<float>(0, 0) = 1000.0; // fx
    K.at<float>(1, 1) = 1000.0; // fy
    K.at<float>(0, 2) = image.cols / 2.0; // cx
    K.at<float>(1, 2) = image.rows / 2.0; // cy
    Mat distCoeffs = Mat::zeros(5, 1, CV_32FC1); // 畸变参数

    // 标定板上的3D点坐标
    vector<Point3f> objectPoints;
    for (int i = 0; i < 6; i++) {
        for (int j = 0; j < 7; j++) {
            objectPoints.push_back(Point3f(i * 0.04, j * 0.04, 0));
        }
    }

    // 检测标定板角点
    vector<Point2f> imagePoints;
    bool found = findChessboardCorners(image, Size(6, 7), imagePoints);

    if (found) {
        // 计算相机外参
        Mat R, T;
        calcExtrinsic(K, distCoeffs, imagePoints, objectPoints, R, T);

        // 计算标定板某个点的3D坐标
        Point2f p2d = imagePoints[0];
        Point3f p3d = objectPoints[0];
        Point3f p3d_ = triangulatePoint(K, distCoeffs, R, T, p2d, p3d);

        cout << "3D point: (" << p3d_.x << ", " << p3d_.y << ", " << p3d_.z << ")" << endl;
    }

    return 0;
}

在上面的代码中，我们首先定义了相机的内参K和畸变参数distCoeffs，然后定义了标定板上的3D点坐标objectPoints。接着，我们使用OpenCV内置的函数findChessboardCorners检测标定板角点，并通过calcExtrinsic函数计算相机的外参R和T。最后，我们使用triangulatePoint函数计算标定板上某个点的3D坐标。