OpenCV相机标定与3D重建(62)根据两个投影矩阵和对应的图像点来计算3D空间中点的坐标函数triangulatePoints()的使用

加粗样式 - 操作系统：ubuntu22.04

OpenCV版本：OpenCV4.9
IDE:Visual Studio Code
编程语言：C++11

算法描述

这个函数通过使用立体相机对3维点的观测，重建这些点的三维坐标（以齐次坐标表示）。

cv::triangulatePoints 是 OpenCV 库中的一个函数，用于根据两个投影矩阵和对应的图像点来计算3D空间中点的坐标。这个过程称为三角测量（Triangulation）。它利用了两个不同视角下观察到的同一空间点在图像平面上的位置以及这两个视角的投影矩阵来进行计算。

函数原型

cpp 复制代码

void cv::triangulatePoints	
(
	InputArray 	projMatr1,
	InputArray 	projMatr2,
	InputArray 	projPoints1,
	InputArray 	projPoints2,
	OutputArray 	points4D 
)

参数

参数projMatr1：第一个相机的3x4投影矩阵，即这个矩阵将世界坐标系中的3D点投影到第一张图像中。
参数projMatr2：第二个相机的3x4投影矩阵，即这个矩阵将世界坐标系中的3D点投影到第二张图像中。
参数projPoints1：第一张图像中特征点的2xN数组。在C++版本中，这也可以是一个特征点的向量或大小为1xN或Nx1的两通道矩阵。
参数projPoints2：第二张图像中对应点的2xN数组。在C++版本中，这也可以是一个特征点的向量或大小为1xN或Nx1的两通道矩阵。
参数points4D：齐次坐标中重建点的4xN数组。这些点返回的是世界坐标系中的坐标。

注意

请记住，所有输入数据都应该是浮点类型（float），以便此函数能够正常工作。

如果使用了来自 stereoRectify 的投影矩阵，则返回的点表示在第一个相机的校正坐标系中。

代码示例

cpp 复制代码

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

using namespace cv;
using namespace std;

// 假设我们已经获得了两个相机的投影矩阵和对应图像中的特征点
void exampleTriangulatePoints()
{
    // 投影矩阵 (3x4) - 这里只是示例数据，实际应用中应根据相机标定获得
    Mat projMatr1 = ( Mat_< double >( 3, 4 ) << 500, 0, 320, 0, 0, 500, 240, 0, 0, 0, 1, 0 );

    Mat projMatr2 = ( Mat_< double >( 3, 4 ) << 500, 0, 320, -100, 0, 500, 240, 0, 0, 0, 1, 0 );

    // 对应的2D点 (N个点)
    vector< Point2f > projPoints1 = { Point2f( 320, 240 ), Point2f( 330, 250 ) };
    vector< Point2f > projPoints2 = { Point2f( 300, 240 ), Point2f( 310, 250 ) };

    // 将2D点转换为适合输入的形式（每个点作为一个列向量）
    Mat points1( 2, projPoints1.size(), CV_32F );
    Mat points2( 2, projPoints2.size(), CV_32F );
    for ( size_t i = 0; i < projPoints1.size(); ++i )
    {
        points1.at< float >( 0, i ) = projPoints1[ i ].x;
        points1.at< float >( 1, i ) = projPoints1[ i ].y;
        points2.at< float >( 0, i ) = projPoints2[ i ].x;
        points2.at< float >( 1, i ) = projPoints2[ i ].y;
    }

    // 输出4D点矩阵
    Mat points4D;

    // 执行三角测量
    triangulatePoints( projMatr1, projMatr2, points1, points2, points4D );

    // 转换为非齐次坐标并打印结果
    for ( int i = 0; i < points4D.cols; ++i )
    {
        float x = points4D.at< float >( 0, i ) / points4D.at< float >( 3, i );
        float y = points4D.at< float >( 1, i ) / points4D.at< float >( 3, i );
        float z = points4D.at< float >( 2, i ) / points4D.at< float >( 3, i );

        cout << "Point " << i + 1 << ": (" << x << ", " << y << ", " << z << ")" << endl;
    }
}

int main()
{
    try
    {
        exampleTriangulatePoints();
    }
    catch ( const cv::Exception& e )
    {
        cerr << "Error: " << e.what() << endl;
        return -1;
    }

    return 0;
}

运行结果

bash 复制代码

Point 1: (2.23711e-17, -0, 5)
Point 2: (0.1, 0.1, 5)