深蓝学院-多传感器融合定位-第3章作业

题目内容

第一题：残差模型推导

补充：鉴于写代码时发现A-LOAM的更新的参数块是四元数param_q和平移矢量param_t，所以我重新写了关于R，t的李代数求导，这样就可以分别对R和t进行残差更新：

第二题: Ceres解析求导

大师兄：Ceres库有自动求导、数值求导和解析求导，A-LOAM中用的是自动求导 但是效率会比较低。

本题代码就是要实现Ceres的解析求导，所以我们主要的工作是输入第一题中得到的Jacobian。
（听起来很简单对吧？）

首先我们可以看到在alom_laser_odometry_node.cpp中Line 382左右是线特征的CostFunction:

ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);

大师兄：这就是我们要着手的地方了，接着进入LidarEdgeFactor，看看这个它自动求导是怎么写的，以及我们如何自己手写解析求导。
小萝卜：大师兄，且慢！problem.AddResidualBlock()每个参数的含义是什么？
大师兄：小萝卜，这就对了，懂得问问题啦！
大师兄：这里的cost_function的我们要定义残差Residual和导数Jacobian的地方，loss_function其实就是Ceres的核函数（暂且不管他），para_q和para_t分别是我们要优化的参数块。

小萝卜：谢谢大师兄，我还有一问！为什么我们一开始手写的Jacobian是用了李代数，算Residual对Transformation矩阵的导数，现在怎么变成对q和t求导了呢？对四元数求导？我可不会呀！
大师兄：这个问题问得很好！老纳一开始也在这个问题上栽了根头。我就在纳闷儿，我们公式是对T求导，然后咱们分成了R和t求导，再后来还变成了q和t求导了呢！
大师兄：其实啊，就是咱们这里把Ceres对T的优化，拆分开了分别对R和t进行优化(SO3)，然而由于咱们的输入parameters是四元数q和t，所以咱们还要把R里面3维的旋转向量v转成4维的q。（当然也可以不拆分开用SE(3)）
大师兄：这一步，其实Ceres自带的LocalParameterization已经帮我们解决了！¹

那我们再来看看他是怎么定义的自动求导：

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
	{
   

		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