第四章来至运动的结构

1、 探测来至运动的结构在这一章，我们将讨论来至运动的结构的概念（SfM），或者从一个运动的相机拍摄到的图像中更好的推测提取出来的几何结构，我们使用OpenCV的Api函数来帮我们完成。让我们将我们使用的冗长的方法约束为使用单个摄像机，通常称为单目方法，并且是一组分离的和稀疏的视频帧而不是连续的视频流。这两个约束在很大程度上简化了系统，这个系统我们将在接下来的页码中进行描述，并且帮助我们理解任何SfM方法的原理。为了实现我们的方法，我们将跟随Hartley和Zisserman的脚步（后面称作H和Z），伴随着他们有创意的书计算机视觉中的多视觉几何的第9章到第12章的记录。在这一章，我们将涉及到以下内

2、容：1、 来至运动结构的概念2、 从一组图像中估计摄像机的运动3、 重构场景4、 从多个视图中重构5、 可视化3D点云自始自终本章假定使用一个标定过的摄像机一个先前标定过的摄像机。标定是在计算机视觉中普遍存在的操作，在OpenCV中得到很好的支持，我们可以使用在前面一张讨论的命令行方法来完成标定。因此，我们假定摄像机内参数存在，并且具体化到K矩阵中，K矩阵为摄像机标定过程的一个结果输出。为了在语言方面表达的清晰，从现在开始我们将单个摄像机称为场景的单个视图而不是光学和获取图像的硬件。一个摄像机在空间中有一个位置，一个观察的方向。在两个摄像机中，有一个平移成分（空间运动）和一个观察方向的旋转。我

3、们同样对场景中的点统一一下术语，世界（world），现实（real），或者3D，意思是一样的，即我们真实世界存在的一个点。这同样适用于图像中的点或者2D,这些点在图像坐标系中，是在这个位置和时间上一些现实的3D点投影到摄像机传感器上形成的。在这一章的代码部分，你将注意参考计算视觉中的多视觉几何（Multiple View Geometry in Computer Vision），例如，/HZ9.12，这指的是这本书的第9章第12个等式。同样的，文本仅包括代码的摘录，而完整的代码包含在伴随着这本书的材料中。来至运动结构的概念第一个我们应当区别的是立体（Stereo or indeed any m

4、ultiview），使用标准平台的三维重建和SfM之间的差异。在两个或多个摄像机的平台中，假定我们已经知道了两个摄像机之间的运动，而在SfM中，我们实际上不知道这个运动并且我们希望找到它。标准平台，来至观察的一个过分简单的点，可以得到一个更加精确的3D几何的重构，因为在估计多个摄像机间距离和旋转时没有误差（距离和旋转已知）。实现一个SfM系统的第一步是找到相机之间的运动。OpenCV可以帮助我们在许多方式上获得这个运动，特别地，使用findFundamentalMat函数。让我们想一下选择一个SfM算法背后的目的。在很多情况下，我们希望获得场景的几何。例如，目标相对于相机的位置和他们的形状是什

5、么。假定我们知道了捕获同一场景的摄像机之间的运动，从观察的一个合理的相似点，现在我们想重构这个几何。在计算视觉术语中称为三角测量（triangulation），并且有很多方法可以做这件事。它可以通过 射线相交的方法完成，这里我们构造两个射线：来至于每个摄像机投影中心的点和每一个图像平面上的点。理想上，这两个射线在空间中将相交于真实世界的一个3D点（这个3D点在每个摄像机中成像），如下面图表展示：实际上，射线相交非常不可靠。H和Z推荐不使用它。这是因为两个射线通常不能相交，让我们回到使用连接两个射线的最短线段上的中间点。相反，H和Z建议一些方法来三角化3D点（trianglulate 3D po

6、ints），这些方法中我们将在重构场景部分讨论两个。OpenCV现在的版本没有包含三角测量的API，因此，这部分我们将自己编写代码。学习完如何从两个视图恢复3D几何之后，我们将看到我们是怎么样加入更多的同一个场景的视图来获得更丰富的重构。在那时，大部分的SfM方法试图依靠束调整（Bundle Adjustment）来优化我们的摄像机和3D点一束估计位置（the bundle of estimated positon），这部分内容在重构提纯部分（Refinement of the reconstruction section）。OpenCV在新的图像拼接的工具箱内包含了用来束调整的方法。然而，使

7、OpenCV和C+完美结合的工作是丰富的外部工具，这些工具可以很容易地整合到一起。因此，我们将看到如何如何整合一个外部的束调节器灵巧的SSBA库。既然我们已经描述了使用OpenCV实现我们的SfM方法的一个概括，我们将看到每个分成是如何实现的。使用一对图像估计摄像机的运动事实上，在我们开始着手两个摄像机之间的运动之前，让我们检查一下我们手边用来执行这个操作的输入和工具。首先，我们有来至（希望并不是非常）不同空间位置的同一场景的两个图像。这是一个强大的资产，并且我们将确保使用它。现在工具都有了，我们应当看一下数学对象，来对我们的图像，相机和场景施加约束。两个非常有用的数学对象是基础矩阵（用F表示

8、）和本征矩阵（用E表示）。除了本征矩阵是假设使用的标定的相机，它们非常相似。这种情况针对我们，所以我们将选着它。OpenCV函数仅允许我们通过findFundamentalMat函数找到基础矩阵。然而，我们非常简单地使用标定矩阵（calibration matrix）K从本征矩阵中获得基础矩阵，如下：Mat_<double> E = K.t() * F * K; /according to HZ (9.12)本征矩阵，是一个3×3大小的矩阵，使用xEx=0在图像中的一点和另外一个图像中的一点之间施加了一个约束，这里x是图像一中的的一点，x是图像二中与之相对应的一点。这非常

9、有用，因为我们将要看到。我们使用的另一个重要的事实是本征矩阵是我们用来为我们的图像恢复两个相机的所有需要，尽管只有尺度，但是我们以后会得到。因此，如果我们获得了本征矩阵，我们知道每一个相机在空间中的位置，并且知道它们的观察方向，如果我们有足够这样的约束等式，那么我们可以简单地计算出这个矩阵。简单的因为每一个等式可以用来解决矩阵的一小部分。事实上，OpenCV允许我们仅使用7个点对来计算它，但是我们希望获得更多的点对来得到一个鲁棒性的解。使用丰富的特征描述子进行点匹配现在我们将使用我们的约束等式来计算本征矩阵。为了获得我们的约束，记住对于图像A中的每一个点我们必须在图像B中找到一个与之相对应的点

10、。我们怎样完成这个匹配呢？简单地通过使用OpenCV的广泛的特征匹配框架，这个框架在过去的几年了已经非常成熟。在计算机视觉中，特征提取和描述子匹配是一个基础的过程，并且用在许多方法中来执行各种各样的操作。例如，检测图像中一个目标的位置和方向，或者通过给出一个查询图像在大数据图像中找到相似的图像。从本质上讲，提取意味着在图像中选择点，使得获得好的特征，并且为它们计算一个描述子。一个描述子是含有多个数据的向量，用来描述在一个图像中围绕着特征点的周围环境。不同的方法有不同的长度和数据类型来表示描述子矢量。匹配是使用它的描述子从另外一个图像中找到一组与之对应的特征。OpenCV提供了非常简单和有效的方

11、法支持特征提取和匹配。关于特征匹配的更多信息可以在Chapter 3少量（无）标记增强现实中找到。让我们检查一个非常简单特征提取和匹配方案：/ detectingkeypointsSurfFeatureDetectordetector();vector<KeyPoint> keypoints1, keypoints2;detector.detect(img1, keypoints1);detector.detect(img2, keypoints2);/ computing descriptorsSurfDescriptorExtractor extractor;Mat descr

12、iptors1, descriptors2;pute(img1, keypoints1, descriptors1);pute(img2, keypoints2, descriptors2);/ matching descriptorsBruteForceMatcher<L2<float>> matcher;vector<DMatch> matches;matcher.match(descriptors1, descriptors2, matches);你可能已经看到类似的OpenCV代码，但是让我们快速的回顾一下它。我们的目的是获得三个元素：两个图像的特征

13、点，特征点的描述子，和两个特征集的匹配。OpenCV提供了一组特征检测器、描述子提取器和匹配器。在这个简单例子中，我们使用SurfFeatureDetector函数来获得SURF（Speeded-Up-Robust Features）特征的2D位置，并且使用SurfDescriptorExtractor函数来获得SURF描述子。我们使用一个brute-force匹配器来获得匹配，这是一个最直接方式来匹配两个特征集，该方法是通过比较第一个集中的每一个特征和第一个集的每一个特征并且获得最好的匹配来实现的。在下一个图像中，我们将看到两个图像上的特征点的匹配：（这两个图像来至于the Fountain

14、-P11 sequence，可以在网址中找到：http:/cvlab.epfl.ch/strecha/multiview/denseMVS.html.）实际上，像我们执行的原始匹配（raw matching），只有到达某一程度时执行效果才比较好并且许多匹配可能是错误的。因此，大多数SfM方法对原始匹配进行一些滤波方式来确保正确和减少错误。一种滤波方式叫做交叉检验滤波，它内置于OpenCV的brute-force匹配器中。也就是说，如果第一个图像的一个特征匹配第二个图像的一个特征，并且通过反向检查，第二个图像的特征也和第一个图像的特征匹配，那么这个匹配被认为是正确的。另一个常见的滤波机制（该机制

15、用在提供的代码中）是基于同一场景的两个图像并且在他们之间存在某一种立体视觉关系，这样的一个事实基础之上来滤波的。在实践中，这个滤波器尝试采用鲁棒性的算法来计算这个基础矩阵，我们将会在寻找相机矩阵（Finding camera matrices）部分学习这种计算方法，并且保留由该计算得到的带有小误差的特征对。使用光流进行点匹配一个替代使用丰富特征（如SURF）匹配的方法，是使用optical flow（OF光流）进行匹配。下面的信息框提供了光流的一个简短的概述。最近的OpenCV为从两个图像获得流场扩展了API，并且现在更快，更强大。我们将尝试使用它作为匹配特征的一个替代品。【注释：光流是匹配来

16、至一幅图像选择的点到另外一幅图像选着点的过程，假定这两个图像是一个视频序列的一部分并且它们彼此非常相近。大多数的光流方法比较一个小的区域，称为搜索窗口或者块，这些块围绕着图像A中的每一点和同样区域的图像B中的每一点。遵循计算机视觉中一个非常普通的规则，称为亮度恒定约束（brightness constancy constraint）（和其他名字），图像中的这些小块从一个图像到另外一个图像不会有太大的变化，因此，他们的幅值差接近于0。除了匹配块，更新的光流方法使用一些额外的方法来获得更好的结果。其中一个方法就是使用图像金字塔，它是图像越来越小的尺寸（大小）版本，这考虑到了工作的从粗糙到精致计算机

17、视觉中一个非常有用的技巧。另外一个方法是定义一个流场上的全局约束，假定这些点相互靠近，向同一方向一起运动。在OpenCV中，一个更加深入的光流方法可以在Chapter Developing Fluid Wall Using the Microsoft Kinect中找到，这个书可以在出版社网站上访问到。】在OpenCV中，使用光流相当的简单，可以通过调用calcOpticalFlowPyrLK函数来实现。我们想要保存光流的结果匹配，像使用丰富的特征那样，在未来，我们希望两种方法可以互换。为了这个目标，我们必须安装一个特殊的匹配方法可以与基于特征的方法互换，这将在下面的代码中看到：Vector&

18、lt;KeyPoint>left_keypoints,right_keypoints;/ Detect keypoints in the left and right imagesFastFeatureDetectorffd;ffd.detect(img1, left_keypoints);ffd.detect(img2, right_keypoints);vector<Point2f>left_points;KeyPointsToPoints(left_keypoints,left_points);vector<Point2f>right_points(left

19、_points.size();/ making sure images are grayscaleMat prevgray,gray;if (img1.channels() = 3) cvtColor(img1,prevgray,CV_RGB2GRAY);cvtColor(img2,gray,CV_RGB2GRAY); else prevgray = img1;gray = img2;/ Calculate the optical flow field:/ how each left_point moved across the 2 imagesvector<uchar>vstat

20、us; vector<float>verror;calcOpticalFlowPyrLK(prevgray, gray, left_points, right_points, vstatus, verror);/ First, filter out the points with high errorvector<Point2f>right_points_to_find;vector<int>right_points_to_find_back_index;for (unsigned inti=0; i<vstatus.size(); i+) if (v

21、statusi &&verrori < 12.0) / Keep the original index of the point in the/ optical flow array, for future useright_points_to_find_back_index.push_back(i);/ Keep the feature point itselfright_points_to_find.push_back(j_ptsi); else vstatusi = 0; / a bad flow/ for each right_point see which de

22、tected feature it belongs toMat right_points_to_find_flat = Mat(right_points_to_find).reshape(1,to_find.size(); /flatten arrayvector<Point2f>right_features; / detected featuresKeyPointsToPoints(right_keypoints,right_features);Mat right_features_flat = Mat(right_features).reshape(1,right_featur

23、es.size();/ Look around each OF point in the right image/ for any features that were detected in its area/ and make a match.BFMatchermatcher(CV_L2);vector<vector<DMatch>>nearest_neighbors;matcher.radiusMatch(right_points_to_find_flat,right_features_flat,nearest_neighbors,2.0f);/ Check th

24、at the found neighbors are unique (throw away neighbors/ that are too close together, as they may be confusing)std:set<int>found_in_right_points; / for duplicate preventionfor(inti=0;i<nearest_neighbors.size();i+) DMatch _m;if(nearest_neighborsi.size()=1) _m = nearest_neighborsi0; / only on

25、e neighbor else if(nearest_neighborsi.size()>1) / 2 neighbors check how close they aredouble ratio = nearest_neighborsi0.distance / nearest_neighborsi1.distance;if(ratio < 0.7) / not too close/ take the closest (first) one_m = nearest_neighborsi0; else / too close we cannot tell which is bette

26、rcontinue; / did not pass ratio test throw away else continue; / no neighbors. :(/ prevent duplicatesif (found_in_right_points.find(_m.trainIdx) = found_in_right_points.end() / The found neighbor was not yet used:/ We should match it with the original indexing / ofthe left point_m.queryIdx = right_p

27、oints_to_find_back_index_m.queryIdx; matches->push_back(_m); / add this matchfound_in_right_points.insert(_m.trainIdx);cout<<"pruned "<< matches->size() <<" / "<<nearest_neighbors.size() <<" matches"<<endl;函数KeyPointsToPoints和Po

28、intsToKeyPoints是用来进行cv:Point2f和cv:KeyPoint结构体之间相互转换的简单方便的函数。从先前的代码片段我们可以看到一些有趣的事情。第一个注意的事情是，当我们使用光流时，我们的结果表明，一个特征从左手边图像的一个位置移动到右手边图像的另外一个位置。但是我们有一组在右手边图像中检测到的新的特征，在光流中从这个图像到左手边图像的特征不一定是对齐的。我们必须使它们对齐。为了找到这些丢失的特征，我们使用一个k邻近（KNN）半径搜索,这给出了我们两个特征，即感兴趣的点落入了2个像素半径范围内。我们可以看得到另外一个事情是用来测试KNN的比例的实现，在SfM中这是一种常见的

29、减少错误的方法。实质上，当我们对左手边图像上的一个特征和右手边图像上的一个特征进行匹配时，它作为一个滤波器，用来移除混淆的匹配。如果右手边图像中两个特征太靠近，或者它们之间这个比例（the rate）太大（接近于1.0），我们认为它们混淆了并且不使用它们。我们也安装一个双重防御滤波器来进一步修剪匹配。下面的图像显示了从一幅图像到另外一幅图像的流场。左手边图像中的分红色箭头表示了块从左手边图像到右手边图像的运动。在左边的第二个图像中，我们看到一个放大了的小的流场区域。粉红色箭头再一次表明了块的运动，并且我们可以通过右手边的两个原图像的片段看到它的意义。在左手边图像上可视的特征正在向左移动穿过图像

30、，粉红色箭头的方向在下面的图像中展示：使用光流法代替丰富特征的优点是这个过程通常更快并且可以适应更多的点，使重构更加稠密。在许多光流方法中也有一个块整体运动的统一模型，在这个模型中，丰富的特征匹配通常不考虑。使用光流要注意的是对于从同一个硬件获取的连续图像，它处理的很快，然而丰富的特征通常不可知。它们之间的差异源于这样的一个事实：光流法通常使用非常基础的特征，像围绕着一个关键点的图像块，然而，高阶丰富特征（例如，SURF）考虑每一个特征点的较高层次的信息。使用光流或者丰富的特征是设计师根据应用程序的输入所做的决定。找到相机矩阵-Finding camera matrices 既然我们获得了两个

31、关键点之间的匹配，我们可以计算基础矩阵并且从中获得本征矩阵。然而，我们必须首先调整我们的匹配点到两个数组，其中数组中的索引对应于另外一个数组中同样的索引。这需要通过findFundametalMat函数获得。我们可能也需要转换KeyPoint 结构到Point2f结构。我们必须特别注意DMatch的queryIdx和trainIdx成员变量，在OpenCV中DMatch是容纳两个关键点匹配的结构，因为它们必须匹配我们使用matche.match()函数的方式。下面的代码部分展示了如何调整一个匹配到两个相应的二维点集，以及如何使用它们来找到基础矩阵：vector<Point2f>im

32、gpts1,imgpts2;for( unsigned inti = 0; i<matches.size(); i+ )/ queryIdx is the "left" imageimgpts1.push_back(keypoints1matchesi.queryIdx.pt);/ trainIdx is the "right" imageimgpts2.push_back(keypoints2matchesi.trainIdx.pt);Mat F = findFundamentalMat(imgpts1, imgpts2, FM_RANSAC,

33、0.1, 0.99, status);Mat_<double> E = K.t() * F * K; /according to HZ (9.12)稍后我们可能使用status二值向量来修剪这些点，使这些点和恢复的基础矩阵匹配。看下面的图像：用来阐述使用基础矩阵修剪后的点匹配。红色箭头表示特征匹配在寻找基础矩阵的过程中被移除了，绿色箭头表示被保留的特征匹配。现在我们已经准备好寻找相机矩阵。这个过程在H和Z书的第九章进行了详细的描述。然而，我们将使用一个直接和简单的方法来实现它，并且OpenCV很容易的为我们做这件事。但是首先，我们将简短地检查我们将要使用的相机矩阵的结构：我们的相机

34、使用该模型，它由两个成分组成，旋转（表示为R）和平移（表示为t）。关于它的一个有趣的事情是它容纳一个非常基本的等式：x=PX，这里x是图像上的二维点，X是空间上的三维点。还有更多，但是这个矩阵给我们一个非常重要的关系，即图像中点和场景中点之间的关系。因此，既然我们有了寻找摄像机矩阵的动机，那么我们将看到这个过程是怎么完成的。下面的的代码部分展示了如何将本征矩阵分解为旋转和平移成分。SVD svd(E);Matx33d W(0,-1,0,/HZ 9.131,0,0,0,0,1);Mat_<double> R = svd.u * Mat(W) * svd.vt; /HZ 9.19Mat

35、_<double> t = svd.u.col(2); /u3Matx34d 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分解成为两部分，一个旋转成分和一个平移成分。实时上，本征矩阵起初是通过这两个成分相乘构成的。完全地是满足我们的好奇心，我们可以看下面的本征矩阵的等式，

36、它在字面意义上表现为：E=txR（x是t的下标）。我可以看到它是由一个平移成分和一个旋转成分组成。我们注意到，我们所做的仅是得到了一个相机矩阵，因此另外一个相机矩阵呢？好的，我们在假定一个相机矩阵是固定的并且是标准的（没有旋转和平移）情况下进行这个操作。下一个矩阵（这里的下一个矩阵表示相对于上面的P）也是标准的：我们从本征矩阵恢复的是另外一个相机相对于固定的相机进行了移动和旋转。这同样意味着我们从这两个相机矩阵中恢复的任何三维点都是拥有第一个相机在世界坐标系中的原点（0，0，0）。然而，这不是一个完全解。H和Z在他们的书中展示了如何和为什么这样的分解有四个可能的相机矩阵，但是仅有一个是正确的。

37、正确的相机矩阵将产生带有一个正Z值（点在摄像机的前面）的重构点。但是我们仅有当学了下一步将要讨论的三角测量和3维重建之后才能理解。还有一个我们可以考虑添加到我们的方法中的事情是错误检测。多次从点的匹配中计算基础矩阵是错误的，并影响相机矩阵。带有错误相机矩阵的进行三角测量是毫无意义的。我们可以安装一个检查来检测是否旋转成分是一个有效的旋转矩阵。牢记旋转矩阵必须是一个行列式值为1（或者-1），我们可以简单地进行如下做法：bool CheckCoherentRotation(cv:Mat_<double>& R) if(fabsf(determinant(R)-1.0 >

38、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,Matx34

39、d& P,Matx34d& P1,vector<DMatch>& matches,vector<CloudPoint>& outCloud)/Find camera matrices/Get Fundamental MatrixMat F = GetFundamentalMat(imgpts1,imgpts2,matches);/Essential matrix: compute then extract cameras R|tMat_<double> E = K.t() * F * K; /according to HZ (

40、9.12)/decompose E to 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.131,0,0,0,0,1);Mat_<double> R = svd_u * Mat(W) * svd_vt; /HZ 9.19Mat_<double> t = svd_u.col(2); /u3if (!CheckCoherentRotation(R) cout<<"

41、;resulting rotation is not coherentn"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结构的事情。像先前所做的，我们应当看一下，用来完

42、成这个事情我们手边所拥有的工具和信息。在前一部分，我们从本征矩阵和矩阵矩阵中获得了两个相机矩阵。我们已经讨论了这些工具用来获得空间中一点的3D位置是如何的有用。那么，我可以返回我们的匹配点来用数据填充等式。点对同样对于计算我们获得的近似计算的误差有用。现在我们看一些如何使用OpenCV执行三角测量。这次我们将会按照Tartley和Strum的三角测量的文章的步骤，文章中他们实现和比较了一些三角剖分的方法。我们将实现他们的线性方法的一种，因为使用OpenCv非常容易编程。回忆一下，我们有两个由2D点匹配和P矩阵产生的关键等式：x=PX和x=PX，x和x是匹配的二维点，X是两个相机进行拍照的真实世

43、界三维点。如果我们重写这个等式，我们可以公式化为一个线性方程系统，该系统可以解出X的值，X正是我们所期望寻找的值。假定X=(x,y,z,1)t（一个合理的点的假设，这些点离相机的中心不太近或者不太远）产生一个形式为AX=B的非齐次线性方程系统。我们可以编码和解决这个问题，如下：Mat_<double> LinearLSTriangulation(Point3d u,/homogenous image point (u,v,1)Matx34d P,/camera 1 matrixPoint3d u1,/homogenous image point in 2nd cameraMatx3

44、4d P1/camera 2 matrix)/build A matrixMatx43d 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 vectorMa

45、tx41d 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 XMat_<double> X;solve(A,B,X,DECOMP_SVD);return X;我们将获得由两个2维点产生的3D点的一个近似。还有一个要注意的事情是，2D点用齐次坐标表示，意味着x和y的值后面追加一个1。我们必须确保这些点在标准化的坐标系中，这意味着它们必须乘以先前的标定矩阵K.我们可能注意到我们简单地利用KP矩阵来替代k矩阵和每一个点相乘（每一点乘以KP)

46、,就是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 (

47、unsigned int i=0; i<pts_size; i+) /convert to normalized homogeneous coordinatesPoint2f kp = pt_set1i.pt;Point3d u(kp.x,kp.y,1.0);Mat_<double> um = Kinv * Mat_<double>(u);u = um.at<Point3d>(0);Point2f kp1 = pt_set2i.pt;Point3d u1(kp1.x,kp1.y,1.0);Mat_<double> um1 = Kinv *

48、Mat_<double>(u1);u1 = um1.at<Point3d>(0);/triangulateMat_<double> X = LinearLSTriangulation(u,P,u1,P1);/calculate reprojection errorMat_<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);/s

49、tore 3D pointpointcloud.push_back(Point3d(X(0),X(1),X(2);/return mean reprojection errorScalar me = mean(reproj_error);return me0;在下面的图像中我们将看到一个两个图像三角测量的结果。这两个图像来至于P-11序列：http:/cvlab.epfl.ch/strecha/multiview/denseMVS.html.上面的两个图像是原始场景的两个视图，下面的一对图像是从两个视图重构得到的点云视图，包含着估计相机朝向喷泉。我们可以看到右手边红色砖块墙部分是如何重构的，并

50、且也可以看到突出于墙的喷泉。然而，像我们前面提到的那样，关于重构存在着这样的一个问题：重构仅能达到尺度上的。我们应当花一些时间来理解达到尺度（up-to-scale）的意思。我们获得的两个摄像机之间的运动存在一个随意测量的单元，也就是说，它不是用厘米或者英寸，而是简单地给出尺度单位。我们重构的相机将是尺度单元距离上的分开。这在很大程度上暗示我们应当决定过会重新获得更多的相机，因为每一对相机都拥有各自的尺度单元，而不是一个一般的尺度。现在我们将讨论我们建立的误差测量是如何可能的帮助我们来找到一个更加鲁棒性的重构。首先，我们需要注意重投影意味着我们简单地利用三角化的3D点并且将这些点重塑到相机上以

51、获得一个重投影的2D点。如果这个距离很大，这意味着我们在三角测量中存在一个误差，因此，在最后的结果中，我们可能不想包含这个点。我们的全局测量是平均重投影距离并且可能提供一个暗示我们的三角剖分总体执行的怎么样。高的重投影率可能表明P矩阵存在问题，因此该问题可能是存在于本征矩阵的计算或者匹配特征点中。我们应当简短地回顾一下在前一部分我们讨论的相机矩阵。我们提到相机矩阵P1的分解可以通过四个不同的方式进行分解，但是仅有一个分解是正确的。既然，我们知道如何三角化一个点，我们可以增加一个检测来看四个相机矩阵中哪一个是有效地的。我们应当跳过在一点实现上的细节，因为它们是本书随书实例代码中的精选（专题）。下

52、一步，我们将要看一下重新获得直视场景的更多的相机，并且组合这些3D重建的结果。从多视图中重建既然我们知道了如何从两个相机中恢复运动和场景几何，简单地应用相同的过程获得额外相机的参数和更多场景点似乎不重要。事实上，这件事并不简单因为我们仅可以获得一个达到尺度的重构，并且每对图像给我们一个不同的尺度。有一些方法可以正确的从多个视场中重构3D场景数据。一个方法是后方交会（resection）或者相机姿态估计（camera pose estimation），也被称为PNP（Perspective-N-Point），这里我们将使用我们已经找到的场景点来解决一个新相机的位置。另外一个方法是三角化更多的点并

53、且看它们是如何适应于我们存在的场景几何的。凭借ICP（Iterative Closest Point ）我们可以获得新相机的位置。在这一章，我们将讨论使用OpenCV的sovlePnP函数来完成第一个方法。第一步我们选择这样的重构类型即使用相机后方交会的增加的3D重构，来获得一个基线场景结构。因为我们将基于一个已知的场景结构来寻找任何相机的位置，我们需要找到要处理的一个初始化的结构和一条基线。我们可以使用先前讨论的方法例如，在第一个视频帧和第二个视频帧之间，通过寻找相机矩阵（使用FindCameraMatrices函数）来获得一条基线并且三角化几何（使用TriangulatePoints函数）

54、。发现一个基础结构后，我们可以继续。然而，我们的方法需要相当多的数据记录。首先，我们需要注意solvePnP函数需要两个对齐的3D和2D点的矢量。对齐的矢量意味着一个矢量的第i个位置与另外一个矢量的第i位置对齐。为了获得这些矢量，我们需要在我们早前恢复的3D点中找到这些点，这些点与在我们新视频帧下的2D点是对齐的。完成这个的一个简单的方式是，对于云中的每一个3D点，附加一个来至2D点的矢量。然后我们可以使用特征匹配来获得一个匹配对。让我们为一个3D点引入一个新的结构，如下：struct CloudPoint cv:Point3d pt;std:vector<int>index_of

55、_2d_origin;它容纳，3D点和一个容器，容器内的元素为每帧图像上2D点的索引值，这些2D点用来计算3D点。当三角化一个新的3D点时，index_of_2d_origin的信息必须被初始化，来记录在三角化中哪些相机涉及到。然而，我们可以使用它来从我们的3D点云追溯到每一帧上的2D点，如下：std:vector<CloudPoint> pcloud; /our global 3D point cloud/check for matches between i'th frame and 0'th frame (and thus the current cloud)

56、std:vector<cv:Point3f> ppcloud;std:vector<cv:Point2f> imgPoints;vector<int> pcloud_status(pcloud.size(),0);/scan the views we already used (good_views)for (set<int>:iterator done_view = good_views.begin(); done_view != good_views.end(); +done_view) int old_view = *done_view;

57、/a view we already used for reconstrcution/check for matches_from_old_to_working between <working_view>'th frame and <old_view>'th frame (and thus the current cloud)std:vector<cv:DMatch> matches_from_old_to_working = matches_matrixstd:make_pair(old_view,working_view);/scan

58、the 2D-2D matched-pointsfor (unsigned int match_from_old_view=0; match_from_old_view<matches_from_old_to_working.size(); match_from_old_view+) / the index of the matching 2D point in <old_view>int idx_in_old_view = matches_from_old_to_workingmatch_from_old_view.queryIdx;/scan the existing cloud to see if this point from <old_view> exists for (unsigned int pcldp=0; pcldp<pcloud.size(); pcldp+) /