《第8讲 视觉里程计 2 》 读书笔记

本文是《视觉SLAM十四讲》第8讲的个人读书笔记，为防止后期记忆遗忘写的。

本节知识脉络

前端对于通过图像来估计位姿变化值R和t有两种主流方法，分别是特征点法和直接法。

特征点法有他的优缺点，缺点中比较明显的是耗时问题。而直接法不需要计算描述子，这是通过图片像素点来寻找并跟踪特征点，大大减少了时间的花费。

直接法是由光流法演化而来，所以有必要介绍下光流法。

直接法分为稀疏法、半稠密法（稠密法，需要GPU加速，忽略），分别介绍他们的程序实践。

然而，直接法也存在问题。特别是灰度不变强假设和图片只能微调变化的鲁棒性。

---

8.1 直接法的引出

特征点法缺点：

 方法：

1、保留特征点，但只计算关键点，不计算描述子。同时，使用光流法（Optical Flow）来 跟踪特征点的运动。这样可以回避计算和匹配描述子带来的时间，但光流本身的计算 需要一定时间；

2、只计算关键点，不计算描述子。同时，使用直接法（Direct Method）来计算特征点 在下一时刻图像的位置。这同样可以跳过描述子的计算过程，而且直接法的计算更加 简单。

3、既不计算关键点、也不计算描述子，而是根据像素灰度的差异，直接计算相机运动。

第一种方法仍然使用特征点，只是把匹配描述子替换成了光流跟踪，估计相机运动时 仍使用对极几何、PnP 或 ICP 算法。而在后两个方法中，我们会根据图像的像素灰度信 息来计算相机运动，它们都称为直接法。相对的，在直接法中，我们并不需要知道点与点之间之间的对应关系，而是 通过最小化光度误差（Photometric error）来求得它们。

1. 关键点的提取与描述子的计算非常耗时。一大半时间都花在计算特征点上。
2. 一大半时间都花在计算特征点上。丢弃了大部分可能有用的图像信息。
3. 相机有时会运动到特征缺失的地方，往往这些地方没有明显的纹理信息。面对一堵白墙，或者一个空荡荡的走廓。

直接法是本讲介绍的重点。它是为了克服特征点法的上述缺点而存在的。直接法根据 像素的亮度信息，估计相机的运动，可以完全不用计算关键点和描述子，于是，既避免了 特征的计算时间，也避免了特征缺失的情况。只要场景中存在明暗变化（可以是渐变，不 形成局部的图像梯度），直接法就能工作。根据使用像素的数量，直接法分为稀疏、稠密和 半稠密三种。相比于特征点法只能重构稀疏特征点（稀疏地图），直接法还具有恢复稠密或 半稠密结构的能力。

---

8.2 光流法

直接法是从光流演变而来的，光流描述了像素 在图像中的运动，而直接法则附带着一个相机运动模型。稀疏光流以 Lucas-Kanade 光流为代表，LK 光流常被用来跟踪 角点的运动。

我们希望用光流法估计这个特征点在其他时刻里图像的位置。引入 光流法的基本假设：

1. 灰度不变假设：同一个空间点的像素灰度值，在各个图像中是固定不变的。
2. 运动相同假设：设某一个窗 口内的像素具有相同的运动。

对左边进行泰勒展开，保留一阶项，得：那么有：

 

dx/dt 为像素在 x 轴上运动速度，而 dy/dt 为 y 轴速度，把它们记为 u, v。同 时 ∂I/∂x 为图像在该点处 x 方向的梯度，另一项则是在 y 方向的梯度，记为 Ix, Iy。图像灰度对时间的变化量记为 It。

那么有：

 

因为，窗口内像素具有同样的运动，那么有：    

这是一个关于 u, v 的超定线性方程，传统解法是求最小二乘解。     如果 一次迭代不够好的话，我们会多迭代几次这个方程。

---

8.3 实践：LK 光流

使用 LK 的目的是跟踪特征点。我 们对第一张图像提取 FAST 角点，然后用 LK 光流跟踪它们，并画在图中。

#include <iostream>
#include <fstream>
#include <list>
#include <vector>
#include <chrono>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/video/tracking.hpp>
using namespace std; 
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;
}

LK跟踪问题对比：

1. 我们会发现位于物体角点处的特征更加稳定。边缘处的特征会沿着边缘“滑动”，这主要是因为沿着边缘移动时特征块的内容基本不变，因此程序容易 认为是同一个地方。而既不在角点，也不在边缘的的特征点则会频繁跳动，位置非常不稳定。
2. 匹配描述子的方法在相机运动较大时仍能成功， 而光流必须要求相机运动是微小的。从这方面来说，光流的鲁棒性比描述子差一些。

总而言之，光流法可以加速基于特征点的视 觉里程计算法，避免计算和匹配描述子的过程，但要求相机运动较慢（或采集频率较高）。

---

8.4 直接法（Direct Methods）

8.4.1 直接法的推导

目标是求第一个相机到第二个相 机的相对位姿变换。我们以第一个相机为参照系，设第二个相机旋转和平移为 R, t（对应李代数为 ξ）。

特征点法中，由于我们通过匹配描述子，知道了 p1, p2 的像素位置，所以可以计 算重投影的位置。

但在直接法中，由于没有特征匹配，我们无从知道哪一个 p2 与 p1 对应 着同一个点。

直接法的思路是根据当前相机的位姿估计值，来寻找 p2 的位置（1）。但若相机位姿不够好，p2 的外观和 p1 会有明显差别。于是，为了减小这个差别，我们优化相机的位姿，来寻找与 p1 更相似的 p2（2）。这同样可以通过解一个优化问题，但此时最小化的不是重投影误差，而是光度误差，也就是 P 的两个像的亮度误差（该最小二乘，估计的目标变量是R，t，以及对应匹配像素点）：

仍是基于灰度不变假设，我们有许多个（比如 N 个）空间点 Pi，那么，整 个相机位姿估计问题变为：

                              

我们关心误差 e 是如何 随着相机位姿 ξ 变化的，需要分析它们的导数关系（g2o 中，要确定顶点和边的关系，需要用ξ来表示e）。因此，使用李代数上的扰动模型。我 们给 exp(ξ) 左乘一个小扰动 exp(δξ)

q 为 P 在扰动之后，位于第二个相机坐标系下的坐标，而 u 为它的像素坐标。利用一阶泰勒展开，有：

∂I2/∂u 为 u 处的像素梯度；

∂u/∂q 为投影方程关于相机坐标系下的三维点的导数。

∂q/∂δξ 为变换后的三维点对变换的导数，这在李代数章节已经介绍过了：

后两项合并

对于 N 个点的问题，我们可以用这种方法计算优化问题的雅可比，然后使用 G-N 或 L-M 计算增量，迭代求解。

---

8.4.2 直接法的讨论

在我们上面的推导中，P 是一个已知位置的空间点，它是怎么来的呢？在 RGB-D 相 机下，我们可以把任意像素反投影到三维空间，然后投影到下一个图像中。如果在单目相 机中，这件事情要更为困难。

1. P 来自于稀疏关键点，我们称之为稀疏直接法。，并且像 L-K 光流那样，假设它周围像素也是不变的。
2. P 来自部分像素。如果像素梯度为零，整一项雅可比就为零， 不会对计算运动增量有任何贡献。因此，可以考虑只使用带有梯度的像素点，舍弃像 素梯度不明显的地方。这称之为半稠密（Semi-Dense）的直接法
3. P 为所有像素，称为稠密直接法。稠密重构需要计算所有像素。需要 GPU 的加速。

---

8.5 实践：RGB-D 的直接法

本书不涉及 GPU 编程，稠密的直接 法就省略掉了。本节我们来考虑 RGB-D 上的稀疏直接法 VO。由于求解直接法最后等价于求解一个优化问题，因此我们可以使用 g2o 或 Ceres 这些 优化库帮助我们求解。

图优化：误差项为单个像素的光度误差。由于 g2o 中本身没有计算光度误差的边，我们需要 自己定义一种新的边。在上述的建模中，直接法图优化问题是由一个相机位姿顶点与许多条一元边组成的。

定义的g2o元素的类：

//模板参数里填入测量值的维度、类型，以及连接此边的顶点
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
};

直接法结合g2o进行线性优化的代码：

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 juzhen= "<<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();
}

在两个图像相差不多的时候，直接法会调整相机的位姿，使 得大部分像素都能够正确跟踪。但是，在稍长一点的时间内，比如说 0-9 帧之间的对比，我 们发现由于相机位姿估计不准确。

---

---

8.5.4 半稠密直接法

我们把先前的稀疏特征点改成了带有明显梯度的像素。于是在 图优化中会增加许多的边。对应的代码变化如下：

稀疏法

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 ) );
            }

 半稠密

 // 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 ) );
                }

可以看到参与估计的像素，像是固定在空间中一样。当相 机旋转时，它们的位置似乎没有发生变化。这代表了我们估计的相机运动是正确的。显然，当像素增多时，优化会更加费 时，所以为了实时性，需要考虑使用较好的像素点，或者降低图像的分辨率。

---

8.5.5 直接法的讨论

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

在非线性优化过程中，这个位姿是由一个初值不断地优化迭代得到的（可以大致赋值初值），我们的初值比较差。

直接法的梯度是直接由图像梯度确定的，因此我们必须保证沿着图像梯度走时，灰 度误差会不断下降。然而，图像通常是一个很强烈的非凸函数。实际当中， 如果我们沿着图像梯度前进，很容易由于图像本身的非凸性（或噪声）落进一个局部极小值中，无法继续优化。只有当相机运动很小，图像中的梯度不会有很强的非凸性时，直接 法才能成立。

---

8.5.6 直接法优缺点总结

优点

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

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

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

缺点

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

• 单个像素没有区分度。找一个和他像的实在太多了！——于是我们要么计算图像块， 要么计算复杂的相关性。由于每个像素对改变相机运动的“意见”不一致。只能少数 服从多数，以数量代替质量。

• 灰度值不变是很强的假设。