3D激光SLAM:ALOAM---Ceres 优化部分及代码分析
【摘要】
3D激光SLAM:ALOAM---Ceres 优化部分及代码分析
前言ALOAM的自动求导部分边缘约束代码分析整体代码
平面约束代码分析整体代码
前言
Ceres solv...
前言
Ceres solver 是谷歌开发的一款用于非线性优化的库，在谷歌的开源激光雷达slam项目cartographer中被大量使用。
Ceres可以解决边界约束鲁棒非线性最小二乘法优化问题。这个概念可以用以下表达式表示：
这一表达式在工程和科学领域有非常广泛的应用。比如统计学中的曲线拟合，或者在计算机视觉中依据图像进行三维模型的构建等等。
ALOAM里面的优化问题的建模和求解都是通过Ceres 进行的,包括前端的帧间里程计和后端的地图优化.
相比于其它优化库(g20 gtsam),Ceres 一个很大的优点就是自动求导功能
通常一个优化器会帮助解决优化问题中大部分内容,但是残差的计算以及残差对优化变量的雅克比矩阵通常需要用户自己定义,而雅克比矩阵通常比较复杂,因此有的优化库如G2O,GTSAM会预先定义好一些常见的优化问题,所涉及的残差及雅克比计算方式,但是并不能覆盖所有场景,一旦有些问题不是优化器事先建模好的问题,那就需要用户自己去定义残差和雅克比矩阵的计算方式,这个会非常麻烦,而且容易出错.ceres通过引用自动求导的功能,无论什么优化问题,用户只需要定义残差的计算方式,自动求导功能会帮助用户计算对应的雅克比矩阵来实现优化问题的求解.
所以在研究一个Ceres的优化是如何使用的,重点就是看自动求导这部分.
Ceres的自动求导使用流程:
- 自定义一个模板类,或者结构体两者差不多.
- 重载()运算符
- ()运算符函数前几个参数是参数块的起始指针
- ()运算符函数最后一个参数是残差模块的指针
- ()运算符负责计算残差
ALOAM的自动求导部分
ALOAM的自动求导部分在src文件夹下的lidarFactor.hpp文件中
边缘约束
代码分析
边缘约束自定义的模板类是
struct LidarEdgeFactor//边缘约束
- 1
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_) {}
- 1
- 2
- 3
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
{
- 1
- 2
- 3
正如前面说的前几个参数是参数块的起始指针
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())};
- 1
- 2
- 3
- 4
将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)};
- 1
- 2
构建当前帧到上一帧的旋转四元数和一个单位四元数
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]};
- 1
- 2
考虑匀速模型(这一帧所有的旋转和平移都是一样的),根据当前点的时间戳占比,来计算,这个点到上一帧的旋转和平移
Eigen::Matrix<T, 3, 1> lp;
// 把当前点根据当前计算的帧间位姿变换到上一帧
lp = q_last_curr * cp + t_last_curr;
- 1
- 2
- 3
把当前点根据当前计算的帧间位姿变换到上一帧
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();
- 1
- 2
- 3
- 4
- 5
- 6
- 7
根据向量的运算,把残差设计成 点到第边的垂线长度.
残差越小说明点里线的距离越近,
所以该优化会不断的迭代,找到一个合适的q和t,使得点到线的距离最小.
return true;
}
- 1
- 2
最后返回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_)));
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
最后使用一个 函数,来返回一个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;
};
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
平面约束
代码分析
平面约束自定义的模板类是
struct LidarPlaneFactor//平面约束
- 1
构造函数是:
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();
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
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();//法向量单位化
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
然后在构造函数里面进行了上帧三个点构成平面的法向量计算与单位化
然后我们看重点的重载()运算符函数
bool operator()(const T *q, const T *t, T *residual) const
{
- 1
- 2
和边缘约束一样
- 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())};
- 1
- 2
- 3
将电云的数据转成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)};//单位四元数
- 1
- 2
用参数块声明 当前帧到上一帧的旋转四元数
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]};
- 1
- 2
根据s当前点的时间戳占比
考虑匀速模型(这一帧所有的旋转和平移都是一样的),根据当前点的时间戳占比,来计算,这个点到上一帧的旋转和平移
Eigen::Matrix<T, 3, 1> lp;//当前点转到上一帧的点声明
lp = q_last_curr * cp + t_last_curr;//当前点转到上一帧
- 1
- 2
当前点转到上一帧
residual[0] = (lp - lpj).dot(ljm);
- 1
点lp和点lpj构成的向量在法向量的投影,就是lp到平面的距离
最后返回true
return true;
}
- 1
- 2
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_)));
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
最后使用一个 函数,来返回一个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;
};
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
文章来源: blog.csdn.net，作者：月照银海似蛟龙，版权归原作者所有，如需转载，请联系作者。
原文链接：blog.csdn.net/qq_32761549/article/details/125746745
【版权声明】本文为华为云社区用户转载文章，如果您发现本社区中有涉嫌抄袭的内容，欢迎发送邮件进行举报，并提供相关证据，一经查实，本社区将立刻删除涉嫌侵权内容，举报邮箱：
cloudbbs@huaweicloud.com
- 点赞
- 收藏
- 关注作者
评论（0）