3 OpenCV两张图片实现稀疏点云的生成

前文：
1 基于SIFT图像特征识别的匹配方法比较与实现
2 OpenCV实现的F矩阵+RANSAC原理与实践

1 E矩阵

1.1 由F到E

$$E=K^T\ast F\ast K$$

E 矩阵可以直接通过之前算好的 F 矩阵与相机内参 K 矩阵获得

Mat E = K.t() * F * K;
1

相机内参获得的方式是一个较为复杂的方式，需要使用棋盘进行定位获得，我们这里直接使用了 OpenMVG 提供的现成的图片和 K 矩阵

1.2 直接使用函数

利用 openCV 提供的 findEssentialMat 函数可以直接得到 E 矩阵

Mat E = findEssentialMat(matchedPoints1, matchedPoints2, K, RANSAC, 0.999, 1.0, inliers);
1

2 相机姿态恢复

这一步可以使用 SVD 来通过 E 矩阵获取相对旋转矩阵 R 和平移向量 t

但是OpenCV直接提供了一个非常便捷的函数 —— recoverPose

其接受本质矩阵 E 、特征点的对应关系、相机的内参信息以及输出的相对旋转矩阵 R 和平移向量 t ；它会自动进行 SVD 分解和其他必要的计算，以恢复相对姿态信息

 //相机姿态恢复，求解R,t,投影矩阵
 Mat R, t;
 recoverPose(E, inlierPoints1, inlierPoints2, K, R, t);
1
2
3

3 相机投影矩阵

要构建相机的投影矩阵（也称为视图矩阵或外参矩阵），需要将旋转矩阵 R 和平移向量 t 合并到一起，投影矩阵通常表示为 3x4 的矩阵，其中旋转矩阵和平移向量都位于其中的适当位置

通常情况下，投影矩阵的形式如下：
$$P_1=K\ast [R|t]$$

$$P_2=K\ast [I|0]$$

实现代码如下：

// 创建两个相机的投影矩阵 [R T]
Mat proj1(3, 4, CV_32FC1);
Mat proj2(3, 4, CV_32FC1);

// 设置第一个相机的投影矩阵为单位矩阵 [I | 0]
proj1(Range(0, 3), Range(0, 3)) = Mat::eye(3, 3, CV_32FC1);
proj1.col(3) = Mat::zeros(3, 1, CV_32FC1);

// 设置第二个相机的投影矩阵为输入的旋转矩阵 R 和平移向量 T
R.convertTo(proj2(Range(0, 3), Range(0, 3)), CV_32FC1);
t.convertTo(proj2.col(3), CV_32FC1);

// 转换相机内参矩阵 K 为浮点型
Mat fK;
K.convertTo(fK, CV_32FC1);

// 计算投影矩阵 [K * [R|T]]
proj1 = fK * proj1;
proj2 = fK * proj2;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

4 三角法得稀疏点云

4.1 三角法计算3D点

对于每对匹配的特征点，可以使用三角法来计算它们的三维坐标；这通常涉及到将两个视角下的像素坐标与相应的投影矩阵相结合，以恢复三维坐标

// 三角法求解稀疏三维点云
Mat point4D_homogeneous(4, inlierPoints1.size(), CV_64F);
triangulatePoints(proj1, proj2, inlierPoints1, inlierPoints2, point4D_homogeneous);
1
2
3

4.2 转换为非齐次坐标

函数 triangulatePoints 得到的 point4D_homogeneous 通常是齐次坐标，需要将它们转换为非齐次坐标，以得到真实的三维点坐标

// 将齐次坐标转换为三维坐标
Mat point3D;
convertPointsFromHomogeneous(point4D_homogeneous.t(), point3D);
cout << point3D << endl;
1
2
3
4

5 匹配颜色

将颜色信息与点云关联在一起

使用了内点（inliers）的坐标从图像中提取了颜色信息，然后将颜色信息与三维点坐标关联起来，生成了带有颜色的稀疏点云

并将其存储在 pointCloud 和 pointColors 中；就可以根据需要进一步处理颜色信息

 // 获取特征点的颜色信息
 vector<Vec3b> colors1, colors2; // 颜色信息

 for (Point2f& inlierPoints : inlierPoints1)
 {
     int x = cvRound(inlierPoints.x); // 关键点的x坐标
     int y = cvRound(inlierPoints.y); // 关键点的y坐标
     Vec3b color = img1.at<Vec3b>(y, x);
     colors1.push_back(color);
 }

 for (Point2f& inlierPoints : inlierPoints2)
 {
     int x = cvRound(inlierPoints.x); // 关键点的x坐标
     int y = cvRound(inlierPoints.y); // 关键点的y坐标
     Vec3b color = img2.at<Vec3b>(y, x);
     colors2.push_back(color);
 }

 // 创建带颜色的点云数据结构
 vector<Point3f> pointCloud;
 vector<Vec3b> pointColors;

 // 关联颜色信息到点云
 for (int i = 0; i < point3D.rows; ++i)
 {
     Point3f point = point3D.at<Point3f>(i);
     Vec3b color1 = colors1[i];
     Vec3b color2 = colors2[i];

     // 在这里可以根据需要选择使用哪个颜色，或者进行颜色插值等处理
     Vec3b finalColor = color1; // 这里示例使用第一个相机的颜色

     pointCloud.push_back(point);
     pointColors.push_back(finalColor);
 }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36

6 生成 ply 文件

手动输出点云 PLY 文件，并包括了 PLY 文件的头部信息以及点云数据的写入；这是一种创建包含颜色信息的 PLY 文件的有效方法

在 PLY 文件的头部信息中，指定了点的数量以及点的属性，包括点的坐标和颜色通道（蓝色、绿色、红色）然后，你循环遍历点云数据，将点的坐标和颜色信息写入PLY文件

其会生成一个包含点云和颜色信息的PLY文件，可以将其用于保存点云以进行可视化或进一步处理

特别注意：图片是RGB还是BGR的颜色通道，这里是RGB

color[0]，color[1] ，color[2] 分别代表蓝色，绿色，红色通道

// 手动输出点云ply文件
ofstream plyFile(PLY_SAVE_PATH);

// ply的头部信息
plyFile << "ply\n";
plyFile << "format ascii 1.0\n";
plyFile << "element vertex " << point3D.rows << "\n";
plyFile << "property float x\n";
plyFile << "property float y\n";
plyFile << "property float z\n";
plyFile << "property uchar blue\n";
plyFile << "property uchar green\n";
plyFile << "property uchar red\n";
plyFile << "end_header\n";

// 写入点云数据
for (int i = 0; i < point3D.rows; ++i)
{
    Vec3b color = pointColors[i];
    const float* point = point3D.ptr<float>(i);
    plyFile << point[0] << " " << point[1] << " " << point[2] << " "
            << static_cast<int>(color[0]) << " "
            << static_cast<int>(color[1]) << " "
            << static_cast<int>(color[2]) << endl;
}

plyFile.close();   
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

7 完整测试代码

关于之前的阶段可以查看我之前的文章

// 定义图像文件路径和保存结果的路径
#define IMG_PATH1 "test_img\\images\\100_7105.jpg"
#define IMG_PATH2 "test_img\\images\\100_7106.jpg"
#define PLY_SAVE_PATH "test_img\\results\\output.ply"
#define K_NUM 2905.88, 0, 1416, 0, 2905.88, 1064, 0, 0, 1 // 3*3

#include 
#include 
#include 
#include 

using namespace std;
using namespace cv;

int main()
{
    // 阶段一------------------------------------------------------------------------------------
    // 读取两幅图像
    Mat img1 = imread(IMG_PATH1);
    Mat img2 = imread(IMG_PATH2);
    if (img1.empty() || img2.empty())
    {
        cout << "无法读取图像" << endl;
        return -1;
    }

    // 创建SIFT对象
    Ptr<SIFT> sift = SIFT::create();
    vector<KeyPoint> keypoints1, keypoints2;
    Mat descriptors1, descriptors2;

    // 检测关键点并计算描述子
    sift->detectAndCompute(img1, noArray(), keypoints1, descriptors1);
    sift->detectAndCompute(img2, noArray(), keypoints2, descriptors2);

    // 使用FLANN进行特征匹配
    FlannBasedMatcher matcher;
    vector<vector<DMatch>> matches;
    matcher.knnMatch(descriptors1, descriptors2, matches, 2);
    vector<DMatch> good_matches;
    for (int i = 0; i < matches.size(); ++i)
    {
        const float ratio = 0.7f;
        if (matches[i][0].distance < ratio * matches[i][1].distance)
        {
            good_matches.push_back(matches[i][0]);
        }
    }
    
    
    // 阶段二------------------------------------------------------------------------------------
    // 声明用于保存匹配点对的容器
    vector<Point2f> matchedPoints1, matchedPoints2;

    for (int i = 0; i < good_matches.size(); ++i)
    {
        matchedPoints1.push_back(keypoints1[good_matches[i].queryIdx].pt);
        matchedPoints2.push_back(keypoints2[good_matches[i].trainIdx].pt);
    }
    

    // 进行基本矩阵F的估计并使用RANSAC筛选
    Mat F;
    vector<uchar> inliers;
    F = findFundamentalMat(matchedPoints1, matchedPoints2, inliers, FM_RANSAC);
    cout << F << endl;

    vector<Point2f> inlierPoints1;
    vector<Point2f> inlierPoints2;
    for (int i = 0; i < inliers.size(); ++i)
    {
        if (inliers[i])
        {
			inlierPoints1.push_back(matchedPoints1[i]);
			inlierPoints2.push_back(matchedPoints2[i]);
		}
	}

    // 相机内参矩阵K
    Mat K = (Mat_<double>(3, 3) << K_NUM);
    cout << K << endl;

    计算本质矩阵E
    //Mat E = findEssentialMat(matchedPoints1, matchedPoints2, K, RANSAC, 0.999, 1.0, inliers);
    Mat E = K.t() * F * K;
    cout << "Essential Matrix (E):" << endl;
    cout << E << endl;
    
    //相机姿态恢复，求解R,t,投影矩阵
    Mat R, t;
    recoverPose(E, inlierPoints1, inlierPoints2, K, R, t);
    cout << "recoverpose" << endl;
    cout << "R:" << R << endl;
    cout << "t:" << t << endl;
    
    // 创建两个相机的投影矩阵 [R T]
    Mat proj1(3, 4, CV_32FC1);
    Mat proj2(3, 4, CV_32FC1);
    
    // 设置第一个相机的投影矩阵为单位矩阵 [I | 0]
    proj1(Range(0, 3), Range(0, 3)) = Mat::eye(3, 3, CV_32FC1);
    proj1.col(3) = Mat::zeros(3, 1, CV_32FC1);
    
    // 设置第二个相机的投影矩阵为输入的旋转矩阵 R 和平移向量 T
    R.convertTo(proj2(Range(0, 3), Range(0, 3)), CV_32FC1);
    t.convertTo(proj2.col(3), CV_32FC1);
    
    // 转换相机内参矩阵 K 为浮点型
    Mat fK;
    K.convertTo(fK, CV_32FC1);
    
    // 计算投影矩阵 [K * [R|T]]
    proj1 = fK * proj1;
    proj2 = fK * proj2;
    
    // 三角法求解稀疏三维点云
    Mat point4D_homogeneous(4, inlierPoints1.size(), CV_64F);
    triangulatePoints(proj1, proj2, inlierPoints1, inlierPoints2, point4D_homogeneous);
   

    // 将齐次坐标转换为三维坐标
    Mat point3D;
    convertPointsFromHomogeneous(point4D_homogeneous.t(), point3D);
    cout << point3D << endl;

    // 获取特征点的颜色信息
    vector<Vec3b> colors1, colors2; // 颜色信息

    for (Point2f& inlierPoints : inlierPoints1)
    {
        int x = cvRound(inlierPoints.x); // 关键点的x坐标
        int y = cvRound(inlierPoints.y); // 关键点的y坐标
        Vec3b color = img1.at<Vec3b>(y, x);
        colors1.push_back(color);
    }

    for (Point2f& inlierPoints : inlierPoints2)
    {
        int x = cvRound(inlierPoints.x); // 关键点的x坐标
        int y = cvRound(inlierPoints.y); // 关键点的y坐标
        Vec3b color = img2.at<Vec3b>(y, x);
        colors2.push_back(color);
    }

    // 创建带颜色的点云数据结构
    vector<Point3f> pointCloud;
    vector<Vec3b> pointColors;

    // 关联颜色信息到点云
    for (int i = 0; i < point3D.rows; ++i)
    {
        Point3f point = point3D.at<Point3f>(i);
        Vec3b color1 = colors1[i];
        Vec3b color2 = colors2[i];

        // 在这里可以根据需要选择使用哪个颜色，或者进行颜色插值等处理
        Vec3b finalColor = color1; // 这里示例使用第一个相机的颜色

        pointCloud.push_back(point);
        pointColors.push_back(finalColor);
    }

    // 手动输出点云ply文件
    ofstream plyFile(PLY_SAVE_PATH);

    // ply的头部信息
    plyFile << "ply\n";
    plyFile << "format ascii 1.0\n";
    plyFile << "element vertex " << point3D.rows << "\n";
    plyFile << "property float x\n";
    plyFile << "property float y\n";
    plyFile << "property float z\n";
    plyFile << "property uchar blue\n";
    plyFile << "property uchar green\n";
    plyFile << "property uchar red\n";
    plyFile << "end_header\n";

    // 写入点云数据
    for (int i = 0; i < point3D.rows; ++i)
    {
        Vec3b color = pointColors[i];
        const float* point = point3D.ptr<float>(i);
        plyFile << point[0] << " " << point[1] << " " << point[2] << " "
                << static_cast<int>(color[0]) << " "
                << static_cast<int>(color[1]) << " "
                << static_cast<int>(color[2]) << endl;
    }

    plyFile.close();   
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191

最终效果：

特别提醒：用 meshlab 打开后记得在右侧的设置框中将 shading 改为None ！！！这样才能看到真正的颜色，也可以把点调大一点好看些