雅可比矩阵推导

线特征：

核心思想：点到直线距离最小。通过平行四边形面积除以对角线长度

面特征

核心思想：使得点到面距离最短

 线/面特征雅可比矩阵解析求导实现

需要修改的代码为

1. include/lidar_localization/models/loam/aloam_factor.hpp

修改好之后，需要修改代码接口部分

面特征

在03-lidar-odometry-advanced/src/lidar_localization/src/aloam_scan_scan_registration_node.cpp中进行如下替换：482行
 // ceres::CostFunction *cost_function = LidarPlaneFactor::Create(curr_point, last_point_a, last_point_b, last_point_c, s);
 ceres::CostFunction *cost_function = new LidarPlaneAnalyticCostFunction(curr_point, last_point_a, last_point_b, last_point_c,s);  
 
 
在03-lidar-odometry-advanced/src/lidar_localization/src/models/loam/aloam_registration.cpp中进行如下替换100行
// ceres::CostFunction *factor_plane = LidarPlaneFactor::Create(
    //     source, 
    //     target_x, target_y, target_z, 
    //     ratio
    // );
ceres::CostFunction *factor_plane = new LidarPlaneAnalyticCostFunction(source,target_x, target_y, target_z,ratio);
 
    problem_.AddResidualBlock(
        factor_plane,
        config_.loss_function_ptr, 
        param_.q, param_.t
    );

线特征：

在03-lidar-odometry-advanced/src/lidar_localization/src/aloam_scan_map_registration_node.cpp中进行如下替换：619行
// ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, point_a, point_b, 1.0);
ceres::CostFunction *cost_function = new LidarEdgeAnalyticCostFunction(curr_point, point_a, point_b, 1.0);
 
在03-lidar-odometry-advanced/src/lidar_localization/src/aloam_scan_scan_registration_node.cpp中进行如下替换：384行：
// ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);
ceres::CostFunction *cost_function = new LidarEdgeAnalyticCostFunction(curr_point, last_point_a, last_point_b, s);
 
在03-lidar-odometry-advanced/src/lidar_localization/src/models/loam/aloam_registration.cpp中进行尼姑如下替换：
// ceres::CostFunction *factor_edge = LidarEdgeFactor::Create(
    //     source, 
    //     target_x, target_y, 
    //     ratio
    // );
ceres::CostFunction *factor_edge = new LidarEdgeAnalyticCostFunction(source, target_x, target_y, ratio);
    
    problem_.AddResidualBlock(
        factor_edge, 
        config_.loss_function_ptr, 
        param_.q, param_.t
    );

源代码利用直接求导的方式定义面特征与线特征自动求导结构体

struct LidarEdgeFactor
struct LidarPlaneFactor

线特征：

//第一个参数为残差块的维数3,第二和第三个参数为参数块的维数。分别为四元数代表旋转4维。平移矩阵3维
class LidarEdgeAnalyticCostFunction : public ceres::SizedCostFunction<3, 4, 3> {
public:
	/*
	curr_point_:当前帧某点，
	last_point_a_:上一帧点a，
	last_point_b_:上一帧点b
	s_:阈值
	*/
    LidarEdgeAnalyticCostFunction(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_) {}
 
  	virtual bool Evaluate(double const* const* parameters,double* residuals,double** jacobians) const 
	{
		/*
		q_last_curr:四元数，
		t_last_curr:代表旋转，平移
		*/
		Eigen::Map<const Eigen::Quaterniond> q_last_curr(parameters[0]);
		Eigen::Map<const Eigen::Vector3d> t_last_curr(parameters[1]);
		Eigen::Vector3d lp;
		Eigen::Vector3d lp_r;
		lp_r = q_last_curr * curr_point; // for computing Jacobian of Rotation: dp_by_dr
		//将当前帧投影到上一帧：对应公式
		lp = q_last_curr * curr_point + t_last_curr; //new point
		//叉乘求四边形面积
		Eigen::Vector3d nu = (lp - last_point_a).cross(lp - last_point_b);
		//上一帧两点连线向量
		Eigen::Vector3d de = last_point_a - last_point_b;
		//三个方向残差
		residuals[0] = nu.x() / de.norm();
		residuals[1] = nu.y() / de.norm();
		residuals[2] = nu.z() / de.norm();
 
		if(jacobians != NULL)
		{
			if(jacobians[0] != NULL)
			{
				Eigen::Vector3d re = last_point_b - last_point_a;
				// Eigen::Matrix3d skew_re = skew(re);
				//求解last_point_b - last_point_a的反对陈矩阵
				Eigen::Matrix3d skew_re;
				skew_re(0,0)=0;skew_re(0,1)=-re(2);skew_re(0,2)=re(1);
				skew_re(1,0)=re(2);skew_re(1,1)=0;skew_re(1,2)=-re(0);
				skew_re(2,0)=-re(1);skew_re(2,1)=re(0);skew_re(2,2)=0;
 
				//  旋转矩阵求导
				//Eigen::Matrix3d skew_lp_r = skew(lp_r);
				Eigen::Matrix3d skew_lp_r;
				//
				skew_lp_r(0,0)=0;skew_lp_r(0,1)=-lp(2);skew_lp_r(0,2)=lp(1);
				skew_lp_r(1,0)=lp(2);skew_lp_r(1,1)=0;skew_lp_r(1,2)=-lp(0);
				skew_lp_r(2,0)=-lp(1);skew_lp_r(2,1)=lp(0);skew_lp_r(2,2)=0;
				Eigen::Matrix<double, 3, 3> dp_by_dr;
				dp_by_dr.block<3,3>(0,0) = -skew_lp_r;
				Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor> > J_so3_r(jacobians[0]);
				J_so3_r.setZero();
				//提取雅克比矩阵左上角的3*3矩阵
				J_so3_r.block<3,3>(0,0) = skew_re * dp_by_dr / de.norm();
 
				// 平移矩阵求导
				Eigen::Matrix<double, 3, 3> dp_by_dt;
				//初始化成单位矩阵
				(dp_by_dt.block<3,3>(0,0)).setIdentity();
				Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > J_so3_t(jacobians[1]);
				J_so3_t.setZero();
				J_so3_t.block<3,3>(0,0) = skew_re * dp_by_dt / de.norm();	
			}
		}
 
		return true;
 
	}
 
protected:
	Eigen::Vector3d curr_point, last_point_a, last_point_b;
	double s;
};

代码详解

因为一些部分涉及到公式，在vs中注释不方便，所以在此详细注释：

将第k+1帧点云投影到第k帧

Eigen::Map<const Eigen::Quaterniond> q_last_curr(parameters[0]);
Eigen::Map<const Eigen::Vector3d> t_last_curr(parameters[1]);
Eigen::Vector3d lp;
lp = q_last_curr * curr_point + t_last_curr; //new point

线特征表达式：

//叉乘求四边形面积
Eigen::Vector3d nu = (lp - last_point_a).cross(lp - last_point_b);
//上一帧两点连线向量
Eigen::Vector3d de = last_point_a - last_point_b;
//三个方向残差
residuals[0] = nu.x() / de.norm();
residuals[1] = nu.y() / de.norm();
residuals[2] = nu.z() / de.norm();

求解雅可比矩阵：

 雅可比矩阵分为两个部分。一部分是距离函数对投影点求导，另一部分是投影点对变换矩阵求导：

Eigen::Vector3d re = last_point_b - last_point_a;
// Eigen::Matrix3d skew_re = skew(re);
//求解last_point_b - last_point_a的反对陈矩阵
Eigen::Matrix3d skew_re;
skew_re(0,0)=0;skew_re(0,1)=-re(2);skew_re(0,2)=re(1);
skew_re(1,0)=re(2);skew_re(1,1)=0;skew_re(1,2)=-re(0);
skew_re(2,0)=-re(1);skew_re(2,1)=re(0);skew_re(2,2)=0;

反对称矩阵如下：

 旋转矩阵部分求导：

其中lp即为(Rp+t)

skew_lp_r(0,0)=0;skew_lp_r(0,1)=-lp(2);skew_lp_r(0,2)=lp(1);
skew_lp_r(1,0)=lp(2);skew_lp_r(1,1)=0;skew_lp_r(1,2)=-lp(0);
skew_lp_r(2,0)=-lp(1);skew_lp_r(2,1)=lp(0);skew_lp_r(2,2)=0;
Eigen::Matrix<double, 3, 3> dp_by_dr;
dp_by_dr.block<3,3>(0,0) = -skew_lp_r;

平移矩阵求导：

平移矩阵为单位矩阵I所以可以直接创建单位矩阵：

// 平移矩阵求导
Eigen::Matrix<double, 3, 3> dp_by_dt;
//初始化成单位矩阵
(dp_by_dt.block<3,3>(0,0)).setIdentity();

将 距离函数对投影点的导数分别乘以旋转的雅可比和对平移的雅可比：

旋转：

Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor> > J_so3_r(jacobians[0]);
J_so3_r.setZero();
//提取雅克比矩阵左上角的3*3矩阵
J_so3_r.block<3,3>(0,0) = skew_re * dp_by_dr / de.norm();

平移：

Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > J_so3_t(jacobians[1]);
J_so3_t.setZero();
J_so3_t.block<3,3>(0,0) = skew_re * dp_by_dt / de.norm();

 面特征

class LidarPlaneAnalyticCostFunction : public ceres::SizedCostFunction<1, 4, 3> {
public:
    LidarPlaneAnalyticCostFunction(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_) {}
 
  	virtual bool Evaluate(double const* const* parameters,
                        double* residuals,
                        double** jacobians) const 
	{
		Eigen::Map<const Eigen::Quaterniond> q_last_curr(parameters[0]);
		Eigen::Map<const Eigen::Vector3d> t_last_curr(parameters[1]);
		Eigen::Vector3d lp;
		Eigen::Vector3d lp_r;
		lp_r = q_last_curr * curr_point; // for computing Jacobian of Rotation: dp_dr
		lp = q_last_curr * curr_point + t_last_curr; //new point
		Eigen::Vector3d de = (last_point_l-last_point_j).cross(last_point_m-last_point_j);
		double nu = (lp-last_point_j).dot(de);
		
		residuals[0] = nu / de.norm();
 
		if(jacobians != NULL)
		{
			if(jacobians[0] != NULL)
			{
				Eigen::Vector3d dX_dp = de / de.norm();
				double X = nu / de.norm();
				Eigen::Vector3d ddh_dp = X * dX_dp / std::abs(X);
				//  Rotation
				//Eigen::Matrix3d skew_lp_r = skew(lp_r);
				Eigen::Matrix3d skew_lp_r;
				skew_lp_r(0,0)=0;skew_lp_r(0,1)=-lp(2);skew_lp_r(0,2)=lp(1);
				skew_lp_r(1,0)=lp(2);skew_lp_r(1,1)=0;skew_lp_r(1,2)=-lp(0);
				skew_lp_r(2,0)=-lp(1);skew_lp_r(2,1)=lp(0);skew_lp_r(2,2)=0;
				Eigen::Matrix<double, 3, 3> dp_dr;
				dp_dr.block<3,3>(0,0) = -skew_lp_r;
				Eigen::Map<Eigen::Matrix<double, 1, 4, Eigen::RowMajor> > J_so3_r(jacobians[0]);
				J_so3_r.setZero();
				J_so3_r.block<1,3>(0,0) = ddh_dp.transpose() * dp_dr;
 
				// Translation
				Eigen::Matrix<double, 3, 3> dp_dt;
				(dp_dt.block<3,3>(0,0)).setIdentity();
				Eigen::Map<Eigen::Matrix<double, 1, 3, Eigen::RowMajor> > J_so3_t(jacobians[1]);
				J_so3_t.setZero();
				J_so3_t.block<1,3>(0,0) = ddh_dp.transpose() * dp_dt;
			}
		}
 
		return true;
 
	}
 
protected:
	Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m;
	double s;
};

 代码详解

将当前点投影到上一帧与线特征相同，此处不再解释；

 计算点到平面距离：

Eigen::Vector3d de = (last_point_l-last_point_j).cross(last_point_m-last_point_j);
double nu = (lp-last_point_j).dot(de);
residuals[0] = nu / de.norm();

注意： eigen里叉乘用cross，点乘用dot

Eigen::Vector3d dX_dp = de / de.norm();
double X = nu / de.norm();
Eigen::Vector3d ddh_dp = X * dX_dp / std::abs(X);

 旋转矩阵和平移矩阵，与线特正相同，此处不再解释;

结果分析:

 利用evo评估轨迹：

APE：

evo_ape kitti ground_truth.txt laser_odom.txt -va --plot --plot_mode xy --save_results jiexi.zip
 
--plot_mode xy // 画出xy轴
 jiexi.zip保存结果压缩文件名称

PRE：

evo_rpe kitti ground_truth.txt laser_odom.txt  -r  trans_part --delta 100 --plot --plot_mode xy --save_plot ./jiexipre --save_results ./jiexipre.zip
 
--save_plot ./jiexipre 将结果图片以此命名保存