视觉里程计

namespace: odometry

这里的视觉里程计特指RGBD视觉里程计。视觉里程计的作用是估计两帧之间相机的运动，是SLAM的第一步。在OnePiece中实现了两种视觉里程计，用得都比较广泛：基于特征点的稀疏匹配，以及基于光流的稠密匹配。用法非常简单：

std::shared_ptr<SparseTrackingResult> SparseTracking(const cv::Mat &source_color, const cv::Mat &target_color, 
const cv::Mat &source_depth, const cv::Mat &target_depth);


std::shared_ptr<SparseTrackingResult> SparseTracking(geometry::RGBDFrame &source_frame, 
geometry::RGBDFrame &target_frame);


std::shared_ptr<SparseTrackingResult> SparseTrackingMILD(const cv::Mat &source_color, const cv::Mat &target_color, 
const cv::Mat &source_depth, const cv::Mat &target_depth);

std::shared_ptr<SparseTrackingResult> SparseTrackingMILD(geometry::RGBDFrame &source_frame, 
geometry::RGBDFrame &target_frame);

std::shared_ptr<DenseTrackingResult> DenseTracking(const cv::Mat &source_color, const cv::Mat &target_color, 
const cv::Mat &source_depth, const cv::Mat &target_depth, 
const geometry::TransformationMatrix &initial_T, int term_type = 0);

std::shared_ptr<DenseTrackingResult> DenseTracking(geometry::RGBDFrame &source_frame, 
geometry::RGBDFrame &target_frame, 
const geometry::TransformationMatrix &initial_T, int term_type = 0);

你可以选择用一对深度图一对彩色图来匹配，也可以用两个之前介绍的geometry::RGBDFrame来匹配。返回的结果是一个智能指针，指向一个odometry::SparseTrackingResult对象，或者是odometry::DenseTrackingResult。二者的定义分别如下：

class SparseTrackingResult
{
    public:
    //相对位姿
    geometry::TransformationMatrix T;
    //匹配的inliers的id对
    geometry::FMatchSet correspondence_set_index;
    //匹配的inliers的点对
    geometry::PointCorrespondenceSet correspondence_set;
    //rmse误差
    double rmse = 1e6;
    //是否追踪成功
    bool tracking_success;
};
class DenseTrackingResult
{
    public:
    //相对位姿
    geometry::TransformationMatrix T;
    //匹配的inliers的id对
    geometry::PixelCorrespondenceSet pixel_correspondence_set;
    //匹配的inliers的点对
    geometry::PointCorrespondenceSet correspondence_set;
    //rmse误差
    double rmse = 1e6;
    //是否追踪成功
    bool tracking_success;
};

主要区别在于，二者最后得到的匹配点的ID类型不同，sparse tracking特征点ID是一维的，一对正确匹配的特征点类似于\((501, 308)\)，指的是source frame中的第501个特征点和第308个特征点为一对。而dense tracking最后点的ID是二维的，结果例如：\([(501, 308), (489, 311)]\)，指的是像素位置\((501, 308)\)对应的3D点与像素位置为\((489, 311)\)对应的3D点为一对。当然，可以将dense tracking的结果整合成与sparse tracking结果一样的形式，但是OnePiece认为这样更接近算法的本质，因为它透漏出一个信息就是：sparse tracking是基于提取出来的特征点的，而dense tracking是逐像素的。

SparseTracking

SparseTracking的基本步骤如下： - 提取特征点 - 特征点匹配 - outlier过滤 - 求得变换矩阵

OnePiece提取的是ORB特征点，速度快。在特征点匹配时，选择了两种策略：OpenCV的BFMatcher（brutal fource），以及MILD中提到的Sparse Match。这个部分的内容也在上述链接中开源了，文章可以参考Beyond SIFT Using Binary features in Loop Closure Detection，在code部分的relative_paper/sparse_match.pdf也是这篇文章。这个文章中提到的方法在OnePiece中被封装成一个类SparseMatcher，主要成员函数：

//Match
void Match(const geometry::Descriptor &source_descriptors, const geometry::Descriptor &target_descriptors, 
    geometry::DMatchSet & matches);
//在match之后，可以进一步改进得到的匹配，
//需要特征点的3D坐标，也就是只用于RGBD frame
void RefineMatches(const geometry::Descriptor &source_descriptors, const geometry::Point3List &source_local_points,
    const geometry::KeyPointSet &source_keypoints,  const geometry::KeyPointSet &target_keypoints, 
    const geometry::TransformationMatrix & H,geometry::DMatchSet & matches);

特征点过滤方法也很多，odometry::outlier_filter类提供了几个常用的方法。

//删除掉距离大于最小距离n倍的匹配（Naive）
void MinDistance(const geometry::KeyPointSet &source_keypoints,
    const geometry::KeyPointSet &target_keypoints,geometry::DMatchSet & matches);
//只保留次优距离与最优距离比值比较大的点（正确的匹配最优结果应该比次优结果好很多）
void KnnMatch(const geometry::Descriptor &source, 
    geometry::Descriptor & target, const cv::BFMatcher &matcher, 
    std::vector< cv::DMatch > &matches, float minRatio = 1.5);
//通过Ransac计算本征矩阵，清点inliers
void Ransac(const geometry::KeyPointSet &source_keypoints, 
    const geometry::KeyPointSet &target_keypoints,geometry::DMatchSet & matches);
//对当前检查的匹配对，随机选择几个别的匹配，查看他们是否consistent
//错误的匹配很难找到其他比较consistent的匹配对
void RanSaPC(const geometry::Point3List &source_local_points, 
    const geometry::Point3List &target_local_points, std::default_random_engine &engine, std::vector< cv::DMatch > &init_matches);

OnePiece将使用MILD中sparse match的tracking函数，封装成SparseTrackingMILD，一般来说相对于SparseTracking有更好的效果，可以得到更多的正确的inliers。

DenseTracking

DenseTracking的原理可以参考论文：Real-Time Visual Odometry from Dense RGB-D Images，也就是related_paper/steinbruecker_sturm_cremers_iccv11.pdf。 上述文章只是追踪颜色图的intensity，也就是光流法，但是很容易拓展到追踪深度图的depth。在OnePiece中，是可以根据最后一个参数term_type来选择使用颜色或者深度，或者是混合项：

if(term_type == 0 )//混合
std::cout<<BLUE<<"[DEBUG]::Using hybrid term"<<RESET<<std::endl;
else if(term_type == 1)//颜色
std::cout<<BLUE<<"[DEBUG]::Using photo term"<<RESET<<std::endl;
else if(term_type == 2)//深度
std::cout<<BLUE<<"[DEBUG]::Using geometry term"<<RESET<<std::endl;

DenseTracking的步骤： - 对source frame的每个pixel根据当前位姿计算source到target重投影的对应点 - 找到所有的inliers对应点 - 根据这些对于inliers求雅可比矩阵，改进位姿 - 重复上述过程

可以看到DenseTracking需要一个初始位姿，如果没有初始估计，需要假设这两个frame的相对位姿很接近。这个过程有点像PointToPoint的ICP，区别在于优化的目标函数不同，因此PointToPoint需要找到最近点而不是通过重投影，找到最近点的过程很耗时。

在具体实现时候，会利用图像金字塔，提高运行效率。

比较

SparseTracking特征点个数少，更快，也不需要初始位姿，但是只考虑了视觉信息，在某些纹理比较少或者颜色图很差的地方就不能用了。而DenseTracking比较慢，需要初始位姿，但是它更鲁棒，甚至在只有深度或者只有颜色图的情况下也能使用，在SparseTracking失败的时候，是一个很好的后备方案。

RGBDFrame

可以看到对于每一种tracking都有两个函数：分别是直接作用于rgb和depth上的：

std::shared_ptr<SparseTrackingResult> SparseTracking(const cv::Mat &source_color, const cv::Mat &target_color, 
const cv::Mat &source_depth, const cv::Mat &target_depth);

与作用于geometry::RGBDFrame上的：

std::shared_ptr<SparseTrackingResult> SparseTracking(geometry::RGBDFrame &source_frame, 
geometry::RGBDFrame &target_frame);

区别在于geometry::RGBDFrame会保存中间信息，搭建系统时可以避免重复计算：

//part of RGBDFrame
/*for sparse odometry*/
geometry::Descriptor descriptor;
geometry::KeyPointSet keypoints;
PointCloud feature_pcd;//for optimization


/*for dense odometry*/
cv::Mat gray;//gray image of rgb
cv::Mat depth32f;//refined depth
std::vector<cv::Mat> color_pyramid;
std::vector<cv::Mat> depth_pyramid;
std::vector<cv::Mat> color_dx_pyramid;
std::vector<cv::Mat> color_dy_pyramid;
std::vector<cv::Mat> depth_dx_pyramid;
std::vector<cv::Mat> depth_dy_pyramid;
std::vector<geometry::ImageXYZ> image_xyz;

上述中std::vector<geometry::ImageXYZ> image_xyz可以看作一种三维图片：对每个Pixel有一个对应点，通过像素坐标来找到对应的3D点，这也是开头提到的DenseTrackingResult中对应点ID为二维的原因。