SLAM十四讲：直接法(8)

最新推荐文章于 2023-07-12 11:14:13 发布

VIP文章 KirutoCode

最新推荐文章于 2023-07-12 11:14:13 发布

阅读量708

点赞数

分类专栏： SLAM十四讲

本文链接：https://blog.csdn.net/McEason/article/details/103199100

版权

文章目录

RGBD稀疏直接法（g2o）
main函数中的操作
半稠密直接

内容总览：一些直接法的代码

RGBD稀疏直接法（g2o）

简述:
假设周围像素不变，不必计算描述子，只使用数百个像素

只能计算稀疏结构

参数：
输入：测量值（空间点的灰度），新的灰度图，相机内参

输出相机位姿

计算误差:

virtual void computeError()
    {
   
        const VertexSE3Expmap* v  =static_cast<const VertexSE3Expmap*> ( _vertices[0] );
        Eigen::Vector3d x_local = v->estimate().map ( x_world_ );        //将世界坐标映射到了图像上，local代表图像局部
        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;        //计算误差，计算值减真实值？
        }
    }

将I1的点通过estimate().map映射到另一个图像上，然后用像素公式计算，误差为I2-I1

计算雅可比J：

    virtual void linearizeOplus( )        //计算呀可比矩阵
    {
   
        if ( level() == 1 )    //范围不对，或者第一次的时候，初始化1*6的0矩阵
        {
   
            _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