视觉SLAM总结-前端视觉里程计-直接法

基于直接法的方法总结：

       SLAM 主要分为视觉前端和优化后端。前端也称为 视觉里程计（VO）。它根据相邻图像的信息，估计出粗略的相机运动，给后端提供较好的初始值。VO 的实现方法，按是否需要提取特征，分为特征点法的前端以及不提特征的直接法前端。尽管特征点法在视觉里程计中占据主流地位，研究者们认识它至少有以下几个缺点：

1. 关键点的提取与描述子的计算非常耗时。
2. 使用特征点时，忽略了除特征点以外的所有信息。
3. 相机有时会运动到特征缺失的地方，往往这些地方都没有什么明显的纹理信息。

我们看到使用特征点确实存在一些问题。针对这些缺点，也存在若干种可行的方法：

1. 只计算关键点，不计算描述子。同时，使用光流法（Optical Flow）来跟踪特征点的运动。这样可以回避计算和匹配描述子带来的时间，但光流本身的计算需要一定时间；
2. 只计算关键点，不计算描述子。同时，使用直接法来计算特征点在下一时刻图像的位置。这同样可以跳过描述子的计算过程，而且直接法的计算更加简单。
3. 既不计算关键点、也不计算描述子——根据像素来直接计算相机运动。

       第一种方法仍然使用特征点，只是把匹配描述子替换成了光流跟踪，估计相机运动时仍使用PnP或ICP算法。而在，后两个方法中，我们会根据图像的像素信息来计算相机运动，它们称为直接法。

       使用特征点法估计相机运动时，我们把特征点看作固定在三维空间的不动点。根据它们在相机中的投影位置，通过最小化重投影误差（Reprojection error）来优化相机运动。在这个过程中，我们需要精确地知道空间点在两个相机中投影后的像素位置——这也就是我们为何要对特征进行匹配或跟踪的理由。而在直接法中，我们最小化的不再是重投影误差，而是测量误差（Phometric error）。
　　直接法是本讲介绍的重点。它的存在就是为了克服特征点法的上述缺点（虽然它会引入另一些问题）。直接法直接根据像素亮度信息，估计相机的运动，可以完全不用计算关键点和描述子。于是，直接法既避免了特征的计算时间，也避免了特征缺失的情况。只要场景中存在明暗变化（可以是渐变，不形成局部的图像特征），直接法就能工作

1.光流法

  光流是一种描述像素随时间在图像之间运动的方法，计算部分像素的称为 稀疏光流，计算所有像素的称为 稠密光流。稀疏光流以 Lucas-Kanade光流 为代表，并可以在SLAM中用于跟踪特征点位置。

 

LK光流推导过程:

灰度不变假设

对左边一阶泰勒展开:

则

简写为

在LK光流中，假设某个窗口（w x w）内的像素具有相同的运动

简写为

从而得到图像间的运动速度或者某块像素的位置。

代码：

OpenCV 中相关函数：calcOpticalFlowPyrLK：C++ calcOpticalFlowPyrLK（）浅析

int main( int argc, char** argv )
{
    if ( argc != 2 )
    {
        cout<<"usage: useLK path_to_dataset"<<endl;
        return 1;
    }
    string path_to_dataset = argv[1];
    string associate_file = path_to_dataset + "/associate.txt";
    
    ifstream fin( associate_file );
    if ( !fin ) 
    {
        cerr<<"I cann't find associate.txt!"<<endl;
        return 1;
    }
    
    string rgb_file, depth_file, time_rgb, time_depth;
    list< cv::Point2f > keypoints;      // 因为要删除跟踪失败的点，使用list
    cv::Mat color, depth, last_color;
    
    for ( int index=0; index<100; index++ )
    {
        fin>>time_rgb>>rgb_file>>time_depth>>depth_file;
        color = cv::imread( path_to_dataset+"/"+rgb_file );
        depth = cv::imread( path_to_dataset+"/"+depth_file, -1 );
        if (index ==0 )
        {
            // 对第一帧提取FAST特征点
            vector<cv::KeyPoint> kps;
            cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
            detector->detect( color, kps );
            for ( auto kp:kps )
                keypoints.push_back( kp.pt );
            last_color = color;
            continue;
        }
        if ( color.data==nullptr || depth.data==nullptr )
            continue;
        // 对其他帧用LK跟踪特征点
        vector<cv::Point2f> next_keypoints; 
        vector<cv::Point2f> prev_keypoints;
        for ( auto kp:keypoints )
            prev_keypoints.push_back(kp);
        vector<unsigned char> status;
        vector<float> error; 
        chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
        cv::calcOpticalFlowPyrLK( last_color, color, prev_keypoints, next_keypoints, status, error );
        chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
        chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
        cout<<"LK Flow use time："<<time_used.count()<<" seconds."<<endl;
        // 把跟丢的点删掉
        int i=0; 
        for ( auto iter=keypoints.begin(); iter!=keypoints.end(); i++)
        {
            if ( status[i] == 0 )
            {
                iter = keypoints.erase(iter);
                continue;
            }
            *iter = next_keypoints[i];
            iter++;
        }
        cout<<"tracked keypoints: "<<keypoints.size()<<endl;
        if (keypoints.size() == 0)
        {
            cout<<"all keypoints are lost."<<endl;
            break; 
        }
        // 画出 keypoints
        cv::Mat img_show = color.clone();
        for ( auto kp:keypoints )
            cv::circle(img_show, kp, 10, cv::Scalar(0, 240, 0), 1);
        cv::imshow("corners", img_show);
        cv::waitKey(0);
        last_color = color;
    }
    return 0;
}

2.直接法

推导公式：

其中 Z1 是 P 的深度，Z2 是 P 在第二个相机坐标系下的深度，也就是 RP + t 的第三个坐标值；

       在直接法中，我们假设一个空间点在各个视角下，成像的灰度是不变的；所以要求解一个优化问题不是重投影误差，而是测量误差（Photometric Error），也就是 P的两个像的亮度误差：

优化该误差的目标函数：

直接法的优缺点总结

优点：

1. 可以省去计算特征点、描述子的时间。

2. 只要求有像素梯度即可，无须特征点。因此，直接法可以在特征缺失的场合下使用。

3. 可以构建半稠密乃至稠密的地图，这是特征点法无法做到的。

缺点：

1. 非凸性。直接法完全依靠梯度搜索，降低目标函数来计算相机位姿。其目标函数中需要取像素点的灰度值，而图像是强烈非凸的函数。这使得优化算法容易进入极小，只在运动很小时直接法才能成功。

2. 单个像素没有区分度。

3. 灰度值不变是很强的假设。如果相机是自动曝光的，当它调整曝光参数时，会使得图像整体变亮或变暗。光照变化时亦会出现这种情况。

参考：

1.https://blog.csdn.net/u011178262/article/details/89685134

2.https://blog.csdn.net/u011178262/article/details/85016981

3.https://blog.csdn.net/zfjBIT/article/details/96008619