当前位置：网站首页>3D激光SLAM:ALOAM---Ceres 优化部分及代码分析
3D激光SLAM:ALOAM---Ceres 优化部分及代码分析
2022-07-16 10:00:00 【月照银海似蛟龙】
前言
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是平移向量的参数块维度.
整体代码
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是平移向量的参数块维度.
整体代码
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;
};
边栏推荐
- [recognizing cloud Nativity] Chapter 4 cloud network section 4.9.4.2 - Implementation of smart network card
- Halcon picture folder rename
- SNMP（简单网络管理协议）
- The difference between query string, formdata and request payload
- 结构思考力
- Tutorial of database system principle and application (020) -- login MySQL
- Halcon 距离计算
- Block的底层实现
- （cf)Codeforces Round ＃807 (Div. 2)A--C思维题
- Joint autoregressive and hierarchical priorities for learned image compression
猜你喜欢
第五人格PC端下载以及解决打不开问题
![[untitled] pseudo class selector and box model](/img/4d/eb5f429d4841baf6d88ad6eb7a5611.png)
[untitled] pseudo class selector and box model
AC管理
The latest version 0.3 of Infini console, which has been waiting for a long time, is officially released!
![[bioinformatics] exosome miRNA growth training camp (14 days)](/img/ff/06bde421040aa80c7bfd7534335f40.png)
[bioinformatics] exosome miRNA growth training camp (14 days)
【今天的小go同学要去丢垃圾（3）】
Unity 基础 之 Camera摄像机属性介绍
The principle and design principle of index
Atcoder beginer contest 259 partial solution
Advanced Mathematics -- Chapter 8 partial derivatives and total derivatives of implicit functions
随机推荐
实现团队工作效率与质量共赢，这款办公协同工具真够用！
(CF) codeforces round 807 (Div. 2) a--c thinking questions
大模型训练难于上青天？效率超群、易用的“李白”模型库来了
脱敏项目的苦乐得失
数据库每日一题---第24天：格式化部门表
灰度直方图
MATLAB数字图像处理 实验二:单像素空域图像增强
The difference between query string, formdata and request payload
AP统计学
应用层 万维网WWW
给已有数据的字段添加索引（达梦数据库版）
The principle and design principle of index
【分享】二层交换和三层交换转发
[QNX Hypervisor 2.2用户手册]8.1 虚拟环境下的开销
English语法_定冠词the_小细节
mysql进阶(四)聊聊mysql中的事务锁机制
高等数学---第八章隐函数偏导数与全微分
[recognizing cloud Nativity] Chapter 4 cloud network section 4.9.4.2 - Implementation of smart network card
Unity 基础 之 Camera摄像机属性介绍
The latest version 0.3 of Infini console, which has been waiting for a long time, is officially released!