一步步学习SFM 三

之前写的VO在feature很丰富的时候，比如fountain系列，貌似可以使用了。但是在面对草地等特征不足的时候，或者帧数增加的时候，solvePnPRansac就会找不到足够的inlier。在这篇博客中，作者提到了关键帧提取的重要性。并不是每一帧都要加入地图中，而是选取距离较远而且匹配成功，运动估计也正确的关键帧加入到地图中。

在code中，关键帧的定义是vector< FRAME > keyframes;进行匹配后要判断NOT_MATCHED，TOO_FAR_AWAY， TOO_CLOSE。这里注意每一个新的图像都是跟上一个keyframe匹配，而不是跟上一个图像匹配。

为了检查为什么inlier不够，我们可以把反投影得到的点显示出来。
cv::line(reprojected,imgPoints[ppt],projected3D[ppt],cv::Scalar(0,0,255),1);表示在名叫reprojected的图中用红色的直线连接原始的点和反投影点。
cv::line(reprojected,imgPoints[inliers[ppt]],projected3D[inliers[ppt]],cv::Scalar(0,0,255),1);在图中显示inlier，也用红色的直线连接。
for(int ppt=0;ppt<imgPoints.size();ppt++) {
cv::circle(reprojected, imgPoints[ppt], 2, cv::Scalar(255,0,0), CV_FILLED);
cv::circle(reprojected, projected3D[ppt], 2, cv::Scalar(0,255,0), CV_FILLED);
}
用蓝色点表示原始点，用绿色点表示反投影点。
cv::circle(reprojected, imgPoints[inliers[ppt]], 2, cv::Scalar(255,255,0), CV_FILLED);用蓝色加上绿色表示inlier点。

在inlier非常少的时候，光流的匹配貌似没有问题，可能是Find2D3DCorrespondences出现了问题。不过即使在inlier非常少的时候，found 1187 3d-2d point correspondences, 说明对应点的数目并不少，不知道为什么solvePnPRansac无法计算R，t。

Find2D3DCorrespondences在之前的点云中寻找当前图像中也出现的匹配点，貌似没什么问题。可能是计算3D点的时候误差逐渐增大，导致一段时间之后2D和3D的对应虽然可以找到，但是计算出的运动完全错误。这个问题在点云中也可以看出来，多个图像之后点云更加分散，导致3D和2D匹配计算运动很不准。在TriangulatePointsBetweenViews中减小反投影点残差的阈值，可以得到更准的3D点云，得到的inlier也就更多。

无论误差大小，总是需要处理从新初始化的情况，所以要拼接点云。在Find2D3DCorrespondences之后是FindPoseEstimation得到R，t，然后TriangulatePointsBetweenViews。 这时候就要用到之前求得的相机姿态。

这里又发现一个问题，PruneMatchesBasedOnF和FindCameraMatrices都是在用GetFundamentalMat，只不过PruneMatchesBasedOnF多了一行matches_matrix[std::make_pair(working_view,older_view)] = FlipMatches(matches_matrix[std::make_pair(older_view,working_view)]);原始代码中两个函数都要运行。也许是8点算法需要先除掉outlier，然后用inlier再计算一遍F才能准。

因为暂时早不到问题的根源，所以先用两个图像匹配，利用kitti中提供的groundtruth作为尺度。奇怪的是运行图像0到7的时候，提示三角定位失败，但是单独运行图像6和7则是成功的。发现是TriangulatePoints中的P必须是identity matrix，不能是上一次相机的姿态。可能是因为上次的姿态计算有误差，造成三角定位的误差过大而出错。

除了运行的问题，它的速度也偏慢，只有2Hz.可以尝试使用更快的特征点检测，比如SVO中用的fast，具体安装参考这里。

8算法不能处理所有点都在同一个平面的问题，具体见这里和这里。为了更好的计算E，可以使用5点算法。算法的code可以在这里，和这里找到。