《视觉SLAM十四讲精品总结》9:直接法和光流法_svo前端 lk光流

挺好的讲解，首先看：

https://blog.csdn.net/weixin_38203573/article/details/79787499

https://blog.csdn.net/potxxx/article/details/86765115

 

 

内容：

二者关系

光流法

直接法

一、二者关系
引出原因：关键点和描述子计算非常耗时，可以保留特征点，使用光流法跟踪特征点运动。

关系：光流法描述像素在图像中运动，直接法利用相机运动模型计算特征点在下一时刻图像中位置。

使用条件：直接法利用图像的像素灰度信息计算相机运动，需要场景中存在明暗变化。

二、LK光流法

光流法常用来跟踪角点的运动。之后用跟踪的特征点，用ICP、PnP或对极几何估计相机运动。

适用场景：要求相机运动缓慢，采集频率高

调用函数calcOpticalFlowPyrLK

void cv::calcOpticalFlowPyrLK(InputArray prevImg,nextImg,prevPts,nextPts,
OutputArray status,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 
)    
 

 1、对第一帧提取FAST特征点存到keypoints中（list类型，之后还会删除）

2、对其他帧用LK跟踪特征点

3.  更新keypoints列表，从prev_keypoints到next_keypoints

 4. 画出 keypoints出为圆圈

int main(int argc, char** argv)
{
    string path_to_dataset = argv[1];
    string associate_file = path_to_dataset + "/associate.txt";
    //读入txt文本到fin中
    ifstream fin(associate_file);
 
    string rgb_file, depth_file, time_rgb, time_depth;
    // 因为要删除跟踪失败的点，keypoints使用list，元素类型是Point2f，坐标
    // 逐帧操作的将color承接为last_color，然后color读取新的图
    list< cv::Point2f > keypoints;      
    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);
        // 1. 对第一帧提取FAST特征点存到keypoints中
        if (index == 0)
        {
            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;
        }
        //实际只有10张图，后面10-99没有数据所以一直continue掉，index秒到99
        if (color.data == nullptr || depth.data == nullptr)
            continue;
        // 2. 对其他帧用LK跟踪特征点
        vector<cv::Point2f> next_keypoints;
        vector<cv::Point2f> prev_keypoints;
        for (auto kp : keypoints)
            prev_keypoints.push_back(kp);
        //调用calcOpticalFlowPyrLK函数
        //status匹配状态，匹配上赋1，否则赋0；
        vector<unsigned char> status;
        vector<float> error;
        cv::calcOpticalFlowPyrLK(last_color, color, prev_keypoints, next_keypoints, status, error);
 
        // 3. 更新keypoints列表，从prev_keypoints到next_keypoints
        int i = 0;
        for (auto iter = keypoints.begin(); iter != keypoints.end(); i++)
        {
            //跟丢了删除，iter保持当前位置，不会++，但i会++；
            //跟踪失败，next_keypoints中数据被跳过
            if (status[i] == 0)
            {
                iter = keypoints.erase(iter);
                continue;
            }
            //跟踪上的好点，才会让iter指向，
            *iter = next_keypoints[i];
            iter++;
        }
        cout << "tracked keypoints: " << keypoints.size() << endl;
        if (keypoints.size() == 0)
        {
            cout << "all keypoints are lost." << endl;
            break;
        }
        // 4. 画出 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;
}

从匹配信息associate.txt输入进去

fin >> time_rgb >> rgb_file >> time_depth >> depth_file;

13050353.359684 rgb/13050353.359684.png 1305031453.374112 depth/1305031453.374112.png

 

list类型的keypoints初始化第一帧检测角点后，将所有角点坐标存入。他的更新只有减少，没有增加(只有跟丢的点进行删除)

next_keypoints和prev_keypoints每帧跟踪循环内被定义的，也就是每次进循环被定义，出循环被释放.

list< Point2f > keypoints;
vector<cv::Point2f> next_keypoints;
vector<cv::Point2f> prev_keypoints;

重点注释：

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

iter是keypoints的迭代器，正常情况下更新keypoints为下一帧特征点坐标，直接*iter=next_keypoints[i]即可。i++,iter++二者同步更新；

特殊情况下，出现跟踪失败的特征点，在keypoints先把这个位置删除，同时.erase()迭代器返回的是下一个的位置。与此同时这里的continue能结束本次循环，重新下一次循环i++，这样可以跳过这个数值，下一次*iter=next_keypoints[i]时，iter已经指向了下一个位置，同时i++了，完美赋值；
 

三、直接法
1、随着一批不需提取特征的方法，如SVO（选取关键点来采用直接法，这类方法称为稀疏方法（sparse））；LSD（选取整幅图像中有梯度的部分来采用直接法，这种方法称为半稠密方法（simi-dense）），直接法渐露其自身优势。DSO（Direct Sparse Odometry，稀疏直接运动估计算法），DSO的前端和LSD-SLAM相似，后端则抛弃了图优化的框架。

2、直接法将数据关联（data association）与位姿估计（pose estimation）放在了一个统一的非线性优化问题中，最小化光度误差。而特征点法则分步求解，即，先通过匹配特征点求出数据之间关联，再根据关联来估计位姿。这两步通常是独立的，在第二步中，可以通过重投影误差来判断数据关联中的外点，也可以用于修正匹配结果。

 

代码详解

调用函数：1、cvtColor   将图像从一个颜色空间转换为另一个颜色空间。

void cv :: cvtColor（src，dst，INT 代码）
//实例
Mat color, gray;        
cvtColor ( color, gray, cv::COLOR_BGR2GRAY );

1、直接法位姿估计的边，构造图优化 EdgeSE3ProjectDirect

    g20中已有的节点和边

• 相机位姿顶点类VertexSE3Expmap（在这里直接使用）
• 3D路标点类VertexSBAPointXYZ
• 重投影误差边类EdgeProjectXYZ2UV

两个虚函数：computeError()计算误差，linearizeOplus()计算雅克比矩阵。

//自定义的光度误差 边EdgeSE3ProjectDirect，顶点为g2o中李代数位姿节点VertexSE3Expmap
class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap>
{
//变量声明，3D点、内参、灰度图像指针image
public:
    Eigen::Vector3d x_world_;  
    float cx_ = 0, cy_ = 0, fx_ = 0, fy_ = 0; 
    cv::Mat* image_ = nullptr;   
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 ){}
    //1. 光度误差计算，3D点投影到图像平面
    virtual void computeError()
    {
        //v是位姿pose,x_local是3D估计值
        const VertexSE3Expmap* v  =static_cast<const VertexSE3Expmap*> ( _vertices[0] );
        Eigen::Vector3d x_local = v->estimate().map ( x_world_ );
        //3D到2D像素点转换
        float x = x_local[0]*fx_/x_local[2] + cx_;
        float y = x_local[1]*fy_/x_local[2] + cy_;
        //检查x y是否出界，距离图像四条边4个像素大小内为有效区域。
        if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows )
        {
            _error ( 0,0 ) = 0.0;
            this->setLevel ( 1 );
        }
        else
        {
            //经过在灰度图中插值获取得的灰度值getPixelValue（x,y）减去测量值灰度值
            _error ( 0,0 ) = getPixelValue ( x,y ) - _measurement;
        }
    }
 
    // 2. 计算线性增量，雅可比矩阵J
    virtual void linearizeOplus( )
    {
        //先判断是否出界，重置
        if ( level() == 1 )
        {
            _jacobianOplusXi = Eigen::Matrix<double, 1, 6>::Zero();
            return;
        }
        //2.1 位姿估计，得到空间坐标系3D坐标
        VertexSE3Expmap* vtx = static_cast<VertexSE3Expmap*> ( _vertices[0] );
        Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ );   // q in book
        //2.2 3D转换为2D像素坐标
        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_;
        //2.3 像素对位姿偏导jacobian from se3 to u,v
        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_;
 
        //2.4 像素梯度部分偏导，求得差分
        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;
        //2.5 总的雅克比是二者相乘
        _jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;
    }
 
    virtual bool read ( std::istream& in ) {}
    virtual bool write ( std::ostream& out ) const {}
 
protected:
    //getPixelValue函数通过双线性差值获得浮点坐标对应插值后的像素值
    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]
               );
    }
 
};

2、直接法估计相机运动poseEstimationDirect（使用g2o）

bool poseEstimationDirect ( const vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw )
{
    // 1.初始化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 );
    // 2.添加顶点相机位姿李代数pose 
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
    pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) );
    pose->setId ( 0 );
    optimizer.addVertex ( pose );
 
    // 3. 添加边，光度误差，一帧图上很多像素代表很多一元边
    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
        );
        //这些边（误差），对应的顶点都是ID为0那一个pose顶点
        edge->setVertex ( 0, pose );
        //整个过程测量值只有第一帧的灰度值，后面的每一帧根据位姿找出像素点，再找到灰度值，都是估计值，
        edge->setMeasurement ( m.grayscale );
        //信息矩阵设置为单位阵，表征每个边的权重都一样
        edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() );
        //依次增加，给边设置一个ID
        edge->setId ( id++ );
        optimizer.addEdge ( edge );
    }
    cout<<"edges in graph: "<<optimizer.edges().size() <<endl;
    // 4. 开始优化
    optimizer.initializeOptimization();
    optimizer.optimize ( 30 );
    Tcw = pose->estimate();
}

3、坐标转换project2Dto3D和project3Dto2D

// 1. 一次测量的值，包括一个世界坐标系下3D点与一个灰度值gray
struct Measurement
{
    Measurement ( Eigen::Vector3d p, float g ) : pos_world ( p ), grayscale ( g ) {}//初始化构造函数
    Eigen::Vector3d pos_world;//声明成员函数
    float grayscale;
};
//2. 像素坐标到3D坐标，RGBD照片d单位毫米，空间点zz单位米，scale单位换算
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 );
}
//3. 3D到像素坐标
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 );
}

4、主框架

int main ( int argc, char** argv )
{
    //根据时间生成随机数
    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;
    //Measurement类存储世界坐标点（以第一帧为参考的FAST关键点）和对应的灰度图像（由color->gray）的灰度值
    vector<Measurement> measurements;
    // 相机内参K
    float cx = 325.5;
    float cy = 253.5;
    float fx = 518.0;
    float fy = 519.0;
    //RGBD相机单位毫米，3D点单位米
    float depth_scale = 1000.0;
    Eigen::Matrix3f K;
    K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f;
    // 位姿，变换矩阵T
    Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity();
 
    cv::Mat prev_color;
    // 我们以第一个图像为参考，对后续图像和参考图像做直接法
    //每一副图像都会与第一帧图像做直接法计算第一帧到当前帧的R,t。
    // 参考帧一直是第一帧，而不是循环流动当前帧的上一帧为参考帧
    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; 
        //转换后的灰度图为g2o优化需要的边提供灰度值
        cv::cvtColor ( color, gray, cv::COLOR_BGR2GRAY );
 
        // 1. 对第一帧提取FAST特征点
        //遍历第一帧特征点数组，筛选，把3D坐标和灰度值放到measurement中
        if ( index ==0 )
        {  
            vector<cv::KeyPoint> keypoints;
            cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
            detector->detect ( color, keypoints );
            for ( auto kp:keypoints )
            {
                // 1.1去掉邻近边缘处的点
                if ( kp.pt.x < 20 || kp.pt.y < 20 || ( kp.pt.x+20 ) >color.cols || ( kp.pt.y+20 ) >color.rows )
                    continue;
                // 1.2 深度图上关键点的深度值d   ，cvRound()返回整数
                ushort d = depth.ptr<ushort> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ];
                if ( d==0 )
                    continue;
                // 找到深度值d后，2D点投影到3D世界坐标系
                Eigen::Vector3d p3d = project2Dto3D ( kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale );
                //1.3 灰度图上关键点灰度值
                float grayscale = float ( gray.ptr<uchar> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ] );
                //得到三维3D坐标和对应的灰度值，作为测量值
                measurements.push_back ( Measurement ( p3d, grayscale ) );
            }
            //第一帧，赋值给pre_color，为什么用clone？？？
            prev_color = color.clone();
            continue;
        }
 
        // 2. 使用直接法计算相机运动T
        //measurements是不变的，之后不断读入的fray灰度图变化
        poseEstimationDirect ( measurements, &gray, K, Tcw );
        cout<<"Tcw="<<Tcw.matrix() <<endl;
 
        // 3.构建一张图画出后续的帧跟第一帧对比的效果
        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;
            //空间点3D坐标，及在第一帧的像素坐标
            Eigen::Vector3d p = m.pos_world;
            Eigen::Vector2d pixel_prev = project3Dto2D ( p ( 0,0 ), p ( 1,0 ), p ( 2,0 ), fx, fy, cx, cy );
            //坐标变换T后，
            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;
}