SlamBook14讲-LKFlow与DirectMethod（直接法）

本文链接：https://blog.youkuaiyun.com/jiahao_1002/article/details/128736856

《视觉SLAM十四讲》的第八讲-视觉里程计2 整个代码写了两三天

本章大部分讨论的内容在于用一种简单快速的方法获得图像中的特征点并计算相机位姿

在之前的方法中，需要逐帧计算图像的描述子，耗费了大量的时间，难以应用至SLAM

光流法和直接法是通过比对前后两帧图像的关系，从而获得特征点的方式，它的优点在于快速

光流法直接调用OpenCV的库，如果自己想实现比较困难，只懂了整体思路

LKFLow:

主要的函数就是

cv::calcOpticalFlowPyrLK(PrevImg, ColorImg, PrevKeyPoints, NextKeyPoints, status, err) ;

函数原型为：

CV_EXPORTS_W void calcOpticalFlowPyrLK( InputArray prevImg, InputArray nextImg,
                                        InputArray prevPts, InputOutputArray nextPts,
                                        OutputArray status, OutputArray err,
                                        Size winSize = Size(21,21), int maxLevel = 3,
                                        TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 0.01),
                                        int flags = 0, double minEigThreshold = 1e-4 );

参数列表依次为：上一帧图像、下一帧图像、上一帧关键点（已知）、下一帧关键点（待求）、关键点状态、报错

因此只需要开始计算的第一帧图像的关键点计算出来，后面的关键点就可以不断迭代计算。因此调用光流法之前，需要先利用之前学习的任意一种关键点提取方法，做至少一次关键点提取。

在计算出下一帧的关键点后，两帧的关键点数量会有所改变，首先应该剔除太过靠近图像边缘的关键点，它们的参考价值很低，容易引进误差。同时，因为相机的位姿变化，可能会有上一帧的关键点在移动到了图像之外，消失在了第二帧图像中。因此最好设计一个动态数组，用于实时修改关键点列表，这里高翔博士用的是std::list

            int j = 0;
            for (auto i = KeyPointsList.begin(); i != KeyPointsList.end(); i++,j++){
                if(status[j] == 0) {
                    i = KeyPointsList.erase(i);
                    continue;
                }
                else{
                    *i = NextKeyPoints[j];
                }
            }

这里注意一个删除list元素的操作:(其中i是STL中的迭代器)

i = List.erase(i);

整体思路很简单，最好能够自己完成一遍光流法算法的实现。

光流法程序结果：(以第二帧为例)

DirectMethod:

直接法的思路与光流法类似，区别在于计算出第一帧图像之后，通过最优化的方法，计算出一个相机的位姿，满足光度误差最小。其中光度误差可以理解为图像上某个像素的灰度值。

其中直接法也分为两种：Sparse与Semidense（稀疏法与半稠密法）

在编写代码的开始过程中，有个弯没绕过来。与最小化重投影误差不同，在之前的方法中，是可以直接获得两帧图像的关键点和他们的匹配关系的，但直接法并不知道第二帧图像的关键点及匹配信息，因此需要多一步计算上一帧图像的关键点经过内参外参的坐标变换后，在第二帧图像同样的位置上的光度（灰度）。后者与上一帧关键点处的灰度做差，得到灰度差，也就是优化的目标函数，为一维标量。

这个地方感觉书中没有讲的很清楚，也稍微吐槽一下，高博士越往后的课程讲解越敷衍了。

除此之外，这里的代码Measurement均用的 $I_{1}$ ，而不是不断更新的。如果想不断迭代着计算，需要对代码进行修改。

图优化的部分与第一帧提取关键点的地方不再追述了

重点在于自定义边的书写

主要需要重写的函数如下：

    virtual void computeError(){
        g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap*>(_vertices[0]);
        Eigen::Vector3d PointXYZTrans = pose->estimate().map(_PointXYZ);
        Eigen::Vector2d PointUV = Point3Dto2D(PointXYZTrans[0], PointXYZTrans[1], PointXYZTrans[2], 519.0, 518.0, 325.5, 253.5);
        _error(0,0) = PhotometricI2(PointUV) - _measurement;
    }

这里的error前部分是第一帧的关键点在经过坐标变换（待求量）在第二帧图像上的光度。

后部分将第一帧的图像关键点的光度作为_measurement，在实现中会有setMeasurement.

    double PhotometricI2(Eigen::Vector2d I2){
        return _Img->ptr<uchar>(int(I2[1]))[int(I2[0])];
    }
    double PhotometricI2_Book(Eigen::Vector2d I2){
        double y = I2[1];
        double x = I2[0];
        uchar* data = & _Img->data[ int ( (y) ) * _Img->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[ _Img->step ] +
                   xx*yy*data[_Img->step+1]
               );
    }

上面的代码是计算其他图像（除第一帧）任意点光度值，第一个是我自己写的，第二个是书中写的。书中使用了二次插值来更精确计算灰度，博主亲自测试后感觉区别不大。

    virtual void linearizeOplus(){
        g2o::VertexSE3Expmap* vtx = static_cast<g2o::VertexSE3Expmap*> ( _vertices[0] );
        Eigen::Vector3d PointXYZTrans = vtx->estimate().map ( _PointXYZ );   // q in book

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

        float u = x* 518.0 * invz + 325.5;
        float v = y* 519.0 * invz + 253.5;

        // 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 *518.0;
        jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *518.0;
        jacobian_uv_ksai ( 0,2 ) = - y*invz *518.0;
        jacobian_uv_ksai ( 0,3 ) = invz *518.0;
        jacobian_uv_ksai ( 0,4 ) = 0;
        jacobian_uv_ksai ( 0,5 ) = -x*invz_2 *518.0;

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

        Eigen::Matrix<double, 1, 2> jacobian_pixel_uv;
        jacobian_pixel_uv(0,0) = (PhotometricI2(Eigen::Vector2d(u+1,v))-PhotometricI2(Eigen::Vector2d(u-1,v)))/2;
        jacobian_pixel_uv(0,1) = (PhotometricI2(Eigen::Vector2d(u,v+1))-PhotometricI2(Eigen::Vector2d(u,v-1)))/2;

        _jacobianOplusXi = jacobian_pixel_uv * jacobian_uv_ksai;
    }

由于是对位姿求导，因此需要定义求导方法雅可比矩阵_jacobianOplusXi

其中 $\frac{\partial I_{2}}{\partial u}$ 是直接分别利用的x,y不同方向上前一个像素与后一个像素的光度之差进行计算的。

这里的雅可比矩阵直接复制的示例代码，没有太多技术含量。

tips:

1.关于变换矩阵，可以使用Eigen自带的库中的

Eigen::Isometry3d T

它可以直接左乘3*1的列向量，或者可以通过T.matrix()来直接获得齐次坐标形式，很好用

同时可以使用T.rotation()与T.translation()获得旋转量与平移量。

2.在调试过程中遇到了几种报错与问题：

error: (-215:Assertion failed) channels() == CV_MAT_CN(dtype) in function 'copyTo'

主要是因此在使用cv::toCopy()时，对象已经因为循环结束释放掉了，把图像定义在循环之外即可

error: static assertion failed: YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES
  833 |   EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(ActualDstTypeCleaned,Src)

这往往是因此Eigen计算过程中某个矩阵计算出现了维数错误的情况。

3.在调试过程中，博主发现直接法对于优化过程中的初始值要求很高，如果在pose->setEstimate时设置一个随机矩阵作为初始迭代值，往往会出现位姿估计严重偏差，这大概是因为在迭代过程中，卡在了某个局部最小值处，从而未能找到正确的相机位姿。因此，将初始迭代值设置为Eigen::Isometry::Identity()，即没有旋转与平移时的位姿开始迭代效果是最好的。（原理也很好理解，因为本身运动过程就是一个从不动的默认位姿开始的）

直接法程序结果：（以第一帧与第三帧为例）

左边是计算出的相机位姿，右边是利用cv::circle和cv::line绘制的关键点与匹配线

程序源码：

LKFLOW：

#include <iostream>
#include <fstream>
#include <list>
#include <vector>

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


void DrawCircle(const cv::Mat& , const std::list<cv::Point2f>& , int);

int main(){
    std::string path = "../data/associate.txt";
    std::ifstream fin(path);
    std::vector<cv::KeyPoint> KeyPointsVec;
    std::list<cv::Point2f> KeyPointsList;
    cv::Mat PrevImg;
    if(!fin) return 1;
    std::string ColorNum, ColorPath, DepthNum, DepthPath;

    for (int i = 0; i<9; i++){
        fin >> ColorNum >> ColorPath >> DepthNum >> DepthPath;

        cv::Mat ColorImg = cv::imread( "../data/" + ColorPath );
        //std::cout << ColorPath << std::endl;
        //cv::Mat DepthImg = cv::imread( "../data/" + DepthPath );

        if(i == 0){

            cv::Ptr<cv::FastFeatureDetector> Detector = cv::FastFeatureDetector::create();
            Detector-> detect(ColorImg, KeyPointsVec);
            for (auto k:KeyPointsVec){
                KeyPointsList.push_back(k.pt); 
            } 
            //DrawCircle(ColorImg, KeyPointsList);       
            PrevImg = ColorImg;
            continue;
        }
        else{
            std::vector<cv::Point2f> PrevKeyPoints;
            std::vector<cv::Point2f> NextKeyPoints;
            for(auto i:KeyPointsList) PrevKeyPoints.push_back(i);
            std::vector<unsigned char> status;
            std::vector<float> err;
            cv::calcOpticalFlowPyrLK(PrevImg, ColorImg, PrevKeyPoints, NextKeyPoints, status, err) ;
            DrawCircle(PrevImg, KeyPointsList, i);
            int j = 0;
            for (auto i = KeyPointsList.begin(); i != KeyPointsList.end(); i++,j++){
                if(status[j] == 0) {
                    i = KeyPointsList.erase(i);
                    continue;
                }
                else{
                    *i = NextKeyPoints[j];
                }
            }
        }

        PrevImg = ColorImg;
    }
    
    return 0;
}

void DrawCircle(const cv::Mat& Img, const std::list<cv::Point2f>& Point, int name){
    cv::Mat ImgShow = Img.clone();
    for(auto i:Point) cv::circle(ImgShow, i, 1, cv::Scalar(0, 0, 255), 2);
    cv::imshow(std::to_string(name), ImgShow);
    cv::waitKey(0);
}

直接法：

#include <iostream>
#include <fstream>

#include <opencv2//core.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

#include <eigen3/Eigen/Core>

#include <g2o/core/base_unary_edge.h>
#include <g2o/types/sba/vertex_se3_expmap.h>
#include <g2o/core/block_solver.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/optimization_algorithm_levenberg.h>

//select DirectMethodMode
#define Method "Sparse"
// #define Method "Semidense"

double PhotometricI2(Eigen::Vector2d );

class PhotometricI1{
public:
    PhotometricI1(Eigen::Vector3d PointXYZ, double Gray):_PointXYZ(PointXYZ), _Gray(Gray){}

    Eigen::Vector3d _PointXYZ;
    double _Gray;
};

inline Eigen::Vector3d Point2Dto3D ( 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 Point3Dto2D ( 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 );
}

class UnaryEdge : public g2o::BaseUnaryEdge<1, double, g2o::VertexSE3Expmap>{  
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW 

    UnaryEdge(Eigen::Vector3d PointXYZ, cv::Mat* Img):_PointXYZ(PointXYZ), _Img(Img){}

    virtual void computeError(){
        g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap*>(_vertices[0]);
        Eigen::Vector3d PointXYZTrans = pose->estimate().map(_PointXYZ);
        Eigen::Vector2d PointUV = Point3Dto2D(PointXYZTrans[0], PointXYZTrans[1], PointXYZTrans[2], 519.0, 518.0, 325.5, 253.5);
        _error(0,0) = PhotometricI2(PointUV) - _measurement;
    }

    virtual void linearizeOplus(){
        g2o::VertexSE3Expmap* vtx = static_cast<g2o::VertexSE3Expmap*> ( _vertices[0] );
        Eigen::Vector3d PointXYZTrans = vtx->estimate().map ( _PointXYZ );   // q in book

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

        float u = x* 518.0 * invz + 325.5;
        float v = y* 519.0 * invz + 253.5;

        // 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 *518.0;
        jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *518.0;
        jacobian_uv_ksai ( 0,2 ) = - y*invz *518.0;
        jacobian_uv_ksai ( 0,3 ) = invz *518.0;
        jacobian_uv_ksai ( 0,4 ) = 0;
        jacobian_uv_ksai ( 0,5 ) = -x*invz_2 *518.0;

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

        Eigen::Matrix<double, 1, 2> jacobian_pixel_uv;
        jacobian_pixel_uv(0,0) = (PhotometricI2(Eigen::Vector2d(u+1,v))-PhotometricI2(Eigen::Vector2d(u-1,v)))/2;
        jacobian_pixel_uv(0,1) = (PhotometricI2(Eigen::Vector2d(u,v+1))-PhotometricI2(Eigen::Vector2d(u,v-1)))/2;

        _jacobianOplusXi = jacobian_pixel_uv * jacobian_uv_ksai;
    }

    virtual bool read ( std::istream& in ) {}
    virtual bool write ( std::ostream& out ) const {}

protected:
    double PhotometricI2(Eigen::Vector2d I2){
        return _Img->ptr<uchar>(int(I2[1]))[int(I2[0])];
    }
    double PhotometricI2_Book(Eigen::Vector2d I2){
        double y = I2[1];
        double x = I2[0];
        uchar* data = & _Img->data[ int ( (y) ) * _Img->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[ _Img->step ] +
                   xx*yy*data[_Img->step+1]
               );
    }


private:
    Eigen::Vector3d _PointXYZ;
    cv::Mat * _Img = nullptr;
};

void PoseEstimate(const std::vector<PhotometricI1>& , cv::Mat* , Eigen::Isometry3d&);


int main(){

    float cx = 325.5;
    float cy = 253.5;
    float fx = 518.0;
    float fy = 519.0;
    float DepthScale = 1000.0;
    Eigen::Matrix3f K;
    K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f;

    std::string AssociatePath = "../data/associate.txt";
    std::ifstream fin(AssociatePath);
    std::string ColorPath, ColorImgName, DepthPath, DepthImgName;
    std::vector<cv::KeyPoint> KeyPoints;
    std::vector<PhotometricI1> I1;
    cv::Mat FirstImg;

    for(int i = 0; i < 9; i++){
        std::cout << "*****************************" << i+1 << "*****************************" << std::endl;
        fin >> ColorPath >> ColorImgName >> DepthPath >> DepthImgName;
        cv::Mat ColorImg = cv::imread( "../data/" + ColorImgName);
        cv::Mat GrayImg;
        cv::cvtColor(ColorImg, GrayImg, cv::COLOR_BGR2GRAY);
        cv::Mat DepthImg = cv::imread( "../data/" + DepthImgName, -1);
        

        if(i == 0 && Method == "Sparse"){
            cv::Ptr<cv::FastFeatureDetector> Dector = cv::FastFeatureDetector::create();
            Dector->detect(ColorImg,KeyPoints);
            for(auto p:KeyPoints){
                if(p.pt.x < 20 || p.pt.y <20 || p.pt.x + 20 > ColorImg.cols || p.pt.y + 20 > ColorImg.rows) continue;
                unsigned short d = DepthImg.ptr<unsigned short>(int(p.pt.y))[int(p.pt.x)];
                if(d == 0) continue;
                I1.push_back(PhotometricI1(Point2Dto3D(p.pt.x, p.pt.y, d, fx, fy, cx, cy, DepthScale), GrayImg.ptr<uchar>(int(p.pt.y))[int(p.pt.x)]));
                
            }
            FirstImg = ColorImg.clone();
            continue;
        }

        else if (i == 0 && Method == "Semidense"){
            for ( int x=10; x<GrayImg.cols-10; x++ )
                for ( int y=10; y<GrayImg.rows-10; y++ )
                {
                    Eigen::Vector2d delta (
                        GrayImg.ptr<uchar>(y)[x+1] - GrayImg.ptr<uchar>(y)[x-1], 
                        GrayImg.ptr<uchar>(y+1)[x] - GrayImg.ptr<uchar>(y-1)[x]
                    );
                    if ( delta.norm() < 50 )
                        continue;
                    ushort d = DepthImg.ptr<ushort> (y)[x];
                    if ( d==0 )
                        continue;
                    Eigen::Vector3d p3d = Point2Dto3D ( x, y, d, fx, fy, cx, cy, DepthScale );
                    float grayscale = float ( GrayImg.ptr<uchar> (y) [x] );
                    I1.push_back ( PhotometricI1 ( p3d, grayscale ) );
                }
            FirstImg = ColorImg.clone();
            continue;
        }
        
        Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
        PoseEstimate(I1, &GrayImg, T);
        std::cout << T.matrix() << std::endl; 
        //std::cout << I1[i]._PointXYZ << std::endl;
        cv::Mat img_show ( ColorImg.rows*2, ColorImg.cols, CV_8UC3 );
        FirstImg.copyTo ( img_show ( cv::Rect ( 0, 0, ColorImg.cols, ColorImg.rows ) ) );
        ColorImg.copyTo ( img_show ( cv::Rect ( 0, ColorImg.rows, ColorImg.cols, ColorImg.rows ) ) );


        for ( auto m: I1 )
        {
            if ( rand() > RAND_MAX/5 )
                continue;
            Eigen::Vector3d p = m._PointXYZ;
            Eigen::Vector2d pixel_prev = Point3Dto2D ( p ( 0,0 ), p ( 1,0 ), p ( 2,0 ), fx, fy, cx, cy );
            Eigen::Vector3d p2 = T * m._PointXYZ;
            Eigen::Vector2d pixel_now = Point3Dto2D ( p2 ( 0,0 ), p2 ( 1,0 ), p2 ( 2,0 ), fx, fy, cx, cy );
            
            if ( pixel_now(0,0)<0 || pixel_now(0,0)>=ColorImg.cols || pixel_now(1,0)<0 || pixel_now(1,0)>=ColorImg.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 ) ), 2, cv::Scalar ( b,g,r ), 4 );
            cv::circle ( img_show, cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) + ColorImg.rows ), 2, cv::Scalar ( b,g,r ), 4 );
            cv::line ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +ColorImg.rows ), cv::Scalar ( b,g,r ), 1 );
        }
        cv::imshow ( "result", img_show );
        cv::waitKey ( 0 );
    }


    return 0;
}

void PoseEstimate(const std::vector<PhotometricI1>& I1, cv::Mat* Img, Eigen::Isometry3d& T){

    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 1>> Block;
    Block::LinearSolverType* LinearSolverPtr = new g2o::LinearSolverDense<Block::PoseMatrixType>();
    Block* SolverPtr = new Block(std::unique_ptr<Block::LinearSolverType> (LinearSolverPtr));
    g2o::OptimizationAlgorithmLevenberg* AlgorithmPtr = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<g2o::Solver> (SolverPtr));
    
    g2o::SparseOptimizer* Optimizer = new g2o::SparseOptimizer();
    Optimizer->setAlgorithm(AlgorithmPtr);

    g2o::VertexSE3Expmap* Pose = new g2o::VertexSE3Expmap();
    //g2o::SE3Quat et(T.rotation(), T.translation());
    g2o::SE3Quat et(T.rotation(), T.translation());
    Pose->setEstimate(et);
    Pose->setId(0);
    Optimizer->addVertex(Pose);

    int j = 0;
    for(auto i:I1){
        UnaryEdge* Edge = new UnaryEdge(i._PointXYZ, Img);
        Edge->setId(j);
        Edge->setVertex(0, Pose);
        Edge->setMeasurement(i._Gray);
        Edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity());
        Optimizer->addEdge(Edge);
        j++;
    }

    Optimizer->initializeOptimization();
    Optimizer->setVerbose(1);
    Optimizer->optimize(30);

    T = Pose->estimate() ;
}