3D激光SLAM:ALOAM---Ceres 优化部分及代码分析

举报
月照银海似蛟龙 发表于 2022/07/29 09:31:37 2022/07/29
【摘要】 ALOAM里面的优化问题的建模和求解都是通过Ceres 进行的,包括前端的帧间里程计和后端的地图优化. 相比于其它优化库(g20 gtsam),Ceres 一个很大的优点就是**自动求导功能** 通常一个优化器会帮助解决优化问题中大部分内容,但是残差的计算以及残差对优化变量的雅克比矩阵通常需要用户自己定义,而雅克比矩阵通常比较复杂,因此有的优化库如G2O,GTSAM会预先定义好一些常见的优化问

前言

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;
};
【版权声明】本文为华为云社区用户原创内容，转载时必须标注文章的来源（华为云社区）、文章链接、文章作者等基本信息， 否则作者和本社区有权追究责任。如果您发现本社区中有涉嫌抄袭的内容，欢迎发送邮件进行举报，并提供相关证据，一经查实，本社区将立刻删除涉嫌侵权内容，举报邮箱： cloudbbs@huaweicloud.com
C语言 智能汽车 机器人
  • 点赞
  • 收藏
  • 关注作者

评论（0）

0/1000
抱歉，系统识别当前为高风险访问，暂不支持该操作

全部回复

上滑加载中

设置昵称

在此一键设置昵称，即可参与社区互动！

*长度不超过10个汉字或20个英文字符，设置后3个月内不可修改。

*长度不超过10个汉字或20个英文字符，设置后3个月内不可修改。

确认 取消

加入云驻计划，成为创作者

  • 华为云周边好礼
  • 免费体验产品
  • 特殊身份标识
  • 线下官方门票
  • 内部专家零距离
  • 与10000+优质创作者共同成长
立即加入