从图像中计算相机运动
先从已知条件开始，我们知道两张图片，由相机在同一场景中运动得到。书中用的图片在这里。要恢复相机运动，我们要用到fundamental matrix F和essential matrix E。F可以从findFundamentalMat函数中得到。为了得到E，我们需要先校准相机，得到相机矩阵K。然后E=K.t*F*K.E是个3x3的矩阵，描述了同一点x在两个图像中的关系,x'Ex=0.
用特征描述算子匹配特征点
为了得到同一点在两张图像中的位置x和x'，我们需要找到匹配的点。我们可以用SURF来寻找特征点和描述算子，然后用brute force来寻找匹配。不过这些匹配中很多是错误的，所以需要过滤掉这些错误的匹配。opencv的brute force方法已经有了cross check filtering，只有当一个特征点对无论从图像A到图像B还是图像B到图像A都是最佳匹配的时候，它才会被保留。除此之外，在计算fundamental matrix的时候，我们只保留inlier。
用光流匹配特征点
光流可以用calcOpticalFlowPyrLK函数得到。从图像A的特征点计算光流到图像B的到的新特征点，并不一定会和图像B中检测到的特征点完全匹配。我们需要用KNN来把它们匹配在一起。另一个在SFM中常用的减小误差的方法是ratio test，它处理图像A中某特征点匹配到图像B中两个点的情况，这时我们不知道两个点哪个才是真正的匹配。ratio test会去掉图像B中两个相距很近的特征点，因为这时两个点都有可能是匹配点，我们完全无法得知该选择哪一个。判断方式是两个匹配点距离图像A中特征点距离的比值不能太大。
处理完距离太近的点，我们还要处理重复的点。这里使用了STL里面的set函数，可以给元素排序。关于set的find的说明在这里。注意原来书中用的是指针vector<DMatch>* matches，在自己写的函数中需要替换成std::vector<cv::DMatch>。我理解的处理重复的点是这样的。因为现在是把跟踪的点和图像B中实际的点做匹配，同一个跟踪点可能被匹配多次，这时候就通过if (found_in_cur_points.find(_m.trainIdx) == found_in_cur_points.end())来判断是不是之前出现过这个跟踪点。如果是新的跟踪点，我们还要使用原来图像A中的特征点的id，而不是图像A中跟踪成功的特征点的id。
使用光流的好处是比较快，能产生更多特征点，得到dense reconstruction。光流通常只用很基本的特征，比如特征点周围的patch，SURF则会考虑更多高级的特征。
恢复相机运动轨迹
相机运动可以用矩阵P表示，包含旋转R和平移t。一个很重要的关系是x=PX，x是图像中的2D点，X是空间中的3D点。这个公式描述了图像点和空间点之间的关系。我们对E进行SVD，然后乘以矩阵W。SVD把E分解为了旋转和平移两个部分。我们现在只得到了一个运动矩阵，它是相对于第一个相机而言的。我们假定第一个相机没有旋转和平移。所以通过x=PX恢复的3D点的坐标原点就是第一个相机。
很多时候F矩阵是错误的，从而运动矩阵也是错误的。为了判断运动的正确性，我们可以检查旋转矩阵，因为它的determinant必须是1或者-1.
重建场景
从2D特征点匹配和运动矩阵，我们已经得到了x=PX, x'=PX'。x和x'是图像A和图像B中的匹配点，X是空间中的3D点。用所有的特征点，我们可以得到一组公式来求解X。假定X=(x,y,z,1)t，可得AX=B。从而可以得到2D匹配点对所对应的3D点，这个过程叫做三角定位。这里注意2D点也是在homogeneous coordinates下，所以在x,y后面加个1.同时这些坐标需要normalized，就是说在使用之前要先乘以相机矩阵K。
因为只有一个相机，那么问题来了，恢复出来的几何结构单位到底是mm还是m呢？这个我们无从知晓，所以恢复的结构是up to scale的。两个相机之间的运动向量的单位可以是mm也可以是m，它只是一个unit scale。我们只能说两个相机之间的距离是一个单位长度，而不知道真实的物理距离。当有多个运动的时候，每两个相机运动的单位长度都是不同的，它们之间并不存在一个统一的单位。
为了得到更准确的重建，我们可以从得到的3D点反投影到图像中得到2D点，然后比较这些新的点和原来图像中的特征点的距离。如果距离很大，那么我们的三角定位得到的3D点很可能是错误的，我们就需要去掉这个点。这些距离的平均值可以用来作为一个全局的评判，如果平均值过大，那么P矩阵是错误的，可能因为错误的E矩阵或者错误的特征点匹配导致。
从多幅图像中重建场景
因为不同的图相对给出的重建有着不同的尺度，多幅图像重建并不是重复两幅图像重建过程那么简单。我们可以用PNP来根据已知空间点来求得新的相机位置。另一种方法是三角定位更多点然后观察它们是不是符合已经重建的场景，这时我们可以用ICP来求得相机位置。这里我们使用opencv中的solvePnP函数来实现第一种方法。
我们首先要找到一个baseline来确定场景的尺度。这个可以通过第一幅和第二幅图像得到相机运动矩阵，然后再三角定位获得空间点的位置。对每个空间的3D点，我们还需要记录它原来的2D点的位置，然后可以在新的图像中匹配得到对应点。这样我们就能获取新的相机位置。注意这里我们用solvePnPRansac而不是solvePnP，因为前者能更好的应对outliers.获得新相机位置之后，我们可以继续三角定位新的3D点。
我们需要新建一个结构来记录3D点和对应的2D点
struct CloudPoint {
cv::Point3d pt;
std::vector<int> index_of_2d_origin;
};
index_of_2d_origin 记录了3D点在图像中所有特征点的index.
重建优化
SFM中一个很重要的过程就是优化重建的场景，又叫做BA。我们把所有已知的数据合并成一个整体。这些数据包括空间点的位置和相机的位置。优化的判断依据是3D点反投影到图像得到2D点之后与原来真实的2D点之间的残差。
我们用SSBA来实现优化过程，具体由CommonInternalsMetricBundleOptimizer函数用来实现优化。这个函数需要知道相机参数，3D点云，每个3D点对应的2D点，和每个相机的位置。
用PCL实现3D点云可视化
首先把3D点转变为PCL的数据结构，也可以把RGB信息包括进去，然后用SOR来过滤outlier。
总结
这个SFM只是最基本的算法，有很多可以改进的地方。比如使用更加精确的三角定位方法，例如N view三角定位。对于SFM，还有很多其他的资源，如libMV，Bundler, VisualSfM。如果需要实时的进行场景重建，我们就要用到SLAM。
code的结构
在这里有上面的code。
在main函数中，第一步是RecoverDepthFromImages.
OnlyMatchFeatures我们首先用光流或者特征点匹配来匹配相邻图像的特征点。
PruneMatchesBasedOnF算出F矩阵之后去掉不符合的特征点
GetBaseLineTriangulation在两个图像中找到初始点云。用homography找到inlier最小的两个图像进行初始化，用的函数是FindCameraMatrices。先找到F矩阵，在计算出E矩阵，从E中回复R和T，判断四个解答中哪一个是对的。如果F矩阵的inlier足够多，那么认为它是合适的。接下来是TriangulatePointsBetweenViews，先得到两张图像中的对应点，再三角定位匹配点。然后我们去掉那些反投影残差很高的点。我们还要判断哪些点已经在之前的点云中出现过，避免重复添加相同的点。
得到初始的匹配之后，我们以此运动为基准处理以后的图像。用Find2D3DCorrespondences找到新的图像中特征点在已有点云中的对应特征点, 然后用FindPoseEstimation找到新的相机的运动轨迹。根据已知2D-3D点计算运动用的是solvePnPRansac。
得到几个相机的运动轨迹和点云后，我们可以用ssba来优化。关于ssba的解释在这里。要build这个library，新建一个3rdparty文件夹，然后在里面放入ssba的source code。根据这里提供的方法，再新建一个build文件夹，就可以进行build。在cmake，make之后，build里面会生成两个.a文件，就是static library。要在cmake里面link这个library，一个简单粗暴的方式就是直接把.a的full path，包括.a文件名，写到cmake的target_link_libraries.另外还要在include_directories中加入3rdparty/SSBA-3.0/，这样才能找到那些header file。
大哥，看了你这个文章很佩服，真正的SLAM大神，我也在美国在硅谷这里搞这个，不知道能否加个微信dengheng2011 如果可以，可以注明slam:)谢谢，非常高兴认识您！
ReplyDelete