一步步学习SFM 二

上一篇看了理论，这里主要看code。

在读取所有图像后， 用MultiCameraPnP初始化。如果没有给相机矩阵，在MultiCameraDistance里面会自动初始化一个。

OnlyMatchFeatures会用OFFeatureMatcher把所有已经读取的图像之间的光流计算出来并进行匹配，然后把结果存储在matches_matrix里面。

然后PruneMatchesBasedOnF会计算F，然后把不符合的匹配去掉。GetBaseLineTriangulation可以在所有图像中找到两张homography匹配最好的，然后计算R，t。

AdjustCurrentBundle使用SBA把点云和相机轨迹进行优化。其中有个参数是K = Count2DMeasurements(pointcloud);就是同一个点在几个图像中都被观察到了。如果没有检查新得到的点是否在已经有的点云中，不知道还能不能使用BA。

如果只计算当前frame与上一张之间的光流，在计算F的时候使用的也是这两个frame的匹配。FindCameraMatrices会输出tmp_pcloud表示outCloud。

这里遇到一个问题，就是TriangulatePointsBetweenViews总是找不到原来点云中的匹配点，导致所有的点都被认为是新的。在onlymatchfeature中，每一张新的frame都要跟原来所有的frame做匹配，否则TriangulatePointsBetweenViews找不到对应点。TriangulatePointsBetweenViews是以older view和working view为基准，所以不能只让当前frame和前一frame进行匹配。

还有一个问题是GetRGBForPointCloud总是报错，BUG: point id:1235490744 should not exist for img #2 which has only 9280，函数是在AdjustCurrentBundle中调用的GetRGBForPointCloud(pointcloud_beforeBA,pointCloudRGB_beforeBA);

可能是int pt_idx = _pcloud[i].imgpt_for_img[good_view]超过边界的问题。经过检查，_pcloud[i].imgpt_for_img.size()是2,表示imgpt_for_img里面只有working view和older view的匹配。后来发现PruneMatchesBasedOnF只处理了当前frame和前一个frame，加入for之后_pcloud[i].imgpt_for_img.size()就是3了。但是那个BUG还是会出现。

经过长时间debug，终于发现是TriangulatePointsBetweenViews函数中初始化new_triangulated[j].imgpt_for_img = std::vector<int>(2,-1);的问题，imgpt_for_img的size可能比实际用到的小。

should be pcloud[pt3d].imgpt_for_img[1] == submatches[ii].queryIdx instead of pcloud[pt3d].imgpt_for_img[0] == submatches[ii].queryIdx

在使用code的时候，如果是fountain dataset效果还可以，换成其他图像可能会计算不出有效F,需要把FindCameraMatrices函数中if(fabsf(determinant(E)) > 1e-07)阈值调高。

在连续运行的时候，会出现not enough inliers to consider a good pose，是因为在solvePnPRansac中inlier不够。根据这里的分析，可能是没有关键帧的提取。之后我会分析如何改进。