直接法视觉里程计(LK,DM,)

目标：

• 理解光流法跟踪特征点的原理

• 理解直接法是如何估计相机位姿的

• 使用g2o进行直接法的计算。

1、直接法引出

特征点法的缺点：

	缺点	解决思路
	关键点的提取与描述子的计算非常耗时。	1、保留特征点，只计算关键点，不计算描述子，使用光流跟踪特征点运动（仍使用特征点，匹配描述子换成光流跟踪，估计相机运动仍使用对极几何，Pnp或ICP算法）
	关注与特征点，忽略了特征点之外的所有信息。	法2、只计算关键点，不计算描述子，使用直接法，计算特征点在下一时刻图像的位置。
	特征缺失（白墙，空荡荡的走廊）	法3、既不计算关键点，也不计算描述子，根据像素灰度差异，直接计算相机运动。

法1还是特征点法；法2，法3，则是直接法；

	特征点法	直接法
优化相机运动	特征点在相机中的投影，最小化投影误差	在直接法中，我们并不需要知道点与点之间之间的对应关系，而是
通过最小化光度误差（Photometric error）来求得它们 最小化光度误差
		SVO，LSD_SLAM
	重构稀疏特征点（稀疏地图）	直接法分为稀疏、稠密和
半稠密三种。
		直接法根据
像素的亮度信息，估计相机的运动

区别：

• 需要配对点，通过其它法得到

• 不需要配对点，直接得到关系

不提特征计算VO的思路：

• 通过其它方式寻找配对点–光流

• 无配对点–直接法

2、光流（特征点法）

一般分为稀疏光流和稠密光流

• 稀疏以Lucas-Kanade（LK）光流为代表

• 稠密以Horn-Schunck（HS）光流为代表（计算所有像素）

        本质上是估计像素再不同的时刻图像中的运动。

2.1 Lucas-Kanade光流

• 灰度不变假设

$$
\left[
\begin{array}{}
u \
v
\end{array}
\right]
^*
= -(A^T A)^{-1} A^T b
$$

$$

3、实践：LK光流

3.1 TMU数据集

1、 rgb.txt, depth.txt 记录了文件的采集时间和对应的文件名
2、 rgb/ depth/ 目录存放采集到的png格式图像，彩图为八位三通道，深度图16位单通道，文件名即采集时间。
3、groundtruth.txt为外部运动捕捉系统采集到的相机位姿（time,tx,ty,tz,qx,qy,qz,qw)

slambook/tooks/associate.py可以进行数据对齐（彩色图，和深度图匹配）
python associate.py rgb.txt depth.txt > associate.txt

3.2 LK光流

OpenCV进行LK光流处理。跟踪特征点。

对第一张图像提取Fast角点。然后LK光流进行跟踪它们；绘画在图中。

ch8/useLK/useLk.cpp

#include <iostream>
#include <fstream>
#include <list>
#include <vector>
#include <chrono>
using namespace std;

#include <opencv4/opencv2/core/core.hpp>
#include <opencv4/opencv2/highgui/highgui.hpp>
#include <opencv4/opencv2/features2d/features2d.hpp>
#include <opencv4/opencv2/video/tracking.hpp>

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 > key_points;      // 因为要删除跟踪失败的点，使用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)
            {
                key_points.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:key_points )
            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=key_points.begin(); iter!=key_points.end(); i++)
        {
            if ( status[i] == 0 )
            {
                iter = key_points.erase(iter);
                continue;
            }
            *iter = next_keypoints[i];
            iter++;
        }
        cout<<"tracked keypoints: "<<key_points.size()<<endl;
        if (key_points.size() == 0)
        {
            cout<<"all key_points are lost."<<endl;
            break; 
        }
        // 画出 key_points
        cv::Mat img_show = color.clone();
        for ( auto kp:key_points )
            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;
}

运行

./build/useLK ../data

最后，我们可以通过光流跟踪的特征点，用 PnP、 ICP 或对极几何来估计相机运动，
这些方法在上一章中都讲过，我们不再讨论。总而言之，光流法可以加速基于特征点的视
觉里程计算法，避免计算和匹配描述子的过程，但要求相机运动较慢（或采集频率较高）。

4、直接法

实践：RGB-D的直接法

直接法估计相机运动

4.1 推导：

4.2 讨论

5 实践 RGB-D的直接法

5.1 稀疏直接法

slambook/ch8/directMethod/direct_sparse.cpp

5,2 定义直接法的边

/********************************************
 * 本节演示了RGBD上的稀疏直接法 
 ********************************************/

// 一次测量的值，包括一个世界坐标系下三维点与一个灰度值
struct Measurement
{
    Measurement ( Eigen::Vector3d p, float g ) : pos_world ( p ), grayscale ( g ) {}
    Eigen::Vector3d pos_world;
    float grayscale;
};

inline Eigen::Vector3d project2Dto3D ( int x, int y, int d, float fx, float fy, float cx, float cy, float scale )
{
    float zz = float ( d ) /scale;
    float xx = zz* ( x-cx ) /fx;
    float yy = zz* ( y-cy ) /fy;
    return Eigen::Vector3d ( xx, yy, zz );
}

inline Eigen::Vector2d project3Dto2D ( float x, float y, float z, float fx, float fy, float cx, float cy )
{
    float u = fx*x/z+cx;
    float v = fy*y/z+cy;
    return Eigen::Vector2d ( u,v );
}

// 直接法估计位姿
// 输入：测量值（空间点的灰度），新的灰度图，相机内参； 输出：相机位姿
// 返回：true为成功，false失败
bool poseEstimationDirect ( const vector<Measurement>& measurements, cv::Mat* gray, Eigen::Matrix3f& intrinsics, Eigen::Isometry3d& Tcw );


// project a 3d point into an image plane, the error is photometric error
// an unary edge with one vertex SE3Expmap (the pose of camera)
class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EdgeSE3ProjectDirect() {}

    EdgeSE3ProjectDirect ( Eigen::Vector3d point, float fx, float fy, float cx, float cy, cv::Mat* image )
        : x_world_ ( point ), fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), image_ ( image )
    {}

    virtual void computeError()
    {
        const VertexSE3Expmap* v  =static_cast<const VertexSE3Expmap*> ( _vertices[0] );
        Eigen::Vector3d x_local = v->estimate().map ( x_world_ );
        float x = x_local[0]*fx_/x_local[2] + cx_;
        float y = x_local[1]*fy_/x_local[2] + cy_;
        // check x,y is in the image
        if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows )
        {
            _error ( 0,0 ) = 0.0;
            this->setLevel ( 1 );
        }
        else
        {
            _error ( 0,0 ) = getPixelValue ( x,y ) - _measurement;
        }
    }

    // plus in manifold
    virtual void linearizeOplus( )
    {
        if ( level() == 1 )
        {
            _jacobianOplusXi = Eigen::Matrix<double, 1, 6>::Zero();
            return;
        }
        VertexSE3Expmap* vtx = static_cast<VertexSE3Expmap*> ( _vertices[0] );
        Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ );   // q in book

        double x = xyz_trans[0];
        double y = xyz_trans[1];
        double invz = 1.0/xyz_trans[2];
        double invz_2 = invz*invz;

        float u = x*fx_*invz + cx_;
        float v = y*fy_*invz + cy_;

        // jacobian from se3 to u,v
        // NOTE that in g2o the Lie algebra is (\omega, \epsilon), where \omega is so(3) and \epsilon the translation
        Eigen::Matrix<double, 2, 6> jacobian_uv_ksai;

        jacobian_uv_ksai ( 0,0 ) = - x*y*invz_2 *fx_;
        jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_;
        jacobian_uv_ksai ( 0,2 ) = - y*invz *fx_;
        jacobian_uv_ksai ( 0,3 ) = invz *fx_;
        jacobian_uv_ksai ( 0,4 ) = 0;
        jacobian_uv_ksai ( 0,5 ) = -x*invz_2 *fx_;

        jacobian_uv_ksai ( 1,0 ) = - ( 1+y*y*invz_2 ) *fy_;
        jacobian_uv_ksai ( 1,1 ) = x*y*invz_2 *fy_;
        jacobian_uv_ksai ( 1,2 ) = x*invz *fy_;
        jacobian_uv_ksai ( 1,3 ) = 0;
        jacobian_uv_ksai ( 1,4 ) = invz *fy_;
        jacobian_uv_ksai ( 1,5 ) = -y*invz_2 *fy_;

        Eigen::Matrix<double, 1, 2> jacobian_pixel_uv;

        jacobian_pixel_uv ( 0,0 ) = ( getPixelValue ( u+1,v )-getPixelValue ( u-1,v ) ) /2;
        jacobian_pixel_uv ( 0,1 ) = ( getPixelValue ( u,v+1 )-getPixelValue ( u,v-1 ) ) /2;

        _jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;
    }

    // dummy read and write functions because we don't care...
    virtual bool read ( std::istream& in ) {}
    virtual bool write ( std::ostream& out ) const {}

protected:
    // get a gray scale value from reference image (bilinear interpolated)
    inline float getPixelValue ( float x, float y )
    {
        uchar* data = & image_->data[ int ( y ) * image_->step + int ( x ) ];
        float xx = x - floor ( x );
        float yy = y - floor ( y );
        return float (
                   ( 1-xx ) * ( 1-yy ) * data[0] +
                   xx* ( 1-yy ) * data[1] +
                   ( 1-xx ) *yy*data[ image_->step ] +
                   xx*yy*data[image_->step+1]
               );
    }
public:
    Eigen::Vector3d x_world_;   // 3D point in world frame
    float cx_=0, cy_=0, fx_=0, fy_=0; // Camera intrinsics
    cv::Mat* image_=nullptr;    // reference image
};

int main ( int argc, char** argv )
{
    if ( argc != 2 )
    {
        cout<<"usage: useLK path_to_dataset"<<endl;
        return 1;
    }
    srand ( ( unsigned int ) time ( 0 ) );
    string path_to_dataset = argv[1];
    string associate_file = path_to_dataset + "/associate.txt";

    ifstream fin ( associate_file );

    string rgb_file, depth_file, time_rgb, time_depth;
    cv::Mat color, depth, gray;
    vector<Measurement> measurements;
    // 相机内参
    float cx = 325.5;
    float cy = 253.5;
    float fx = 518.0;
    float fy = 519.0;
    float depth_scale = 1000.0;
    Eigen::Matrix3f K;
    K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f;

    Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity();

    cv::Mat prev_color;
    // 我们以第一个图像为参考，对后续图像和参考图像做直接法
    for ( int index=0; index<10; index++ )
    {
        cout<<"*********** loop "<<index<<" ************"<<endl;
        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 ( color.data==nullptr || depth.data==nullptr )
            continue; 
        cv::cvtColor ( color, gray, cv::COLOR_BGR2GRAY );
        if ( index ==0 )
        {
            // 对第一帧提取FAST特征点
            vector<cv::KeyPoint> keypoints;
            cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
            detector->detect ( color, keypoints );
            for ( auto kp:keypoints )
            {
                // 去掉邻近边缘处的点
                if ( kp.pt.x < 20 || kp.pt.y < 20 || ( kp.pt.x+20 ) >color.cols || ( kp.pt.y+20 ) >color.rows )
                    continue;
                ushort d = depth.ptr<ushort> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ];
                if ( d==0 )
                    continue;
                Eigen::Vector3d p3d = project2Dto3D ( kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale );
                float grayscale = float ( gray.ptr<uchar> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ] );
                measurements.push_back ( Measurement ( p3d, grayscale ) );
            }
            prev_color = color.clone();
            continue;
        }
        // 使用直接法计算相机运动
        chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
        poseEstimationDirect ( measurements, &gray, K, Tcw );
        chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
        chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
        cout<<"direct method costs time: "<<time_used.count() <<" seconds."<<endl;
        cout<<"Tcw="<<Tcw.matrix() <<endl;

        // plot the feature points
        cv::Mat img_show ( color.rows*2, color.cols, CV_8UC3 );
        prev_color.copyTo ( img_show ( cv::Rect ( 0,0,color.cols, color.rows ) ) );
        color.copyTo ( img_show ( cv::Rect ( 0,color.rows,color.cols, color.rows ) ) );
        for ( Measurement m:measurements )
        {
            if ( rand() > RAND_MAX/5 )
                continue;
            Eigen::Vector3d p = m.pos_world;
            Eigen::Vector2d pixel_prev = project3Dto2D ( p ( 0,0 ), p ( 1,0 ), p ( 2,0 ), fx, fy, cx, cy );
            Eigen::Vector3d p2 = Tcw*m.pos_world;
            Eigen::Vector2d pixel_now = project3Dto2D ( p2 ( 0,0 ), p2 ( 1,0 ), p2 ( 2,0 ), fx, fy, cx, cy );
            if ( pixel_now(0,0)<0 || pixel_now(0,0)>=color.cols || pixel_now(1,0)<0 || pixel_now(1,0)>=color.rows )
                continue;

            float b = 255*float ( rand() ) /RAND_MAX;
            float g = 255*float ( rand() ) /RAND_MAX;
            float r = 255*float ( rand() ) /RAND_MAX;
            cv::circle ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), 8, cv::Scalar ( b,g,r ), 2 );
            cv::circle ( img_show, cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), 8, cv::Scalar ( b,g,r ), 2 );
            cv::line ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), cv::Scalar ( b,g,r ), 1 );
        }
        cv::imshow ( "result", img_show );
        cv::waitKey ( 0 );

    }
    return 0;
}

bool poseEstimationDirect ( const vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw )
{
    // 初始化g2o
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock;  // 求解的向量是6＊1的
    DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense< DirectBlock::PoseMatrixType > ();
    DirectBlock* solver_ptr = new DirectBlock ( linearSolver );
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // G-N
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr ); // L-M
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );
    optimizer.setVerbose( true );

    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
    pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) );
    pose->setId ( 0 );
    optimizer.addVertex ( pose );

    // 添加边
    int id=1;
    for ( Measurement m: measurements )
    {
        EdgeSE3ProjectDirect* edge = new EdgeSE3ProjectDirect (
            m.pos_world,
            K ( 0,0 ), K ( 1,1 ), K ( 0,2 ), K ( 1,2 ), gray
        );
        edge->setVertex ( 0, pose );
        edge->setMeasurement ( m.grayscale );
        edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() );
        edge->setId ( id++ );
        optimizer.addEdge ( edge );
    }
    cout<<"edges in graph: "<<optimizer.edges().size() <<endl;
    optimizer.initializeOptimization();
    optimizer.optimize ( 30 );
    Tcw = pose->estimate();
}

5.3 使用直接法估计相机运动

build/direct_sparse ~/dataset/rgbd_dataset_freiburg1_desk

5.4 半稠密直接法

    我们很容易就能把程序拓展成半稠密的直接法形式。对参考帧中，先提取梯度较明显
的像素，然后用直接法，以这些像素为图优化边，来估计相机运动。

            // select the pixels with high gradiants 
for ( int x=10; x<gray.cols-10; x++ )
    for ( int y=10; y<gray.rows-10; y++ )
    {
        Eigen::Vector2d delta (
            gray.ptr<uchar>(y)[x+1] - gray.ptr<uchar>(y)[x-1], 
            gray.ptr<uchar>(y+1)[x] - gray.ptr<uchar>(y-1)[x]
        );
        if ( delta.norm() < 50 )
            continue;
        ushort d = depth.ptr<ushort> (y)[x];
        if ( d==0 )
            continue;
        Eigen::Vector3d p3d = project2Dto3D ( x, y, d, fx, fy, cx, cy, depth_scale );
        float grayscale = float ( gray.ptr<uchar> (y) [x] );
        measurements.push_back ( Measurement ( p3d, grayscale ) );
    }

这只是一个很简单的改动。我们把先前的稀疏特征点改成了带有明显梯度的像素。于是在
图优化中会增加许多的边。这些边都会参与估计相机位姿的优化问题，利用大量的像素而
不单单是稀疏的特征点。由于我们并没有使用所有的像素，所以这种方式又称为半稠密方
法（Semi-dense）

5.5 直接法的讨论

相比于特征点法，直接法完全依靠优化来求解相机位姿。从式（8.16）中可以看到，像
素梯度引导着优化的方向。如果我们想要得到正确的优化结果，就必须保证大部分像素梯
度能够把优化引导到正确的方向。

5.6 直接法优缺点总结

优点	省略计算特征点，描述子的时间，
	只需要有像素梯度即可，无需特征点
	可以构建半稠密，乃至稠密地图，（特征点法无法做到）
缺点	非凸性
	单个像素没有区分度
	混渎职不变是很强的假设

思考

1、除了 LK 光流之外，还有哪些光流方法？它们各有什么特点？

2、在本节的程序的求图像梯度过程中，我们简单地求了 u + 1 和 u - 1 的灰度之差除 2，
作为 u 方向上的梯度值。这种做法有什么缺点？提示：对于距离较近的特征，变化
应该较快；而距离较远的特征在图像中变化较慢，求梯度时能否利用此信息？

3、在稀疏直接法中，假设单个像素周围小块的光度也不变，是否可以提高算法鲁棒性？
请编程实现此事。

4、使用 Ceres 实现 RGB-D 上的稀疏直接法和半稠密直接法。

5、相比于 RGB-D 的直接法，单目直接法往往更加复杂。除了匹配未知之外，像素的距
离也是待估计的。我们需要在优化时把像素深度也作为优化变量。阅读 [59, 57]，你
能理解它的原理吗？如果不能，请在 13 讲之后再回来阅读。

6、由于图像的非凸性，直接法目前还只能用于短距离，非自动曝光的相机。你能否提出
增强直接法鲁棒性的方案？阅读 [58, 60] 可能会给你一些灵感。

必备知识：

• 泰勒展开

• 超定线性方程（最小二乘法求解）