SfM两视图三维点云重建--【VS2015+OpenCV3.4+PCL1.8】

概述

视觉三维重建，是指使用相机采集的图片、根据相关知识推导目标物体三维信息的过程。这里借鉴《视觉SLAM14讲》中的分类方法，将视觉三维重建分为基于特征点和非提取特征点的重建。本文旨在使用基于特征点的重建方法完成相邻两张图片间的三维重建。

步骤

基于特征点的视觉三维重建方法主要包括如下流程：

1. 提取单张图片特征点；
2. 多张图片间特征点的匹配；
3. 利用对极几何约束，使用SfM恢复两相机间的三维变换关系；
4. 使用三角量测法（或称三角化法），重构二维图像对应点的三维点信息。

按照我的理解，上述流程可以概括为如下两部分：求解相机内外参、三维重建。

• 求解相机内外参：
  提取特征点并进行匹配只是为了组成匹配对，以方便计算基本矩阵F/本质矩阵E（对极几何约束），而计算出矩阵E或F后，即可进行SVD分解得到两相机间的三维变换关系（旋转矩阵R、平移矩阵t）。至此，是为求解相机内外参的过程。当然，也可以使用相机标定的方式获取相机内外参数，但多张重建会比较繁琐。
• 三维重建
  在已知两图片间变换矩阵的情况下，就可以使用三角化进行三维重建了。重建后的三维点是以第一张图片对应的相机坐标系为世界坐标系。

这里只是进行简单的介绍，并不涉及复杂的原理，有兴趣的可参考14讲中的内容，网上也有许多优秀博文可供参考。

代码

环境：Win10+VS2015+OpenCV3.4+PCL1.8

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

#include <opencv2/features2d/features2d.hpp> 
#include <opencv2/xfeatures2d/nonfree.hpp>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>

using namespace std;
using namespace cv;
using namespace pcl;
using namespace cv::xfeatures2d;

// ratio & symmetry test
void ratioTest(vector<vector<DMatch>> &matches, vector<DMatch> &goodMatches);
void symmetryTest(const vector<DMatch>& matches1, const vector<DMatch>& matches2, vector<DMatch>& symMatches);

// 从匹配对中提取特征点
void fillPoints(vector<DMatch> goodMatches, vector<KeyPoint> keypoints1, vector<KeyPoint> keypoints2, vector<Point2f>& points1, vector<Point2f>& points2);

// 三维重建
void reconstruct(Mat& K, Mat& fundamentalMatrix, vector<Point2f>& points1, vector<Point2f>& points2, vector<Point3f>& points3D, Mat_<double>& R, Mat_<double>& t);

// 获取关键点RGB
void getPointColor(vector<Point2f> points1, Mat BaseImageLeft, vector<Vec3b>& colors);

int main(int argc, char* argv[])
{
   
	// PCL可视化
	PointCloud<PointXYZRGB>::Ptr cloud(new PointCloud<PointXYZRGB>);
	boost::shared_ptr<visualization::PCLVisualizer> viewer(new visualization::PCLVisualizer("3D viewer")); // 实例化PCLVisualizer对象，窗口命名为3D viewer

	Mat K;			// 内参数矩阵
	Matx34d P, P1;  // 两图片的相机坐标

	// 1.读入图片
	Mat BaseImageLeft = imread(".\\images\\004.png", -1);
	Mat BaseImageRight = imread(".\\images\\006.png", -1);
	if (BaseImageLeft.empty() || BaseImageRight.empty())
	{
   
		cout << "ERROR! NO IMAGE LOADED!" << endl;
		return -1;
	}
	cout << "processing..." << endl;
	// 2.SIFT提取特征点
	Ptr<Feature2D> detector = xfeatures2d::SIFT::create(0, 3, 0.04, 10);

	vector<KeyPoint> keypoints_1, keypoints_2; // 关键点
	Mat descriptors_1, descriptors_2;		   // 描述符

	detector->detectAndCompute(BaseImageLeft, noArray(), keypoints_1, descriptors_1);
	detector->detectAndCompute(BaseImageRight, noArray(), keypoints_2, descriptors_2);

	// 3.Flann匹配特征点
	vector<vector<DMatch>> matches1, matches2;
	vector<DMatch> goodMatches1, goodMatches2, goodMatches, outMatches;

	FlannBasedMatcher matcher;