A-LOAM线/面特征雅可比解析求导
雅可比矩阵推导
线特征：
核心思想：点到直线距离最小。通过平行四边形面积除以对角线长度
面特征
核心思想：使得点到面距离最短
线/面特征雅可比矩阵解析求导实现
需要修改的代码为
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 q_last_curr(parameters[0]);
Eigen::Map 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 dp_by_dr;
dp_by_dr.block<3,3>(0,0) = -skew_lp_r;
Eigen::Map > 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 dp_by_dt;
//初始化成单位矩阵
(dp_by_dt.block<3,3>(0,0)).setIdentity();
Eigen::Map > 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 q_last_curr(parameters[0]);
Eigen::Map 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 dp_by_dr;
dp_by_dr.block<3,3>(0,0) = -skew_lp_r;
平移矩阵求导：
平移矩阵为单位矩阵I所以可以直接创建单位矩阵：
// 平移矩阵求导
Eigen::Matrix dp_by_dt;
//初始化成单位矩阵
(dp_by_dt.block<3,3>(0,0)).setIdentity(); 
将 距离函数对投影点的导数分别乘以旋转的雅可比和对平移的雅可比：
旋转：
Eigen::Map > 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 > 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 q_last_curr(parameters[0]);
Eigen::Map 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 dp_dr;
dp_dr.block<3,3>(0,0) = -skew_lp_r;
Eigen::Map > 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 dp_dt;
(dp_dt.block<3,3>(0,0)).setIdentity();
Eigen::Map > 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 将结果图片以此命名保存
