3D激光SLAM:ALOAM

前言

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

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

{

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

然后继续看里面的内容:

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

Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};

Eigen::Matrix lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};

Eigen::Matrix lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};

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

Eigen::Quaternion q_last_curr{q[3], q[0], q[1], q[2]};

Eigen::Quaternion q_identity{T(1), T(0), T(0), T(0)};

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

q_last_curr = q_identity.slerp(T(s), q_last_curr);

Eigen::Matrix t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};

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

Eigen::Matrix lp;

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

lp = q_last_curr * cp + t_last_curr;

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

Eigen::Matrix nu = (lp - lpa).cross(lp - lpb);//模是三角形面积

Eigen::Matrix 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这里的第1个3是指的三维残差,然后的4是四元数的参数块维度,之后的3是平移向量的参数块维度.

整体代码

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

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

{

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

Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};

Eigen::Matrix lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};

Eigen::Matrix lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};

//Eigen::Quaternion q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};

Eigen::Quaternion q_last_curr{q[3], q[0], q[1], q[2]};//当前帧到上一帧的旋转四元数

Eigen::Quaternion q_identity{T(1), T(0), T(0), T(0)};//单位四元数

//计算的是上一帧到当前帧的位姿变换 , 因此根据匀速模型,计算该点对应的位姿

//这里暂不考虑畸变,因此这里不做任何变换

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

q_last_curr = q_identity.slerp(T(s), q_last_curr);

Eigen::Matrix t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};

Eigen::Matrix lp;//当前帧的点转到上一帧

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

lp = q_last_curr * cp + t_last_curr;

Eigen::Matrix nu = (lp - lpa).cross(lp - lpb);//模是三角形面积

Eigen::Matrix 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 cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};

Eigen::Matrix lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())};

Eigen::Matrix ljm{T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())};

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

Eigen::Quaternion q_last_curr{q[3], q[0], q[1], q[2]};//当前帧到上一帧的旋转四元数

Eigen::Quaternion q_identity{T(1), T(0), T(0), T(0)};//单位四元数

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

q_last_curr = q_identity.slerp(T(s), q_last_curr);

Eigen::Matrix t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};

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

Eigen::Matrix 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这里的第1个1是指的一维残差,然后的4是四元数的参数块维度,之后的3是平移向量的参数块维度.

整体代码

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

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

{

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

Eigen::Matrix cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};//当前点

Eigen::Matrix lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())};//上帧j点

//Eigen::Matrix lpl{T(last_point_l.x()), T(last_point_l.y()), T(last_point_l.z())};

//Eigen::Matrix lpm{T(last_point_m.x()), T(last_point_m.y()), T(last_point_m.z())};

Eigen::Matrix ljm{T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())};//构造里面求的法向量

//Eigen::Quaternion q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};

Eigen::Quaternion q_last_curr{q[3], q[0], q[1], q[2]};//当前帧到上一帧的旋转四元数

Eigen::Quaternion q_identity{T(1), T(0), T(0), T(0)};//单位四元数

//计算的是上一帧到当前帧的位姿变换 , 因此根据匀速模型,计算该点对应的位姿

//这里暂不考虑畸变,因此这里不做任何变换

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

q_last_curr = q_identity.slerp(T(s), q_last_curr);

Eigen::Matrix t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};

Eigen::Matrix 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;

};