ALOAM:Ceres 优化部分及代码解析

月照银海似蛟龙

分类：建图导航

个人专栏：3d 激光slam

发布时间 2022.07.13阅读数 4450 评论数 0

前言

Ceres solver 是谷歌开发的一款用于非线性优化的库，在谷歌的开源激光雷达slam项目cartographer中被大量使用。

Ceres可以解决边界约束鲁棒非线性最小二乘法优化问题。这个概念可以用以下表达式表示：
在这里插入图片描述
这一表达式在工程和科学领域有非常广泛的应用。比如统计学中的曲线拟合，或者在计算机视觉中依据图像进行三维模型的构建等等。

ALOAM里面的优化问题的建模和求解都是通过Ceres 进行的,包括前端的帧间里程计和后端的地图优化.

相比于其它优化库(g20 gtsam),Ceres 一个很大的优点就是自动求导功能

通常一个优化器会帮助解决优化问题中大部分内容,但是残差的计算以及残差对优化变量的雅克比矩阵通常需要用户自己定义,而雅克比矩阵通常比较复杂,因此有的优化库如G2O,GTSAM会预先定义好一些常见的优化问题,所涉及的残差及雅克比计算方式,但是并不能覆盖所有场景,一旦有些问题不是优化器事先建模好的问题,那就需要用户自己去定义残差和雅克比矩阵的计算方式,这个会非常麻烦,而且容易出错.ceres通过引用自动求导的功能,无论什么优化问题,用户只需要定义残差的计算方式,自动求导功能会帮助用户计算对应的雅克比矩阵来实现优化问题的求解.

所以在研究一个Ceres的优化是如何使用的,重点就是看自动求导这部分.

Ceres的自动求导使用流程:

自定义一个模板类,或者结构体两者差不多.
重载()运算符
()运算符函数前几个参数是参数块的起始指针
()运算符函数最后一个参数是残差模块的指针
()运算符负责计算残差

ALOAM的自动求导部分

ALOAM的自动求导部分在src文件夹下的lidarFactor.hpp文件中

边缘约束

代码分析

边缘约束自定义的模板类是

struct LidarEdgeFactor//边缘约束

ALOAM是用结构体自定义的

    LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_,
                    Eigen::Vector3d last_point_b_, double s_)
        : curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {}

4个成员变量,用来在生成结构体的时候初始化后面的重载函数里面的变量

curr_point 当前点
last_point_a上一帧的点a
last_point_b上一帧的点b
s当前点的时间戳占比

然后我们看重点的重载()运算符函数

    template <typename T>
    bool operator()(const T *q, const T *t, T *residual) const
    {

正如前面说的前几个参数是参数块的起始指针
q是四元数参数块
t是平移向量的参数块
最后一个参数是残差模块的指针
residual就是残差模块

然后继续看里面的内容:

        // 将double数据转成eigen的数据结构,注意这里必须都写成模板
        Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
        Eigen::Matrix<T, 3, 1> lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};
        Eigen::Matrix<T, 3, 1> lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};

将double数据转成eigen的数据结构,注意这里必须都写成模板

        Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};
        Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};

构建当前帧到上一帧的旋转四元数和一个单位四元数

        q_last_curr = q_identity.slerp(T(s), q_last_curr);
        Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};

考虑匀速模型(这一帧所有的旋转和平移都是一样的),根据当前点的时间戳占比,来计算,这个点到上一帧的旋转和平移

        Eigen::Matrix<T, 3, 1> lp;
        // 把当前点根据当前计算的帧间位姿变换到上一帧
        lp = q_last_curr * cp + t_last_curr;

把当前点根据当前计算的帧间位姿变换到上一帧

        Eigen::Matrix<T, 3, 1> nu = (lp - lpa).cross(lp - lpb);//模是三角形面积
        Eigen::Matrix<T, 3, 1> de = lpa - lpb;

        //残差的模是该点到底边的垂线长度
        residual[0] = nu.x() / de.norm();
        residual[1] = nu.y() / de.norm();
        residual[2] = nu.z() / de.norm();

根据向量的运算,把残差设计成点到第边的垂线长度.
残差越小说明点里线的距离越近,

所以该优化会不断的迭代,找到一个合适的q和t,使得点到线的距离最小.

        return true;
    }

最后返回true

    static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_,
                                       const Eigen::Vector3d last_point_b_, const double s_)
    {
        return (new ceres::AutoDiffCostFunction<
                LidarEdgeFactor, 3, 4, 3>(
            new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_)));
    }

最后使用一个函数,来返回一个new AutoDiffCostFunction

<lidaredgefactor, 3,="" 4,="" 3="">这里的第1个3是指的三维残差,然后的4是四元数的参数块维度,之后的3是平移向量的参数块维度.</lidaredgefactor,>

整体代码

struct LidarEdgeFactor//边缘约束
{
    LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_,
                    Eigen::Vector3d last_point_b_, double s_)
        : curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {}

    template <typename T>
    bool operator()(const T *q, const T *t, T *residual) const
    {
        // 将double数据转成eigen的数据结构,注意这里必须都写成模板
        Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
        Eigen::Matrix<T, 3, 1> lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};
        Eigen::Matrix<T, 3, 1> lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};

        //Eigen::Quaternion<T> q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};
        Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};//当前帧到上一帧的旋转四元数
        Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};//单位四元数

        //计算的是上一帧到当前帧的位姿变换 , 因此根据匀速模型,计算该点对应的位姿
        //这里暂不考虑畸变,因此这里不做任何变换
        //考虑匀速模型(这一帧所有的旋转和平移都是一样的),根据当前点的时间戳占比,来计算,这个点到上一帧的旋转和平移
        q_last_curr = q_identity.slerp(T(s), q_last_curr);
        Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};

        Eigen::Matrix<T, 3, 1> lp;//当前帧的点转到上一帧
        // 把当前点根据当前计算的帧间位姿变换到上一帧
        lp = q_last_curr * cp + t_last_curr;

        Eigen::Matrix<T, 3, 1> nu = (lp - lpa).cross(lp - lpb);//模是三角形面积
        Eigen::Matrix<T, 3, 1> de = lpa - lpb;

        //残差的模是该点到底边的垂线长度
        residual[0] = nu.x() / de.norm();
        residual[1] = nu.y() / de.norm();
        residual[2] = nu.z() / de.norm();

        return true;
    }

    static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_,
                                       const Eigen::Vector3d last_point_b_, const double s_)
    {
        return (new ceres::AutoDiffCostFunction<
                LidarEdgeFactor, 3, 4, 3>(
            new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_)));
    }

    Eigen::Vector3d curr_point, last_point_a, last_point_b;
    double s;
};

平面约束

代码分析

平面约束自定义的模板类是

struct LidarPlaneFactor//平面约束

构造函数是:

    LidarPlaneFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_,
                     Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_)
        : curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_),
          last_point_m(last_point_m_), s(s_)
    {
        ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m);
        ljm_norm.normalize();
    }

4个初始化参数

curr_point 当前点
last_point_j 上帧j点
last_point_l 上帧l点
last_point_m 上帧m点
s 当前点的时间戳占比

    LidarPlaneFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_,
                     Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_)
        : curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_),
          last_point_m(last_point_m_), s(s_)
    {
        ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m);//计算上帧三个点构成平面的法向量
        ljm_norm.normalize();//法向量单位化
    }

然后在构造函数里面进行了上帧三个点构成平面的法向量计算与单位化

然后我们看重点的重载()运算符函数

    bool operator()(const T *q, const T *t, T *residual) const
    {

和边缘约束一样

q是四元数参数块
t是平移向量的参数块
最后一个参数是残差模块的指针
residual就是残差模块

然后继续看里面的内容:

        Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
        Eigen::Matrix<T, 3, 1> lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())};
        Eigen::Matrix<T, 3, 1> ljm{T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())};

将电云的数据转成eigen的数据结构,注意这里必须都写成模板
转了当前点,上帧j点 ,和构造函数里面求的法向量

        Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};//当前帧到上一帧的旋转四元数
        Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};//单位四元数

用参数块声明当前帧到上一帧的旋转四元数

        q_last_curr = q_identity.slerp(T(s), q_last_curr);
        Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};

根据s当前点的时间戳占比
考虑匀速模型(这一帧所有的旋转和平移都是一样的),根据当前点的时间戳占比,来计算,这个点到上一帧的旋转和平移

        Eigen::Matrix<T, 3, 1> lp;//当前点转到上一帧的点声明
        lp = q_last_curr * cp + t_last_curr;//当前点转到上一帧

当前点转到上一帧

residual[0] = (lp - lpj).dot(ljm);

点lp和点lpj构成的向量在法向量的投影,就是lp到平面的距离
最后返回true

        return true;
    }

    static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_j_,
                                       const Eigen::Vector3d last_point_l_, const Eigen::Vector3d last_point_m_,
                                       const double s_)
    {
        return (new ceres::AutoDiffCostFunction<
                LidarPlaneFactor, 1, 4, 3>(
            new LidarPlaneFactor(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_)));
    }

最后使用一个函数,来返回一个new AutoDiffCostFunction

<lidaredgefactor,1, 4,="" 3="">这里的第1个1是指的一维残差,然后的4是四元数的参数块维度,之后的3是平移向量的参数块维度.</lidaredgefactor,1,>

整体代码

struct LidarPlaneFactor//平面约束
{
    LidarPlaneFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_,
                     Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_)
        : curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_),
          last_point_m(last_point_m_), s(s_)
    {
        ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m);//计算上帧三个点的法向量
        ljm_norm.normalize();//法向量单位化
    }

    template <typename T>
    bool operator()(const T *q, const T *t, T *residual) const
    {
        //将double数据转成eigen的数据结构,注意这里必须都写成模板
        Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};//当前点
        Eigen::Matrix<T, 3, 1> lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())};//上帧j点
        //Eigen::Matrix<T, 3, 1> lpl{T(last_point_l.x()), T(last_point_l.y()), T(last_point_l.z())};
        //Eigen::Matrix<T, 3, 1> lpm{T(last_point_m.x()), T(last_point_m.y()), T(last_point_m.z())};
        Eigen::Matrix<T, 3, 1> ljm{T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())};//构造里面求的法向量

        //Eigen::Quaternion<T> q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};
        Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};//当前帧到上一帧的旋转四元数
        Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};//单位四元数

        //计算的是上一帧到当前帧的位姿变换 , 因此根据匀速模型,计算该点对应的位姿
        //这里暂不考虑畸变,因此这里不做任何变换
        //考虑匀速模型(这一帧所有的旋转和平移都是一样的),根据当前点的时间戳占比,来计算,这个点到上一帧的旋转和平移        
        q_last_curr = q_identity.slerp(T(s), q_last_curr);
        Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};


        Eigen::Matrix<T, 3, 1> lp;//当前点转到上一帧的点声明
        lp = q_last_curr * cp + t_last_curr;//当前点转到上一帧

        residual[0] = (lp - lpj).dot(ljm);//点lp和点lpj构成的向量在法向量的投影,就是lp到平面的距离

        return true;
    }

    static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_j_,
                                       const Eigen::Vector3d last_point_l_, const Eigen::Vector3d last_point_m_,
                                       const double s_)
    {
        return (new ceres::AutoDiffCostFunction<
                LidarPlaneFactor, 1, 4, 3>(
            new LidarPlaneFactor(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_)));
    }

    Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m;
    Eigen::Vector3d ljm_norm;
    double s;
};

Ceres ALOAM

原创文章作者：月照银海似蛟龙。如若转载，请注明出处：古月居 https://www.guyuehome.com/38587

打赏 0

上一篇：相机和livox激光雷达外参标定：在gazebo中采集标定数据与处理

下一篇：ALOAM:帧间里程计代码解读

ALOAM:Ceres 优化部分及代码解析

月照银海似蛟龙

前言

ALOAM的自动求导部分

边缘约束

代码分析

整体代码

平面约束

代码分析

整体代码

为你推荐

机器人环境感知①——ROS环境下RGBD相机的使用

ROS Navigation源代码剖析(2)-move_base启动过程

PCL补充之滤波、提取、配准方法

一种lidar和里程计标定的方法介绍--lidar_align

一起做激光SLAM[六]isam于SLAM位姿因子图优化的使用

Autoware 笔记No.8 ENet 障碍物识别（vision segment ENet detect）

评论（0）

关于作者

月照银海似蛟龙

153

0

165

7

IMU预积分在优化问题中的建模及外参标定

无人机 gazebo 仿真---rotors_simulator 功能包介绍

ALOAM：后端lasermapping最终篇---地图更新及消息发布

相关推荐

从零开始搭二维激光SLAM --- 使用单线雷达实现LIO-SAM中的特征点提取

基于EKF（拓展卡尔曼滤波器）的机器人定位（MATLAB编程）

经典文献阅读之--FAST-LIVO（Fast-LIO系列的激光视觉惯性里程计）

VINS学习04———Omni教程

激光slam：LeGO-LOAM---代码编译安装与gazebo测试

ubuntu16.04下用笔记本摄像头和ROS编译运行ORB_SLAM2的单目AR例程

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

TA的专栏

3d 激光slam

专栏导航

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板