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