c++学习【6】三角化获得三维点坐标，并可视化

    Mat T1 = (Mat_<float>(3, 4) <<
        1, 0, 0, 0,
        0, 1, 0, 0,
        0, 0, 1, 0);
    Mat T2 = (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)
        );

表示相机的投影矩阵。投影矩阵是一个3x4的矩阵，其中前三列代表相机的旋转和缩放变换（R），第四列代表相机的平移变换（t）。

通过Mat_<>模板类的构造函数，可以直接对Mat对象进行初始化，并使用“<<”运算符来填充矩阵元素。这里创建一个3行4列的单精度浮点型矩阵，<<运算符用于依次填充矩阵的每个元素。

因为已经求得旋转平移了，也就是相机位姿知道。就是R、t是已知的旋转矩阵和平移向量，at函数用于获取矩阵R和t中指定位置的元素值。

// 将像素坐标转换至相机坐标 

    //
    vector<Point2f> pts_1, pts_2;

通过对像素坐标进行归一化，即减去光心的横纵坐标，然后除以焦距，就可以得到相机坐标系下的坐标。最后，函数返回一个Point2f类型的相机坐标。这里假设物体在相机坐标系下的z坐标值为1。

p是输入的待转换的像素坐标，K是相机的内参矩阵。 

Point2f pixel2cam(const Point2d& p, const Mat& K)
{
    return Point2f
    (
        (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
        (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
    );
}

    for (DMatch m : matches)
    {
        // 将像素坐标转换至相机坐标
        pts_1.push_back(pixel2cam(keypoints_1[m.queryIdx].pt, K));
        pts_2.push_back(pixel2cam(keypoints_2[m.trainIdx].pt, K));
    }

Triangulated points shape: [500 x 4] 

    //
    Mat pts_4d;
    cv::triangulatePoints(T1, T2, pts_1, pts_2, pts_4d);

两个相机的投影矩阵，分别是3x4的矩阵，
对应两个相机图像上的像素点坐标，每个点坐标都是2x1的矩阵
存储三角化后的三维点坐标，每个点坐标都是4x1的齐次坐标，

在函数的执行过程中，triangulatePoints会先将projPoints1和projPoints2转换为齐次坐标形式，然后利用两个相机的投影矩阵进行三角化，得到对应的三维点坐标。最终输出的points4D是一个4xN的矩阵，每一列代表一个三维点的齐次坐标。

需要注意的是，triangulatePoints函数输出的三维点坐标是在相机坐标系下表示的归一化坐标。如果需要得到实际的三维坐标，还需要进行后续的坐标变换操作。

  定义了一个存储3D点的向量容器。其中Point3d是一个自定义的类，用于表示三维空间中的点，包括x、y、z三个坐标值。 

    //-- 三D坐标
    vector<Point3d> points;

    // 转换成非齐次坐标
    for (int i = 0; i < pts_4d.cols; i++)
    {
        Mat x = pts_4d.col(i);
        x /= x.at<float>(3, 0); // 归一化
        Point3d p(
            x.at<float>(0, 0),
            x.at<float>(1, 0),
            x.at<float>(2, 0)
        );
        points.push_back(p);
    }

保存成bin文件 

    #include <fstream>

    std::ofstream outfile("points.bin", std::ios::out | std::ios::binary);
    outfile.write((char*)&points[0], points.size() * sizeof(cv::Point3d));
    outfile.close();

500个点

    int num_points = points.size();
    std::cout << "Number of points: " << num_points << std::endl;
    // 将点云数据保存为文本文件
    ofstream file("points.txt");
    for (int i = 0; i < points.size(); i++) {
        Point3d p = points[i];
        file << p.x << " " << p.y << " " << p.z << endl;
    }
    file.close();