视觉SLAM笔记(44) RGB-D 的直接法


1. 稀疏直接法

现在演示如何使用稀疏的直接法
为了保持程序简单,使用 RGB-D 数据而非单目数据,这样可以省略掉单目的深度恢复部分
基于特征点的深度恢复已经在 视觉SLAM笔记(34) 三角测量 介绍过了
而基于块匹配的深度恢复将在后面介绍

所以这里考虑 RGB-D 上的稀疏直接法 VO(Visual Odometry,视觉里程计)
由于求解直接法最后等价于求解一个优化问题
因此可以使用 g2o 或 Ceres 这些优化库帮助我们求解
以 g2o 为例设计实验,在使用 g2o 之前,需要把直接法抽象成一个图优化问题

显然,直接法是由以下顶点和边组成的:

  1. 优化变量为一个相机位姿,因此需要一个位姿顶点
    由于在推导中使用了李代数,故程序中使用李代数表达的 SE(3) 位姿顶点
    视觉SLAM笔记(37) 求解 PnP 一样,将使用“VertexSE3Expmap”作为相机位姿
  2. 误差项为单个像素的光度误差
    由于整个优化过程中 II1(p(p1)) 保持不变,可以把它当成一个固定的预设值
    然后调整相机位姿,使 II2(p(p2)) 接近这个值
    于是,这种边只连接一个顶点,为 一元边
    由于 g2o 中本身没有计算光度误差的边,需要自己定义一种新的边

在上述的建模中,直接法图优化问题是由一个相机位姿顶点与许多条一元边组成的
如果使用稀疏的直接法,那大约会有几百至几千条这样的边;稠密直接法则会有几十万条边
优化问题对应的线性方程是计算李代数增量,本身规模不大(6×6)
所以主要的计算时间会花费在每条边的误差与雅可比的计算上

下面的实验中,先来定义一种用于直接法位姿估计的边
然后,使用该边构建图优化问题并求解之


2. 定义直接法的边

首先来定义计算光度误差的边
创建 VSLAM_note/044/direct_sparse.cpp 文件
按照 视觉SLAM笔记(43) 直接法 的推导,还需要给出它的雅可比矩阵:

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::BaseUnaryEdge
在继承时,需要在模板参数里填入测量值的维度、类型,以及连接此边的顶点
同时,把空间点 P、相机内参和图像存储在该边的成员变量中

为了让 g2o 优化该边对应的误差,需要覆写两个虚函数:

  • computeError() 计算误差值
  • linearizeOplus() 计算雅可比

可以看到,这里的雅可比计算 与 误差相对于李代数的雅可比矩阵是一致的:

在这里插入图片描述
注意在程序中的误差计算里,使用了 II2(p(p2)I) − I1(p(p1)) 的形式
因此前面的负号可以省去,只需把像素梯度乘以像素到李代数的梯度即可

在程序中,相机位姿是用浮点数表示的,投影到像素坐标也是浮点形式
为了更精细地计算像素亮度,要对图像进行插值
这里采用了简单的双线性插值,也可以使用更复杂的插值方式,但计算代价可能会变高一些


3. 使用直接法估计相机运动

定义了 g2o 边后,将节点和边组合成图,就可以调用 g2o 进行优化了
在这个实验中,读取数据集的 RGB-D 图像序列
图片依旧以 视觉SLAM笔记(42) 光流法跟踪特征点 中的TUM 公开数据集为例
以第一个图像为参考帧,然后用直接法求解后续图像的位姿
在参考帧中,对第一张图像提取 FAST 关键点(不需要描述子)
并使用直接法估计这些关键点在第二个图像中的位置
以及第二个图像的相机位姿
这就构成了一种简单的稀疏直接法
最后,画出这些关键点在第二个图像中的投影

$ ./direct_sparse ../../042/data/

程序会在作图之后暂停,可以看到特征点的位置关系,终端也会输出迭代误差的下降过程
在这里插入图片描述)
看到在两个图像相差不多的时候,直接法会调整相机的位姿,使得大部分像素都能够正确跟踪
但是,在稍长一点的时间内,比如说 0-9 帧之间的对比
发现由于相机位姿估计不准确,特征点出现了明显的偏移现象


4. 半稠密直接法

很容易就能把程序拓展成半稠密的直接法形式
对参考帧中,先提取梯度较明显的像素,然后用直接法,以这些像素为图优化边,来估计相机运动
direct_semidense.cpp对先前的程序做如下的修改:

// 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. 直接法的讨论

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

不妨设身处地地扮演一下优化算法
假设对于参考图像,测量到一个灰度值为 229 的像素
并且,由于知道它的深度,可以推断出空间点 PP 的位置
在这里插入图片描述
此时又得到了一张新的图像,需要估计它的相机位姿
这个位姿是由一个初值不断地优化迭代得到的
假设初值比较差,在这个初值下,空间点 P 投影后的像素灰度值是 126
于是,这个像素的误差为 229 − 126 = 103
为了减小这个误差,希望微调相机的位姿,使像素更亮一些

怎么知道往哪里微调,像素会更亮呢?
这就需要用到局部的像素梯度
在图像中发现,沿 uu 轴往前走一步,该处的灰度值变成了 123,即减去了 3
同样地,沿 v 轴往前走一步,灰度值减 18,变成 108
在这个像素周围,看到梯度是 [−3; −18]
为了提高亮度,会建议优化算法微调相机,使 PP 的像往左上方移动

在这个过程中,用像素的局部梯度近似了它附近的灰度分布
不过请注意真实图像并不是光滑的,所以这个梯度在远处就不成立了

但是,优化算法不能只听这个像素的一面之词,还需要听取其他像素的建议
综合听取了许多像素的意见之后,优化算法选择了一个和建议的方向偏离不远的地方
计算出一个更新量 exp(ξexp(ξ^))
加上更新量后,图像从 II2 移动到了 II2′,像素的投影位置也变到了一个更亮的地方
看到,通过这次更新, 误差变小了

在理想情况下,期望误差会不断下降,最后收敛

但是实际是不是这样呢?
是否真的只要沿着梯度方向走,就能走到一个最优值?
注意到,直接法的梯度是直接由图像梯度确定的
因此必须保证沿着图像梯度走时,灰度误差会不断下降

然而,图像通常是一个很强烈的非凸函数
在这里插入图片描述
实际当中,如果沿着图像梯度前进,很容易由于图像本身的非凸性(或噪声)落进一个局部极小值中,无法继续优化
只有当相机运动很小,图像中的梯度不会有很强的非凸性时,直接法才能成立

在例程中,只计算了单个像素的差异,并且这个差异是由灰度直接相减得到的
然而,单个像素没有什么区分性,周围很可能有好多像素和它的亮度差不多
所以,有时会使用小的图像块(patch),并且使用更复杂的差异度量方式
例如归一化相关性(Normalized Cross Correlation, NCC)等
而例程为了简单起见,使用了误差的平方和,以保持和推导的一致性


参考:

《视觉SLAM十四讲》


相关推荐:

视觉SLAM笔记(43) 直接法
视觉SLAM笔记(42) 光流法跟踪特征点
视觉SLAM笔记(41) 光流
视觉SLAM笔记(40) 特征点的缺陷
视觉SLAM笔记(39) 求解 ICP


发布了217 篇原创文章 · 获赞 292 · 访问量 289万+

没有更多推荐了,返回首页

©️2019 CSDN 皮肤主题: Age of Ai 设计师: meimeiellie

分享到微信朋友圈

×

扫一扫,手机浏览