仅供参考，还未运行程序，理解部分有误，请参考英文原版。

绿色部分非文章内容，是个人理解。

转载请注明：http://blog.csdn.net/raby_gyl/article/details/17471617

Chapter 4:Exploring Structure from  Motion Using OpenCV

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

在这一章，我们将涉及到以下内容：

1、来至运动结构的概念

2、从一对图像中估计摄像机的运动

3、重构场景

4、从多个视图中重构

5、重构提纯（Refinement of the reconstruction）

6、可视化3D点云

本章自始自终假定使用一个标定过的相机——预先标定的相机。在计算机视觉中标定是一个普遍存在的操作，在OpenCV中得到了很好的支持，我们可以使用在前一章讨论的命令行工具来完成标定。因此，我们假定摄像机内参数存在，并且具体化到K矩阵中，K矩阵为摄像机标定过程的一个结果输出。

为了在语言方面表达的清晰，从现在开始我们将单个摄像机称为场景的单个视图而不是光学和获取图像的硬件。一个摄像机在空间中有一个位置，一个观察的方向。在两个摄像机中，有一个平移成分（空间运动）和一个观察方向的旋转。我们同样对场景中的点统一一下术语，世界（world），现实（real），或者3D，意思是一样的，即我们真实世界存在的一个点。这同样适用于图像中的点或者2D,这些点在图像坐标系中，是在这个位置和时间上一些现实的3D点投影到摄像机传感器上形成的。

在这一章的代码部分，你将注意参考《计算视觉中的多视觉几何》（Multiple View Geometry in Computer Vision），例如，//HZ9.12，这指的是这本书的第9章第12个等式。同样的，文本仅包括代码的摘录，而完整的代码包含在伴随着这本书的材料中。

来至运动结构的概念——Structure from Motion concepts

让我们想一下选择一个SfM算法背后的目的。在很多情况下，我们希望获得场景的几何。例如，目标相对于相机的位置和他们的形状是什么。假定我们知道了捕获同一场景的摄像机之间的运动，从观察的一个合理的相似点，现在我们想重构这个几何。在计算视觉术语中称为三角测量（triangulation），并且有很多方法可以做这件事。它可以通过 射线相交的方法完成，这里我们构造两个射线：来至于每个摄像机投影中心的点和每一个图像平面上的点。理想上，这两个射线在空间中将相交于真实世界的一个3D点（这个3D点在每个摄像机中成像），如下面图表展示：

Mat_<double> E = K.t() * F * K; //according to HZ (9.12)

// detectingkeypoints
SurfFeatureDetectordetector();
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);

在下一个图像中，我们将看到两个图像上的特征点的匹配：（这两个图像来至于the Fountain-P11 sequence，可以在网址中找到：http://cvlab.epfl.ch/~strecha/multiview/denseMVS.html.）

Vector<KeyPoint>left_keypoints,right_keypoints;
// Detect keypoints in the left and right images
FastFeatureDetectorffd;
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());
// making sure images are grayscale
Mat 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 images
vector<uchar>vstatus; vector<float>verror;
calcOpticalFlowPyrLK(prevgray, gray, left_points, right_points, 
vstatus, verror);
// First, filter out the points with high error
vector<Point2f>right_points_to_find;
vector<int>right_points_to_find_back_index;
for (unsigned inti=0; i<vstatus.size(); i++) {
if (vstatus[i] &&verror[i] < 12.0) {
// Keep the original index of the point in the
// optical flow array, for future use
right_points_to_find_back_index.push_back(i);
// Keep the feature point itself
right_points_to_find.push_back(j_pts[i]);
} else {
vstatus[i] = 0; // a bad flow
}
}
// for each right_point see which detected feature it belongs to
Mat right_points_to_find_flat = Mat(right_points_to_find).
reshape(1,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());
// 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 that 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 prevention
for(inti=0;i<nearest_neighbors.size();i++) {
DMatch _m;
if(nearest_neighbors[i].size()==1) {
_m = nearest_neighbors[i][0]; // only one neighbor
} else if(nearest_neighbors[i].size()>1) {
// 2 neighbors – check how close they are
double ratio = nearest_neighbors[i][0].distance / 
nearest_neighbors[i][1].distance;
if(ratio < 0.7) { // not too close
// take the closest (first) one
_m = nearest_neighbors[i][0];
} else { // too close – we cannot tell which is better
continue; // did not pass ratio test – throw away
}
} else {
continue; // no neighbors... :(
}
// prevent duplicates
if (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_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;

下面的图像显示了从一幅图像到另外一幅图像的流场。左手边图像中的分红色箭头表示了块从左手边图像到右手边图像的运动。在左边的第二个图像中，我们看到一个放大了的小的流场区域。粉红色箭头再一次表明了块的运动，并且我们可以通过右手边的两个原图像的片段看到它的意义。在左手边图像上可视的特征正在向左移动穿过图像，粉红色箭头的方向在下面的图像中展示：

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

vector<Point2f>imgpts1,imgpts2;
for( unsigned inti = 0; i<matches.size(); i++ )
{
// queryIdx is the "left" image
imgpts1.push_back(keypoints1[matches[i].queryIdx].pt);
// trainIdx is the "right" image
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)

稍后我们可能使用status二值向量来修剪这些点，使这些点和恢复的基础矩阵匹配。看下面的图像：用来阐述使用基础矩阵修剪后的点匹配。红色箭头表示特征匹配在寻找基础矩阵的过程中被移除了，绿色箭头表示被保留的特征匹配。

现在我们已经准备好寻找相机矩阵。这个过程在H和Z书的第九章进行了详细的描述。然而，我们将使用一个直接和简单的方法来实现它，并且OpenCV很容易的为我们做这件事。但是首先，我们将简短地检查我们将要使用的相机矩阵的结构：

我们的相机使用该模型，它由两个成分组成，旋转（表示为R）和平移（表示为t）。关于它的一个有趣的事情是它容纳一个非常基本的等式：x=PX，这里x是图像上的二维点，X是空间上的三维点。还有更多，但是这个矩阵给我们一个非常重要的关系，即图像中点和场景中点之间的关系。因此，既然我们有了寻找摄像机矩阵的动机，那么我们将看到这个过程是怎么完成的。下面的的代码部分展示了如何将本征矩阵分解为旋转和平移成分。

SVD svd(E);
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
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分解成为两部分，一个旋转成分和一个平移成分。实时上，本征矩阵起初是通过这两个成分相乘构成的。完全地是满足我们的好奇心，我们可以看下面的本征矩阵的等式，它在字面意义上表现为：E=[t]xR（x是t的下标）。我可以看到它是由一个平移成分和一个旋转成分组成。

我们注意到，我们所做的仅是得到了一个相机矩阵，因此另外一个相机矩阵呢？好的，我们在假定一个相机矩阵是固定的并且是标准的（没有旋转和平移）情况下进行这个操作。下一个矩阵（这里的下一个矩阵表示相对于上面的P）也是标准的：

我们从本征矩阵恢复的另外一个相机相对于固定的相机进行了移动和旋转。这同样意味着我们从这两个相机矩阵中恢复的任何三维点都是拥有第一个相机在世界坐标系中的原点（0，0，0）。

然而，这不是一个完全解。H和Z在他们的书中展示了如何和为什么这样的分解有四个可能的相机矩阵，但是仅有一个是正确的。正确的相机矩阵将产生带有一个正Z值（点在摄像机的前面）的重构点。但是我们仅有当学了下一步将要讨论的三角测量和3维重建之后才能理解。

还有一个我们可以考虑添加到我们的方法中的事情是错误检测。多次从点的匹配中计算基础矩阵是错误的，并影响相机矩阵。带有错误相机矩阵的进行三角测量是毫无意义的。我们可以安装一个检查来检测是否旋转成分是一个有效的旋转矩阵。牢记旋转矩阵必须是一个行列式值为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;
}
We can now see how all these elements combine into a function that recovers the P
matrices, as follows:
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
)
{
//Find camera matrices
//Get Fundamental Matrix
Mat F = GetFundamentalMat(imgpts1,imgpts2,matches);
//Essential matrix: compute then extract cameras [R|t]
Mat_<double> E = K.t() * F * K; //according to HZ (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.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变量中。下一部分我们将揭示如何使用这些相机来获得场景的三维重建。

重构场景——Reconstructing the scene

接下来我们看一下从我们已经获得的信息中恢复场景的3D结构的事情。像先前所做的，我们应当看一下，用来完成这个事情我们手边所拥有的工具和信息。在前一部分，我们从本征矩阵和矩阵矩阵中获得了两个相机矩阵。我们已经讨论了这些工具用来获得空间中一点的3D位置是如何的有用。那么，我可以返回我们的匹配点来用数据填充等式。点对同样对于计算我们获得的近似计算的误差有用。

现在我们看一些如何使用OpenCV执行三角测量。这次我们将会按照Tartley和Strum的三角测量的文章的步骤，文章中他们实现和比较了一些三角剖分的方法。我们将实现他们的线性方法的一种，因为使用OpenCv非常容易编程。

回忆一下，我们有两个由2D点匹配和P矩阵产生的关键等式：x=PX和x’=P’X，x和x’是匹配的二维点，X是两个相机进行拍照的真实世界三维点。如果我们重写这个等式，我们可以公式化为一个线性方程系统，该系统可以解出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 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;
}

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

在下面的图像中我们将看到一个两个图像三角测量的结果。这两个图像来至于P-11序列：

http://cvlab.epfl.ch/~strecha/multiview/denseMVS.html.

上面的两个图像是原始场景的两个视图，下面的一对图像是从两个视图重构得到的点云视图，包含着估计相机朝向喷泉。我们可以看到右手边红色砖块墙部分是如何重构的，并且也可以看到突出于墙的喷泉。

然而，像我们前面提到的那样，关于重构存在着这样的一个问题：重构仅能达到尺度上的。我们应当花一些时间来理解达到尺度（up-to-scale）的意思。我们获得的两个摄像机之间的运动存在一个随意测量的单元，也就是说，它不是用厘米或者英寸，而是简单地给出尺度单位。我们重构的相机将是尺度单元距离上的分开。这在很大程度上暗示我们应当决定过会重新获得更多的相机，因为每一对相机都拥有各自的尺度单元，而不是一个一般的尺度。

现在我们将讨论我们建立的误差测量是如何可能的帮助我们来找到一个更加鲁棒性的重构。首先，我们需要注意重投影意味着我们简单地利用三角化的3D点并且将这些点重塑到相机上以获得一个重投影的2D点。如果这个距离很大，这意味着我们在三角测量中存在一个误差，因此，在最后的结果中，我们可能不想包含这个点。我们的全局测量是平均重投影距离并且可能提供一个暗示——我们的三角剖分总体执行的怎么样。高的重投影率可能表明P矩阵存在问题，因此该问题可能是存在于本征矩阵的计算或者匹配特征点中。

我们应当简短地回顾一下在前一部分我们讨论的相机矩阵。我们提到相机矩阵P1的分解可以通过四个不同的方式进行分解，但是仅有一个分解是正确的。既然，我们知道如何三角化一个点，我们可以增加一个检测来看四个相机矩阵中哪一个是有效地的。我们应当跳过在一点实现上的细节，因为它们是本书随书实例代码中的精选（专题）。

下一步，我们将要看一下重新获得直视场景的更多的相机，并且组合这些3D重建的结果。

从多视图中重建——Reconstruction from many views 

既然我们知道了如何从两个相机中恢复运动和场景几何，简单地应用相同的过程获得额外相机的参数和更多场景点似乎不重要。事实上，这件事并不简单因为我们仅可以获得一个达到尺度的重构，并且每对图像给我们一个不同的尺度。

有一些方法可以正确的从多个视场中重构3D场景数据。一个方法是后方交会（resection）或者相机姿态估计（camera pose estimation），也被称为PNP（Perspective-N-Point），这里我们将使用我们已经找到的场景点来解决一个新相机的位置。另外一个方法是三角化更多的点并且看它们是如何适应于我们存在的场景几何的。凭借ICP（Iterative Closest Point ）我们可以获得新相机的位置。在这一章，我们将讨论使用OpenCV的sovlePnP函数来完成第一个方法。

第一步我们选择这样的重构类型即使用相机后方交会的增加的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点。当三角化一个新的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)
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; //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_matrix[std::make_pair(old_view,working_view)];
//scan the 2D-2D matched-points
for (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_working[match_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++) {
// see if this 2D point from <old_view> contributed to this 3D 
point in the cloud
if (idx_in_old_view == pcloud[pcldp].index_of_2d_origin[old_view] 
&& pcloud_status[pcldp] == 0) //prevent duplicates
{
//3d point in cloud
ppcloud.push_back(pcloud[pcldp].pt);
//2d point in image <working_view>
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));

既然我们正在使用sovlePnPRansac函数而不是sovlePnP函数，因为它对于异常值有更好的鲁棒性。既然我们获得了一个新的P1矩阵，我们可以简单的再次使用我们早先定义的TriangualtePoints函数并且用更多的3D点来填充我们的3D点云。

在下面的图像中，我们看到一个增加的喷泉场景的重构（访问：http://cvlab.epfl.ch/~strecha/multiview/denseMVS.html,）,从第四个图像。左上角的图像是使用了4个图像的重构；参加拍摄的相机用带有白线的红色简单表示，箭头表示了方向。其他的图像展示了更多 的相机来添加更多的点到点云中。

重构提纯——Refinement of the reconstruction 

SfM方法中最重要的一个部分是提纯和最优化重构场景，通常被称作BA过程（Bundle Adjustment）。这是一个优化步骤，这里我们获得的所有数据适应于一个统一的模型。3D点的位置和相机的位置都得到了最优化，因此重投影误差最小（也就是说估计的3D点重投影到图像上接近于起源的2D点的位置）。这个过程通常需要解决带有几十千个参数指令的巨大的线性方程。这个过程可能会有些费力，但是我们前面采取的步骤允许带有一个束调节（bundle adjuster）的简单的整合。前面看起来奇怪的事情变的清晰了。例如，我们保留为点云中的每一个3D点保存原始的2D点的理由。

束调节的一个实现算法是简单稀疏束条调节SSBA（Simple Sparse Bundle Adjustment）库。我们将选择它作为我们的BA优化器， 因为它拥有简单的API。它仅需要少量的输入参数，这些输入参数我们可以相当简单的从我们的数据结构中创建。SSBA中我们使用的关键对象是CommonInternasMetricBundleOptimizer函数，这个函数执行最优化。它需要相机参数，3D点云，点云中每一个点相对应的2D图像点，以及直视场景的相机。到现在为止，利用这些参数应该很直接。我们应当注意这个BA方法假定所有图像通过同样的硬件获取，因此共同的内部，其他操作模式可能不需要假定这样的情况。我们可以执行束调节（Bundle Adjustment），如下：

voidBundleAdjuster::adjustBundle(
vector<CloudPoint>&pointcloud,
const Mat&cam_intrinsics,
conststd::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;
// intrinsic parameters matrix
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;
// Normalize the measurements to match the unit focal length.
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点云——Visualizing 3D point clouds with PCL

当操作3D数据时，通过简单地观察重投影误差测量或原始点信息很难快速的理解结果是否正确。另一方面，如果我们观察点云（itself）,我们可以立即的检查这个点是否有意义或者存在误差。为了可视化，我们将使用一个很有前途的OpenCV的姊妹工程，称为点云库（Point Cloud Librar）（PCL）。它带有许多可视化和分析点云的工具，例如找到一个平面，匹配点云，分割目标以及排除异常值。如果我们的目标不是一个点云，而是一些高级信息例如3D模型，这些工具将非常有用。

首先，我们需要在PCL的数据结构中表示我们的点云（本质上是3D点的列表）。可以通过如下的做法实现：

pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
void PopulatePCLPointCloud(const vector<Point3d>& pointcloud,
const std::vector<cv::Vec3b>& pointcloud_RGB
)
//Populate point cloud
{
cout<<"Creating point cloud...";
cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
for (unsigned int i=0; i<pointcloud.size(); i++) {
// get the RGB color value for the point
Vec3b rgbv(255,255,255);
if (pointcloud_RGB.size() >= i) {
rgbv = pointcloud_RGB[i];
}
// check for erroneous coordinates (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 color, needs to be represented as an integer
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值。我们同样也可以对原始点云应用一个滤波器，这将消除可能是异常值的点，使用统计移除移除（statistical outlier removal）（SOR）工具如下：

Void SORFilter() {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointC
loud<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
}
}

下面的图像展示了统计移除移除工具（statistical outlier removal tool）使用之后的输出结果。左手边的图像是SfM的原始结果点云。右手边的图像展示经过SOR操作滤波之后的点云。我们能够注意到一些离群的点被移除了，剩下了一个更干净的点云。

使用实例代码——Using the example code 

我们可以在这本书提供的材料中找到SfM的实例代码。我们现在看一些怎么样编译，运行和利用它。代码使用CMake,一个交叉编译环境，类似于Maven或者SCons。我们同样应当确保我们有下面的所有前提条件来编译我们的应用程序：

•  OpenCV v2.3 or highe
•  PCL v1.6 or higher
•  SSBA v3.0 or higher

首先，我们必须建立编译环境。为此，我们可能创建一个文件夹，命名为build，我们将所有编译相关的文件存储在这里。现在我们将假定所有的命令行操作都是在build/文件夹内，虽然这个过程是类似的（取决于文件的位置），即使没有使用build文件夹。

我们应当确保CMake可以找到SSBA和PCL，如果PCL正确的安装了，那没有问题。然而，我们必须通过-DSSBA_LIBRARY_DIR=...编译参数设置正确的位置来找到SSBA的预编译库。如果我们正在使用Windows操作系统，我们可以使用Microsoft Visual Studio来编译，因此，我们应当运行下面的的命令：

cmake –G "Visual Studio 10" -DSSBA_LIBRARY_DIR=../3rdparty/SSBA-3.0/
build/ ..

如果我们使用Linux,Mac Os，或者其他Unix-Like操作系统，我们执行下面的命令：

cmake –G "Unix Makefiles" -DSSBA_LIBRARY_DIR=../3rdparty/SSBA-3.0/build/ 
..

如果我们喜欢使用MacOS上的XCode,执行下面的命令：

cmake –G Xcode -DSSBA_LIBRARY_DIR=../3rdparty/SSBA-3.0/build/ ..

CMake同样可以为Eclipse,Codeblocks,和更多的环境编译宏命令。CMake完成创建编译环境之后，我们准备编译。如果我们正在使用一个Unix-like系统，我们可以简单地执行这个生成工具（the make utility），否则我们应当使用我们开发环境的编译过程。

编译完成之后，我们应当获得了一个执行程序名为ExploringSfMExex，这用来运行SfM过程。不带参数运行这个程序会导致如下的显示： USAGE: ./ExploringSfMExec <path_to_images>

为了在图像集上执行这个过程，我们应当提供位置作为驱动来找到图像文件。如果提供了有效的位置，过程开始，我应当看到这个进程和屏幕上的调试信息。程序的结束将显示源于图像的点云。按1和2键，可以切换到调整的(adjusted)点云和非调整的(non-adjusted)点云。

总结——Summary

在这一章，我们已经看到了OpenCV是怎样用一个既简单编码又好理解的方式来帮助我们处理来至运动的结构。OpenCV的API包含了一些有用的函数和数据结构，这使得我们生活的更加轻松，同样地协助我们有一个更清洁的实现。

然而，最先进的SfM方法更复杂。那里存在很多问题，我们选择忽略，喜欢简单化，以及在这些地方通常有更多的错误检查。对于不同的SfM成分，我们选择的方法同样可以再次访问。例如，H和Z提出了一个高精度的三角测量方法，甚至使用N-视图三角测量，曾经他们利用多个图像理解特征之间的关系。

如果我们想延伸和深化熟悉SfM，当然，我们将从观察其他开源SfM库中收益。一个特别感兴趣的工程是libMV，它实现了大量的SfM成分，通过互换，这可能获得最好的结果。华盛顿大学有一个伟大的作品，为很多类型SfM（Bundler and Visual SfM）提供了工具。这项作品的灵感来至于微软的在线产品，称作PhotoSynth。网上有很多容易访问到的SfM的实现，并且一个人仅需来找到更多的SfM的实现。

我们没有深入讨论的另外一个重要的关系是SfM和可视化定位以及映射，更好的称为同步定位和映射（SLAM）方法。在这一章，我们已经处理给出的图像集和一个视频序列，并且在这些情况下，使用SfM是可行的。然而，一些应用没有预记录的数据集并且必须动态时引导重建。这个过程被成为映射，并且当我们正在使用特征匹配和2D跟踪以及三角测量后来创建世界的3D映射时，这个过程被完成。

在下一章，我们将看到如何使用机器学习中的各种技术利用OpenCV从图像中提出车牌数字。

参考——References

•  Multiple View Geometry in Computer Vision,Richard Hartley and Andrew 
Zisserman,Cambridge University Press
•  Triangulation, Richard I. Hartley and Peter Sturm, Computer vision and image 
understanding, Vol. 68, pp. 146-157
•  http://cvlab.epfl.ch/~strecha/multiview/denseMVS.html
•  On Benchmarking Camera Calibration and Multi-View Stereo for High Resolution 
Imagery,C. Strecha, W. von Hansen, L. Van Gool, P. Fua,and U. Thoennessen, 
CVPR
•  http://www.inf.ethz.ch/personal/chzach/opensource.html
•  http://www.ics.forth.gr/~lourakis/sba/
•  http://code.google.com/p/libmv/
•  http://www.cs.washington.edu/homes/ccwu/vsfm/
•  http://phototour.cs.washington.edu/bundler/
•  http://photosynth.net/
•  http://en.wikipedia.org/wiki/Simultaneous_localization_and_
mapping
•  http://pointclouds.org
•  http://www.cmake.org

[转]第四章 使用OpenCV探测来至运动的结构——Chapter 4:Exploring Structure from Motion Using OpenCV

原文：http://www.cnblogs.com/rongfangliu/p/5399209.html