文章目录
- OpenCV构建SFM模型
- SFM的概念
- 从一对图像估计相机运动
- 使用丰富特征描述符的点匹配
- 利用光流进行点匹配
- 寻找相机矩阵
- 场景重建
- 从多个场景重建
- 重构的细化
- 使用PCL可视化3D点云
- 使用实例代码
本文是翻译自经典书籍Mastering OPENCV第4章。
OpenCV构建SFM模型
在本章中，我们将讨论运动恢复结构（SfM）的概念，或者更好地说，使用OpenCV API中的函数从通过相机运动拍摄的图像中提取几何结构来帮助我们。首先，让我们避免其他漫长的路实现目标，而限制只使用单摄像机（通常称为单目方法）和离散且稀疏的帧集（而不是连续的视频流）的方法。这两个约束将大大简化我们将在接下来的页面中绘制的系统，并帮助我们理解任何SfM方法的基本原理。
在本章中，我们将介绍以下内容：
- 运动恢复结构的概念
- 从一对图像估计相机运动
- 重建场景
- 从多个视图重建
- 重建的细化
- 可视化三维点云
在本章中，我们假设使用事先校准过的校准摄像机。校准是计算机视觉中普遍存在的操作，OpenCV使用命令行工具完全支持该操作，并在前面的章节中进行了讨论。因此，我们假设摄像机的固有参数存在于K矩阵中，K矩阵是标定过程的输出之一。
为了让事情在语言方面更清楚，从这一点上讲，我们将把相机称为场景的单一视图，而不是拍摄图像的光学和硬件。相机在空间中有一个位置和一个观察方向。在两个摄影机之间，有一个平移元素（在空间中移动）和视图方向的旋转。
我们还将统一场景、世界、真实或3D中的点的术语，使其成为同一事物，即存在于我们真实世界中的点。这同样适用于图像中的点或2D中的点，即在该位置和时间投影到相机传感器上的某个真实3D点的图像坐标中的点。
SFM的概念
我们应该做的第一个区别是立体（或任何多视图）、使用校准器械的3D重建和SfM之间的区别。当一个由两个或多个假设我们已经知道摄像机之间的运动是什么的摄像机组成，但在SfM中，我们实际上不知道这个运动，我们希望找到它。
从简化的角度来看，校准的器械可以更精确地重建三维几何体，因为在估计摄像机之间的距离和旋转时没有误差，这是已知的。实现SfM系统的第一步是找到摄像机之间的运动。OpenCV可以通过多种方式帮助我们获得此运动，特别是使用findFundamentalMat函数。
让我们考虑一下选择SfM算法背后的目标。在大多数情况下，我们想要得到场景的几何形状，例如，物体与相机的关系及其形式。假设我们已经知道拍摄同一个场景的相机之间的运动，从一个合理相似的观点来看，我们现在想要重建几何。在计算机视觉术语中，这被称为三角化测量，有很多方法可以实现。它可以通过射线相交的方式来完成，我们在这里构造两条射线:一条来自每个相机的投影中心，另一条来自每个成像平面上的一点。理想情况下，这些光线在空间中的交点会在现实世界中的一个3D点上相交，在每个相机中成像，如下图所示:
实际上，射线相交是非常不可靠的;H和Z（书）建议不要这样做。这是因为这两条射线通常不相交，所以我们只好使用连接这两条射线的最短线段上的中点。相反，H和Z提出了一些三角化3D点的方法，我们将在重建场景部分讨论其中的一些方法。当前版本的OpenCV没有包含一个简单的用于三角测量的API，所以这部分我们将自己编写代码。
在我们学习了如何从两个视图恢复3D几何，我们将看到如何才能合并更多的视图来自相同的场景，以获得更丰富的重建。在这一点上，大多数SfM方法都试图在重建部分的细化中，通过BA优化来优化我们的摄像机和3D点的估计位置束。OpenCV在其新的图像拼接工具箱中包含了BA优化的方法。然而，使用OpenCV和c++的美妙之处在于大量的外部工具可以很容易地集成到流水线中。因此，我们将看到如何集成一个外部光束调节器，即整洁的SSBA库。
现在，我们已经概述了使用OpenCV实现SfM的方法，接下来我们将看到每个元素是如何实现的。
从一对图像估计相机运动
在我们开始实际寻找两个相机之间的运动之前，让我们检查一下我们手头上的输入和工具来执行这个操作。首先，我们有两个来自空间不同位置(不是完全不同的)的相同场景的图像。这是一项强大的条件，我们将确保使用它。现在就工具而言，我们应该看看数学对象对我们的图像、相机和场景施加的约束。
两个非常有用的数学对象是基础矩阵(用F表示)和本质矩阵(用E表示)。它们大多相似，除了本质矩阵假设使用校准过的摄像机;这就是我们的情况，所以我们会选择它。OpenCV只允许我们通过findFundamentalMat函数找到本质矩阵;然而，利用校准矩阵K，从其得到本质矩阵非常简单，如下所示:
Mat_<double> E = K.t() * F * K; //according to HZ (9.12)本质矩阵是一个3 x 3大小的矩阵，它在一幅图像中的一点和另一幅图像中的一点之间施加了一个约束，，其中x是图像1中的一点，x’是图像2中的对应点。这是非常有用的，我们马上就会看到。我们使用的另一个重要事实是，本质矩阵是所有我们需要的，以恢复我们的图像的两个相机，尽管只有规模;我们稍后会讲到。所以，如果我们得到了本质矩阵，我们就知道每个相机在空间中的位置，以及它正在看的地方。如果我们有足够的约束方程，我们就可以很容易地计算出矩阵，因为每个方程都可以用来求解矩阵的一小部分。事实上，OpenCV允许我们只使用7对点对来计算它，但希望我们会有更多对点对，并得到一个更健壮的解决方案。
使用丰富特征描述符的点匹配
现在我们将利用约束方程来计算本质矩阵。为了得到我们的约束条件，记住对于图像A中的每个点，我们必须在图像b中找到一个对应的点，我们如何实现这样的匹配呢?只需使用OpenCV的广泛的特征匹配框架，该框架在过去几年已经非常成熟。特征提取和描述符匹配是计算机视觉中一个重要的过程，在许多方法中都被用于执行各种操作。例如，检测图像中物体的位置和方向，或通过给定的查询在大型图像数据库中搜索类似的图像。
从本质上说，提取是指在图像中选择使特征更好的点，并为它们计算一个描述符。描述符是描述图像中特征点周围环境的数字向量。不同方法的描述符向量有不同的长度和数据类型。匹配是使用描述符从一个集合中找到另一个集合中的对应特征的过程。OpenCV提供了非常简单和强大的方法来支持特征提取和匹配。更多关于特征匹配的信息可以在第三章“无标记增强现实”中找到。
让我们来研究一个非常简单的特征提取和匹配方案：
// detectingkeypoints
SurfFeatureDetector detector;
vector<KeyPoint> keypoints1, keypoints2;
detector.detect(img1, keypoints1);
detector.detect(img2, keypoints2);
// computing descriptors
SurfDescriptorExtractor extractor;
Mat descriptors1, descriptors2;
extractor.compute(img1, keypoints1, descriptors1);
extractor.compute(img2, keypoints2, descriptors2);
// matching descriptors
BruteForceMatcher<L2<float>> matcher;
vector<DMatch> matches;
matcher.match(descriptors1, descriptors2, matches);您可能已经看到过类似的OpenCV代码，但是让我们快速回顾一下它。我们的目标是获得三个元素:两幅图像的特征点，它们的描述符，以及两组特征之间的匹配。OpenCV提供了一系列的特征检测器、描述符提取器和匹配器。在这个简单的示例中，我们使用SurfFeatureDetector函数来获取加速鲁棒特性(SURF)特性的2D位置，并使用SurfDescriptorExtractor函数来获取SURF描述符。我们使用一个暴力匹配器来获得匹配，这是最直接的方法来匹配两个特征集，通过比较第一个集合中的每个特征和第二个集合中的每个特征来实现(因此使用暴力的措辞)，并获得最佳匹配。
在下一张图片中,我们将看到http://cvlab.epfl.ch/~strecha/multiview/denseMVS.html上的两张Fountain-P11序列图像的特征点匹配。
实际上，像我们刚才执行的原始匹配只有在一定程度上是好的，许多匹配可能是错误的。因此，大多数SfM方法都会对匹配项执行某种形式的过滤，以确保正确性并减少错误。一种形式的过滤，即内置的OpenCV的蛮力匹配器，是交叉检查过滤。也就是说，如果第一幅图像的一个特征与第二幅图像的一个特征相匹配，则认为匹配为真，反之，第二幅图像的特征与第一幅图像的特征也相匹配。所提供的代码中使用的另一种常见的过滤机制是，根据两幅图像属于同一场景，并且它们之间具有某种立体视图关系的事实进行过滤。在实践中，过滤器试图稳健地计算基础矩阵，我们将在寻找相机矩阵部分学习基础矩阵，并保留与此计算相对应的那些特征对，具有较小的误差。
利用光流进行点匹配
使用丰富特性(如SURF)的另一种选择是使用光流(OF)。下面的信息框简要介绍光流情况。OpenCV最近扩展了从两张图片获取流场的API，现在它更快更强大了。我们将尝试使用它作为匹配特征的替代方法。
光流是将一幅图像中的点与另一幅图像中的点进行匹配的过程，假设两幅图像都是一个序列的一部分，且彼此相对接近。大多数光流方法比较小的区域,称为搜索窗口或补丁,在每一个点的图像在同一区域图像b .在计算机视觉很常见的规则之后,称为亮度恒常性约束(和其他名称),图像的小补丁不会大幅改变从一个图像,因此它们的减量应该接近于零。除了匹配的补丁，新的光流方法使用了一些额外的方法来获得更好的结果。一种是使用图像金字塔，它是图像尺寸越来越小的版本，允许“从粗到细”工作——这是计算机视觉中常用的技巧。另一种方法是在流场上定义全局约束，假设彼此靠近的点在同一方向上“一起移动”。关于OpenCV中光流方法的更深入的综述可以在使用Microsoft Kinect开发流体壁章节中找到，该章节可以在Packt网站上找到。
通过调用calcOpticalFlowPyrLK函数，在OpenCV中使用光流相当简单。然而，我们希望将OF的匹配结果与使用特征点的匹配结果保持类似，因为在未来，我们希望这两种方法可以互换。为此，我们必须安装一个特殊的匹配方法——一个可以与前面的基于特性的方法互换的方法，我们将在下面的代码部分中看到:
Vector<KeyPoint> left_keypoints,right_keypoints;
// 提取左图和右图的特征点，FAST特征点
FastFeatureDetector ffd;
ffd.detect(img1, left_keypoints);
ffd.detect(img2, right_keypoints);
vector<Point2f> left_points;
KeyPointsToPoints(left_keypoints,left_points);
vector<Point2f> right_points(left_points.size());
// 确保图像转换为灰度图
Mat prevgray,gray;
if (img1.channels() == 3) {
cvtColor(img1,prevgray,CV_RGB2GRAY);
cvtColor(img2,gray,CV_RGB2GRAY);
}
else {
prevgray = img1;
gray = img2;
}
// 计算光流的部分：
// how each left_point moved across the 2 images
vector<uchar> vstatus; //表示是否追踪到的状态1或0
vector<float> verror;
calcOpticalFlowPyrLK(prevgray, gray, left_points, right_points,
vstatus, verror);
// 首先，过滤出误差较大的点
vector<Point2f> right_points_to_find;
vector<int> right_points_to_find_back_index;
for (unsigned int i=0; i < vstatus.size(); i++) {
if (vstatus[i] &&verror[i] < 12.0) {
//保留点的原始索引在光流阵列中，以备未来使用
right_points_to_find_back_index.push_back(i);
//保持特征点本身
right_points_to_find.push_back(j_pts[i]);
} else {
vstatus[i] = 0; // a bad flow
}
}
// 对于每一个右点，查看它属于哪个检测到的特征
Mat right_points_to_find_flat = Mat(right_points_to_find).reshape(1, right_point_to_find.size()); //flatten array
vector<Point2f> right_features; // detected features
KeyPointsToPoints(right_keypoints,right_features);
Mat right_features_flat = Mat(right_features).reshape(1,right_features.size());
// 查看右侧图像的每个点，在每个区域内检测到的特征并对其匹配
BFMatchermatcher(CV_L2);
vector<vector<DMatch>> nearest_neighbors;
matcher.radiusMatch(right_points_to_find_flat,
right_features_flat,
nearest_neighbors,
2.0f);
//检查被发现的邻居是唯一的（丢掉那些太靠近的邻居，因为他们会混淆）
std::set<int> found_in_right_points; //防止重复
for(int i=0;i<nearest_neighbors.size();i++) {
DMatch _m;
if(nearest_neighbors[i].size()==1) {
_m = nearest_neighbors[i][0]; //只有一个邻居
}
else if(nearest_neighbors[i].size()>1) {
//两个邻居，检查谁更近
double ratio = nearest_neighbors[i][0].distance / nearest_neighbors[i][1].distance;
if(ratio < 0.7) { //小则说明不是很近，即第一个更近
_m = nearest_neighbors[i][0];
}
else { // 如果太近了，我们无法判断到底哪个更好
continue; // 没有通过比率测试，则进入下一个
}
}
else {
continue; // 没有邻居则跳过
}
// 防止重复
if (found_in_right_points.find(_m.trainIdx) == found_in_right_points.end()) {
// 被找到的邻居还没有被使用，我们应该匹配原始的左点的索引。
_m.queryIdx = right_points_to_find_back_index[_m.queryIdx];
matches->push_back(_m); // add this match
found_in_right_points.insert(_m.trainIdx);
}
}
cout<<"pruned "<< matches->size() <<" / "<<nearest_neighbors.size()<<" matches"<<endl;函数keypointstoppoints和PointsToKeyPoints是cv::Point2f和cv::KeyPoint结构体之间的简单方便的转换函数。
在前面的代码段中，我们可以看到许多有趣的事情。首先要注意的是，当我们使用光流时，我们的结果显示一个特征从图像左边的一个位置移动到图像右边的另一个位置。但我们在右边的图像中检测到一组新的特征，不一定与从图像到左边的光流的特征一致。我们必须使他们。为了找到那些丢失的特征，我们使用k-nearest neighbor (kNN)半径搜索，它给我们最多两个特征，它们位于感兴趣点的2像素半径内。
我们还可以看到kNN的比率测试的实现，这是SfM中减少错误的一种常见实践。从本质上讲，它是一个过滤器，当我们在左侧图像中的一个特征和右侧图像中的两个特征之间有匹配时，它可以消除令人困惑的匹配。如果右手边图像中的两个特征靠得太近，或者它们之间的比率太大(接近1.0)，我们认为它们令人困惑，所以不使用它们。我们还安装了一个重复预防过滤器来进一步删除匹配。
下图显示了从一个图像到另一个图像的流场。左侧图像中的粉色箭头表示从左侧图像到右侧图像的斑块移动。在左边的第二张图中，我们看到了一个被放大的流场的小区域。粉红色的箭头再次显示了补丁的运动，我们可以通过观察右边的两个原始图像片段看出这是有意义的。左侧图像中的视觉特征在图像中向左移动，方向为如下图所示的粉色箭头:
使用光流代替特征点法的优点是，过程通常更快，可以容纳更多的点匹配，使重建更密集。在许多光流方法中，还存在一个整体的patch整体运动模型，通常不考虑匹配丰富的特征。在使用光流时需要注意的是，它最适合由相同硬件拍摄的连续图像，而丰富的功能大多与此无关。这种差异源于这样一个事实，即光流方法通常使用非常基本的特征，如关键点周围的图像块，而高阶更丰富的特征(例如SURF)考虑到每个关键点的高级信息。使用光流或丰富的特性是应用程序设计者应该根据输入做出的决定。
寻找相机矩阵
现在我们已经得到了关键点之间的匹配，我们可以计算基本矩阵，并从中得到本质矩阵。但是，我们必须首先将匹配点对齐到两个数组中，其中一个数组中的下标对应于另一个数组中的相同下标。这是findFundamentalMat函数所需要的。我们还需要将KeyPoint结构转换为Point2f结构。我们必须特别注意DMatch的queryIdx和trainIdx成员变量，DMatch是OpenCV结构体，用于保存两个关键点之间的匹配，因为它们必须与我们使用matcher.match()函数的方式保持一致。下面的代码部分展示了如何将匹配对齐到两个相应的2D点集，以及如何使用这些点来找到基本矩阵:
vector<Point2f> imgpts1,imgpts2;
for( unsigned int i = 0; i<matches.size(); i++ )
{
// queryIdx 是左图像
imgpts1.push_back(keypoints1[matches[i].queryIdx].pt);
// trainIdx 是右图像
imgpts2.push_back(keypoints2[matches[i].trainIdx].pt);
}
Mat F = findFundamentalMat(imgpts1, imgpts2, FM_RANSAC, 0.1, 0.99,status);
Mat_<double> E = K.t() * F * K; //according to HZ (9.12)我们稍后可以使用状态二进制向量来修剪那些与恢复的基本矩阵对齐的点。下图是用基本矩阵进行剪枝后的点匹配图。红色箭头表示在寻找F矩阵的过程中去掉的特征匹配，绿色箭头表示保留的特征匹配。
现在我们准备找到相机矩阵。这个过程在H和Z的书的第9章中有详细的描述;然而，我们将使用一个非常直接和简单的实现，OpenCV使事情对我们来说非常容易。但首先，我们将简要地检查我们将要使用的相机矩阵的结构。
这是相机的模型,它包含两个元素，旋转®和平移(表示为t)。有趣的是，它拥有一个非常基本方程: ，其中x是一个2D点形象，X是一个3D空间。除此之外，这个矩阵还为我们提供了图像点和场景点之间非常重要的关系。那么，现在我们有了寻找相机矩阵的动机，我们将看看它是如何做到的。下面的代码部分展示了如何将基本矩阵分解为旋转和平移元素:
SVD svd(E);
Matx33d W(0,-1, 0,1, 0 ,0 ,0, 0, 1);
Mat_<double> R = svd.u * Mat(W) * svd.vt; //HZ 9.19
Mat_<double> t = svd.u.col(2); //u3
Matx34d P1( R(0,0),R(0,1), R(0,2), t(0),
R(1,0),R(1,1), R(1,2), t(1),
R(2,0),R(2,1), R(2,2), t(2));非常简单。我们所要做的就是对我们之前得到的本质矩阵进行奇异值分解(SVD)，并将其乘以一个特殊的矩阵W。不用太深入地研究我们所做的数学解释，我们可以说SVD运算将矩阵E分解成两部分，一个旋转元素和平移元素。事实上，本质矩阵最初是由这两个元素的乘积构成的。如果为了满足我们的好奇心，我们可以看看下列本质矩阵的方程，它出现在文献中:。我们看到它由(某种形式)一个平移元素和一个旋转元素R组成。
我们注意到我们刚才做的只给了我们一个相机矩阵，那么另一个相机矩阵在哪里呢?好吧，我们在假设一个摄像机矩阵是固定的和规范的(没有旋转和平移)下执行这个操作。下一个相机矩阵也是规范的:
我们从基本矩阵中恢复的另一个相机相对于固定的那个移动和旋转了。这也意味着我们从这两个相机矩阵中恢复的任何3D点都将在世界原点点(0,0,0)拥有第一个相机。
然而，这并不是完整的解决方案。H和Z在他们的书中展示了如何和为什么这种分解实际上有四个可能的相机矩阵，但只有一个是正确的。正确的矩阵将产生具有正Z值的重构点(相机前面的点)。但是我们只能在学习了三角测量和3D重建之后才能理解，这将在下一节中讨论。
我们可以考虑添加到方法中的另一件事是错误检查。很多次从点匹配计算基本矩阵是错误的，这影响了相机矩阵。继续用有缺陷的相机矩阵进行三角测量是毫无意义的。我们可以安装一个检查，看看旋转元素是否是一个有效的旋转矩阵。记住旋转矩阵的行列式必须为1(或-1)，我们可以简单地做以下操作:
bool CheckCoherentRotation(cv::Mat_<double>& R) {
if(fabsf(determinant(R))-1.0 > 1e-07) {
cerr<<"det(R) != +-1.0, this is not a rotation matrix"<<endl;
return false;
}
return true;
}现在我们可以看到所有这些元素是如何组合成一个恢复P矩阵的函数的，如下所示:
void FindCameraMatrices(const Mat& K,const Mat& Kinv,
const vector<KeyPoint>& imgpts1,
const vector<KeyPoint>& imgpts2,
Matx34d& P,Matx34d& P1,
vector<DMatch>& matches,
vector<CloudPoint>& outCloud)
{
//找到相机矩阵F
//得到基础矩阵
Mat F = GetFundamentalMat(imgpts1,imgpts2,matches);
//本质矩阵：计算并提取相机【R|t】
Mat_<double> E = K.t() * F * K; //according to HZ (9.12)
//分解 E 到 P' , HZ (9.19)
SVD svd(E,SVD::MODIFY_A);
Mat svd_u = svd.u;
Mat svd_vt = svd.vt;
Mat svd_w = svd.w;
Matx33d W(0,-1,0,//HZ 9.13
1,0,0,
0,0,1);
Mat_<double> R = svd_u * Mat(W) * svd_vt; //HZ 9.19
Mat_<double> t = svd_u.col(2); //u3
if (!CheckCoherentRotation(R)) {
cout<<"resulting rotation is not coherent\n";
P1 = 0;
return;
}
P1 = Matx34d(R(0,0),R(0,1),R(0,2),t(0),
R(1,0),R(1,1),R(1,2),t(1),
R(2,0),R(2,1),R(2,2),t(2));
}在这一点上，我们有两个摄像机，我们需要重建场景。标准的第一个相机，在P变量中，和我们计算的第二个相机，形成了在P1变量中的基本矩阵。下一节将揭示我们如何使用这些相机来获得场景的3D结构。
场景重建
接下来，我们研究从目前获得的信息中恢复场景的 3D 结构的问题。 正如我们之前所做的那样，我们应该查看手头的工具和信息来实现这一目标。 在上一节中，我们从基本矩阵和基本矩阵中获得了两个相机矩阵； 我们已经讨论了这些工具将如何用于获取空间点的 3D 位置。 然后，我们可以回到我们的匹配点对，用数值数据填充我们的方程。 点对也可用于计算我们从所有近似计算中得到的误差。
现在是时候看看我们如何使用 OpenCV 执行三角测量了。 这一次，我们将按照 Hartley 和 Sturm 在他们的文章 Triangulation 中采取的步骤，他们在其中实施和比较了一些三角测量方法。 我们将实现他们的一种线性方法，因为使用 OpenCV 编写代码非常简单。
请记住，我们有两个由 2D 点匹配和 P 矩阵产生的关键方程：x=PX和 x'= P'X，其中 x 和 x’ 匹配 2D 点，X 是两个相机成像的真实世界 3D 点。 如果我们重写方程，我们可以制定一个线性方程组，可以求解 X 的值，这正是我们想要找到的。 假设 X = (x, y, z, 1)t（对于离相机中心不太近或不太远的点的合理假设）会创建一个 AX = B 形式的非齐次线性方程组。我们可以编码并求解这个方程组如下：
Mat_<double> LinearLSTriangulation(Point3d u,//homogenous image point (u,v,1)
Matx34d P,//camera 1 matrix
Point3d u1,//homogenous image point in 2nd camera
Matx34d P1)//camera 2 matrix
{
//build A matrix
Matx43d A(u.x*P(2,0)-P(0,0),u.x*P(2,1)-P(0,1),u.x*P(2,2)-P(0,2),
u.y*P(2,0)-P(1,0),u.y*P(2,1)-P(1,1),u.y*P(2,2)-P(1,2),
u1.x*P1(2,0)-P1(0,0), u1.x*P1(2,1)-P1(0,1),u1.x*P1(2,2)-P1(0,2),
u1.y*P1(2,0)-P1(1,0), u1.y*P1(2,1)-P1(1,1),u1.y*P1(2,2)-P1(1,2));
//build B vector
Matx41d B(-(u.x*P(2,3)-P(0,3)),
-(u.y*P(2,3)-P(1,3)),
-(u1.x*P1(2,3)-P1(0,3)),
-(u1.y*P1(2,3)-P1(1,3)));
//solve for X
Mat_<double> X;
solve(A,B,X,DECOMP_SVD);
return X;
}这将为我们提供由两个 2D 点产生的 3D 点的近似值。 还有一点需要注意的是，2D 点是用齐次坐标表示的，这意味着 x 和 y 值都附加了 1。我们应该确保这些点在归一化坐标中，这意味着它们事先乘以校准矩阵 K . 我们可能会注意到，我们可以简单地使用 KP 矩阵（K 矩阵乘以 P 矩阵）来代替将每个点乘以矩阵 K，就像 H 和 Z 在第 9 章中所做的那样。我们现在可以在 点匹配以获得完整的三角测量，如下所示：
double TriangulatePoints(const vector<KeyPoint>& pt_set1,
const vector<KeyPoint>& pt_set2,
const Mat&Kinv,
const Matx34d& P,
const Matx34d& P1,
vector<Point3d>& pointcloud)
{
vector<double> reproj_error;
for (unsigned int i=0; i<pts_size; i++) {
//convert to normalized homogeneous coordinates
Point2f kp = pt_set1[i].pt;
Point3d u(kp.x,kp.y,1.0);
Mat_<double> um = Kinv * Mat_<double>(u);
u = um.at<Point3d>(0);
Point2f kp1 = pt_set2[i].pt;
Point3d u1(kp1.x,kp1.y,1.0);
Mat_<double> um1 = Kinv * Mat_<double>(u1);
u1 = um1.at<Point3d>(0);
//triangulate
Mat_<double> X = LinearLSTriangulation(u,P,u1,P1);
//calculate reprojection error
Mat_<double> xPt_img = K * Mat(P1) * X;
Point2f xPt_img_(xPt_img(0)/xPt_img(2),xPt_img(1)/xPt_img(2));
reproj_error.push_back(norm(xPt_img_-kp1));
//store 3D point
pointcloud.push_back(Point3d(X(0),X(1),X(2)));
}
//return mean reprojection error
Scalar me = mean(reproj_error);
return me[0];
}在下图中，我们将看到来自 http://cvlab.epfl.ch/~strecha/multiview/denseMVS.html 的 Fountain P-11 序列中的两个图像的三角测量结果。 顶部的两个图像是场景的原始两个视图，底部的一对是从两个视图重建的点云的视图，包括估计的相机看着喷泉。 我们可以看到红砖墙的右侧部分是如何重建的，还有从墙上突出的喷泉。
然而，正如我们之前所讨论的，我们有一个问题，即重建只是按比例进行的。 我们应该花点时间来理解 up-to-scale 的含义。
我们在两个相机之间获得的运动将有一个任意的测量单位，也就是说，它不是以厘米或英寸为单位，而只是一个给定的比例单位。 我们重建的相机将相距一个比例单位。 如果我们决定稍后恢复更多相机，这将产生重大影响，因为每对相机都有自己的比例单位，而不是一个共同的单位。
我们现在将讨论我们设置的误差度量如何帮助我们找到更稳健的重建。 首先我们应该注意，重投影意味着我们只需将三角化的 3D 点在相机上重新成像以获得重新投影的 2D 点，然后我们比较原始 2D 点和重新投影的 2D 点之间的距离。 如果这个距离很大，这意味着我们可能在三角测量中有一个错误，所以我们可能不想在最终结果中包含这个点。 我们的全局度量是平均重投影距离，可能会提示我们三角测量的整体执行情况。 高平均重投影率可能表明 P 矩阵存在问题，因此可能存在计算基本矩阵或匹配特征点的问题。
我们应该简要回到上一节中对相机矩阵的讨论。 我们提到可以通过四种不同的方式组合相机矩阵 P1，但只有一种组合是正确的。 现在我们知道如何对一个点进行三角测量，我们可以添加一个检查来查看四个相机矩阵中的哪一个是有效的。 在这一点上，我们将跳过实现细节，因为它们在本书所附的示例代码中有所介绍。
接下来，我们将看看如何恢复更多查看同一场景的摄像机，并结合 3D 重建结果。
从多个场景重建
既然我们知道如何从两个摄像机中恢复运动和场景几何图形，那么通过应用相同的过程来获取额外摄像机和更多场景点的参数似乎是微不足道的。 这件事其实并不简单，我们只能得到一个符合比例的重建，每对图片给我们的比例是不同的。
有多种方法可以从多个视图正确重建 3D 场景数据。 一种方法是切除或相机姿态估计，也称为透视 N 点 (PNP)，我们尝试使用我们已经找到的场景点来求解新相机的位置。 另一种方法是对更多点进行三角测量，看看它们如何适应我们现有的场景几何； 这将通过迭代最近点 (ICP) 程序告诉我们新相机的位置。 在本章中，我们将讨论使用 OpenCV 的 solvePnP 函数来实现第一种方法。
我们在这种重建中选择的第一步——使用相机切除的增量 3D 重建——是获得基线场景结构。 由于我们要根据场景的已知结构来寻找任何新相机的位置，我们需要找到一个初始结构和一个要使用的基线。我们可以使用我们之前讨论过的方法（例如，在第一帧和第二帧之间）通过查找相机矩阵（使用 FindCameraMatrices 函数）和对几何进行三角测量（使用 TriangulatePoints 函数）来获取基线。
找到初始结构后，我们可以继续； 然而，我们的方法需要相当多的簿记。 首先我们应该注意，solvePnP函数需要两个对齐的 3D 和 2D 点向量。 对齐向量意味着一个向量中的第 i 个位置与另一个向量中的第 i 个位置对齐。 为了获得这些向量，我们需要在我们之前恢复的 3D 点中找到那些点，这些点与我们新帧中的 2D 点对齐。 一个简单的方法是为云中的每个 3D 点附加一个向量，表示它来自的 2D 点。 然后我们可以使用特征匹配来获得匹配对。
让我们介绍一个新的结构表示3D点，如下所示：
struct CloudPoint {
cv::Point3d pt;
std::vector<int> index_of_2d_origin;
};在 3D 点的顶部，它保存了每个帧所具有的 2D 点向量内的 2D 点的索引，这对这个 3D 点有贡献。 index_of_2d_origin 的信息必须在对新的 3D 点进行三角测量时进行初始化，记录哪些相机参与了三角测量。 然后我们可以使用它从我们的 3D 点云回溯到每一帧中的 2D 点，如下所示：
std::vector<CloudPoint> pcloud; //全局的3D点云
//检测第1帧和第0帧之间的匹配 (and thus the current cloud)
std::vector<cv::Point3f> ppcloud;
std::vector<cv::Point2f> imgPoints;
vector<int> pcloud_status(pcloud.size(),0);
//扫描我们已经使用的视图 (good_views)
for (set<int>::iterator done_view = good_views.begin(); done_view !=good_views.end(); ++done_view)
{
int old_view = *done_view; //我们已经用于重建的视图
//检测在<working_view>帧和<old_view>帧之间的matches_from_old_to_working (and thus the current cloud)
std::vector<cv::DMatch> matches_from_old_to_working =
matches_matrix[std::make_pair(old_view,working_view)];
//扫描2D-2D的匹配点
for (unsigned int match_from_old_view=0;
match_from_old_view<matches_from_old_to_working.size();
match_from_old_view++) {
// 在<old_view>中的匹配2D点的指标
int idx_in_old_view = matches_from_old_to_working[match_from_old_view].queryIdx;
//扫描现有的点云看是否来自<old_view>
for (unsigned int pcldp=0; pcldp<pcloud.size(); pcldp++) {
// 查看 <old_view> 中的这个 2D 点是否有助于点云中的这个 3D 点
if (idx_in_old_view == pcloud[pcldp].index_of_2d_origin[old_view]
&& pcloud_status[pcldp] == 0) //防止重复
{
//3d点在点云中
ppcloud.push_back(pcloud[pcldp].pt);
//在图像<working_view>的2D点
Point2d pt_ = imgpts[working_view] [matches_from_old_to_working[match_from_old_view].trainIdx].pt;
imgPoints.push_back(pt_);
pcloud_status[pcldp] = 1;
break;
}
}
}
}
cout<<"found "<<ppcloud.size() <<" 3d-2d point correspondences"<<endl;现在我们有一个对齐的场景中的 3D 点与新帧中的 2D 点，我们可以使用它们来恢复相机位置，如下所示：
cv::Mat_<double> t,rvec,R;
cv::solvePnPRansac(ppcloud, imgPoints, K, distcoeff, rvec, t, false);
//get rotation in 3x3 matrix form
Rodrigues(rvec, R); //罗德里格斯公式
P1 = cv::Matx34d(R(0,0),R(0,1),R(0,2),t(0),
R(1,0),R(1,1),R(1,2),t(1),
R(2,0),R(2,1),R(2,2),t(2));请注意，我们使用的是 solvePnPRansac 函数而不是 solvePnP 函数，因为它对异常值更稳健。 现在我们有了一个新的 P1 矩阵，我们可以简单地再次使用我们之前定义的 TriangulatePoints 函数，并用更多的 3D 点填充我们的点云。
在下图中，我们在 http://cvlab.epfl.ch/~strecha/multiview/denseMVS.html 看到 Fountain-P11 场景的增量重建，从第 4 张图像开始。 左上图是使用四张图片后的重建图； 参与的摄像机显示为红色金字塔，白线表示方向。 其他图像显示了更多相机如何向云添加更多点。
重构的细化
SfM 方法最重要的部分之一是细化和优化重建的场景，也称为捆绑调整 (BA) 过程。 这是一个优化步骤，我们收集的所有数据都适合一个整体模型。3D 点的位置和相机的位置都得到了优化，因此重投影误差最小化（即，近似的 3D 点被投影在靠近原始 2D 点位置的图像上）。 这个过程通常需要求解数万个参数量级的非常大的线性方程。 这个过程可能有点费力，但我们之前采取的步骤将允许与捆绑调节器轻松集成。 一些之前看起来很奇怪的事情可能会变得清晰； 例如，我们为云中的每个 3D 点保留原始 2D 点的原因。
束调整算法的一种实现是简单稀疏束调整 (SSBA) 库； 我们将选择它作为我们的 BA 优化器，因为它有一个简单的 API。 它只需要几个输入参数，我们可以从我们的数据结构中轻松创建这些参数。 我们将从 SSBA 使用的关键对象是 CommonInternalsMetricBundleOptimizer 函数，它执行优化。它需要相机参数，3D点云，点云中每个点对应的2D图像点，以及观察场景的相机。 到目前为止，得到这些参数应该很简单。我们应该注意到，这种 BA 方法假设所有图像都是由相同的硬件拍摄的，因此通用的内部结构，其他操作模式可能不会假设这一点。 我们可以按如下方式执行捆绑调整：
voidBundleAdjuster::adjustBundle(vector<CloudPoint> &pointcloud,
const Mat &cam_intrinsics,
const std::vector<std::vector<cv::KeyPoint>> &imgpts,
std::map<int ,cv::Matx34d> &Pmats)
{
int N = Pmats.size(), M = pointcloud.size(), K = -1;
cout<<"N (cams) = "<< N <<" M (points) = "<< M <<" K (measurements) ="<< K <<endl;
StdDistortionFunction distortion;
//相机内参矩阵
Matrix3x3d KMat;
makeIdentityMatrix(KMat);
KMat[0][0] = cam_intrinsics.at<double>(0,0);
KMat[0][1] = cam_intrinsics.at<double>(0,1);
KMat[0][2] = cam_intrinsics.at<double>(0,2);
KMat[1][1] = cam_intrinsics.at<double>(1,1);
KMat[1][2] = cam_intrinsics.at<double>(1,2);
...
// 3D point cloud
vector<Vector3d >Xs(M);
for (int j = 0; j < M; ++j)
{
Xs[j][0] = pointcloud[j].pt.x;
Xs[j][1] = pointcloud[j].pt.y;
Xs[j][2] = pointcloud[j].pt.z;
}
cout<<"Read the 3D points."<<endl;
// convert cameras to BA datastructs
vector<CameraMatrix> cams(N);
for (inti = 0; i< N; ++i)
{
intcamId = i;
Matrix3x3d R;
Vector3d T;
Matx34d& P = Pmats[i];
R[0][0] = P(0,0); R[0][1] = P(0,1); R[0][2] = P(0,2); T[0] = P(0,3);
R[1][0] = P(1,0); R[1][1] = P(1,1); R[1][2] = P(1,2); T[1] = P(1,3);
R[2][0] = P(2,0); R[2][1] = P(2,1); R[2][2] = P(2,2); T[2] = P(2,3);
cams[i].setIntrinsic(Knorm);
cams[i].setRotation(R);
cams[i].setTranslation(T);
}
cout<<"Read the cameras."<<endl;
vector<Vector2d > measurements;
vector<int> correspondingView;
vector<int> correspondingPoint;
// 2D corresponding points
for (unsigned int k = 0; k <pointcloud.size(); ++k)
{
for (unsigned int i=0; i<pointcloud[k].imgpt_for_img.size(); i++) {
if (pointcloud[k].imgpt_for_img[i] >= 0) {
int view = i, point = k;
Vector3d p, np;
Point cvp = imgpts[i][pointcloud[k].imgpt_for_img[i]].pt;
p[0] = cvp.x;
p[1] = cvp.y;
p[2] = 1.0;
// 标准化测量以匹配单位焦距
scaleVectorIP(1.0/f0, p);
measurements.push_back(Vector2d(p[0], p[1]));
correspondingView.push_back(view);
correspondingPoint.push_back(point);
}
}
} // end for (k)
K = measurements.size();
cout<<"Read "<< K <<" valid 2D measurements."<<endl;
...
// perform the bundle adjustment
{
CommonInternalsMetricBundleOptimizeropt(V3D::FULL_BUNDLE_FOCAL_
LENGTH_PP, inlierThreshold, K0, distortion, cams, Xs, measurements,
correspondingView, correspondingPoint);
opt.tau = 1e-3;
opt.maxIterations = 50;
opt.minimize();
cout<<"optimizer status = "<<opt.status<<endl;
}
...
//extract 3D points
for (unsigned int j = 0; j <Xs.size(); ++j)
{
pointcloud[j].pt.x = Xs[j][0];
pointcloud[j].pt.y = Xs[j][1];
pointcloud[j].pt.z = Xs[j][2];
}
//extract adjusted cameras
for (int i = 0; i< N; ++i)
{
Matrix3x3d R = cams[i].getRotation();
Vector3d T = cams[i].getTranslation();
Matx34d P;
P(0,0) = R[0][0]; P(0,1) = R[0][1]; P(0,2) = R[0][2]; P(0,3) = T[0];
P(1,0) = R[1][0]; P(1,1) = R[1][1]; P(1,2) = R[1][2]; P(1,3) = T[1];
P(2,0) = R[2][0]; P(2,1) = R[2][1]; P(2,2) = R[2][2]; P(2,3) = T[2];
Pmats[i] = P;
}
}这段代码虽然很长，但主要是关于将我们的内部数据结构与 SSBA 的数据结构相互转换，并调用优化过程。
下图显示了 BA 的效果。 左边两幅图是调整前点云的点，从两个角度看，右边的图是优化后的云。 这种变化是相当戏剧性的，从不同视图进行三角测量的点之间的许多错位现在大部分都得到了巩固。 我们还可以注意到调整如何创建了更好的平面重建。
使用PCL可视化3D点云
在处理 3D 数据时，仅通过查看重投影误差测量或原始点信息很难快速了解结果是否正确。 另一方面，如果我们查看点云本身，我们可以立即验证它是否有意义或是否存在错误。 对于可视化，我们将使用一个新兴的 OpenCV 姊妹项目，称为点云库 (PCL)。 它带有许多用于可视化和分析点云的工具，例如查找平面、匹配点云、分割对象和消除异常值。 如果我们的目标不是点云而是一些高阶信息（例如 3D 模型），这些工具非常有用。
首先，我们应该在 PCL 的数据结构中表示我们的点云（本质上是一个 3D 点列表）。 这可以按如下方式完成：
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
void PopulatePCLPointCloud(const vector<Point3d>& pointcloud,
const std::vector<cv::Vec3b>& pointcloud_RGB)
//填充点云
{
cout<<"Creating point cloud...";
cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
for (unsigned int i=0; i<pointcloud.size(); i++) {
//从点中获取它的RGB 值
Vec3b rgbv(255,255,255);
if (pointcloud_RGB.size() >= i) {
rgbv = pointcloud_RGB[i];
}
// 检查错误的坐标 (NaN, Inf, etc.)
if (pointcloud[i].x != pointcloud[i].x || isnan(pointcloud[i].x) ||
pointcloud[i].y != pointcloud[i].y || isnan(pointcloud[i].y) ||
pointcloud[i].z != pointcloud[i].z || isnan(pointcloud[i].z) ||
fabsf(pointcloud[i].x) > 10.0 ||
fabsf(pointcloud[i].y) > 10.0 ||
fabsf(pointcloud[i].z) > 10.0) {
continue;
}
pcl::PointXYZRGB pclp;
// 3D coordinates
pclp.x = pointcloud[i].x;
pclp.y = pointcloud[i].y;
pclp.z = pointcloud[i].z;
// RGB 颜色，需要用整数表示
uint32_t rgb = ((uint32_t)rgbv[2] << 16 | (uint32_t)rgbv[1] << 8 | (uint32_t)rgbv[0]);
pclp.rgb = *reinterpret_cast<float*>(&rgb);
cloud->push_back(pclp);
}
cloud->width = (uint32_t) cloud->points.size(); // number of points
cloud->height = 1; // a list of points, one row of data
}为了获得良好的可视化效果，我们还可以提供颜色数据作为从图像中获取的 RGB 值。 我们还可以对原始云应用过滤器，以消除可能是异常值的点，使用统计异常值去除 (SOR) 工具，如下所示：
Void SORFilter() {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>);
std::cerr<<"Cloud before SOR filtering: "<< cloud->width * cloud->height <<" data points"<<std::endl;
// Create the filtering object
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB>sor;
sor.setInputCloud (cloud);
sor.setMeanK (50);
sor.setStddevMulThresh (1.0);
sor.filter (*cloud_filtered);
std::cerr<<"Cloud after SOR filtering: "<<cloud_filtered->width * cloud_filtered->height
<<" data points "<<std::endl;
copyPointCloud(*cloud_filtered,*cloud);
}然后我们可以使用 PCL 的 API 来运行一个简单的点云可视化器，如下所示：
Void RunVisualization(const vector<cv::Point3d>& pointcloud,
const std::vector<cv::Vec3b>& pointcloud_RGB) {
PopulatePCLPointCloud(pointcloud,pointcloud_RGB);
SORFilter();
copyPointCloud(*cloud,*orig_cloud);
pcl::visualization::CloudViewer viewer("Cloud Viewer");
// run the cloud viewer
viewer.showCloud(orig_cloud,"orig");
while (!viewer.wasStopped ())
{
// NOP
}
}下图显示了使用统计异常值去除工具后的输出。 左侧的图像是 SfM 的原始合成云，带有相机位置和云的特定部分的放大视图。 右侧的图像显示了 SOR 操作后的过滤云。 我们可以注意到一些杂散点被移除，留下更干净的点云：
使用实例代码
我们可以在本书的支持材料中找到 SfM 的示例代码。现在，我们将了解如何构建、运行和使用它。 该代码使用了 CMake，这是一个类似于 Maven 或 SCons 的跨平台构建环境。 我们还应该确保我们具备以下所有构建应用程序的先决条件：
- OpenCV v2.3 或更高版本
- PCL v1.6 或更高版本
- SSBA v3.0 或更高版本
如果我们使用 Linux、Mac OS 或其他类 Unix 操作系统，我们执行以下命令：
cmake –G "Unix Makefiles" -DSSBA_LIBRARY_DIR=../3rdparty/SSBA-3.0/build/ ..
