A-LOAM线/面特征雅可比解析求导

最新推荐文章于 2023-08-21 18:13:19 发布
原创 最新推荐文章于 2023-08-21 18:13:19 发布 · 2.2k 阅读 · 17 ·
CC 4.0 BY-SA版权
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
文章标签：

#自动驾驶

本文详细介绍了如何在lidar odometry中使用雅可比矩阵推导，从线特征的边缘距离优化到面特征的平面拟合，展示了LidarEdgeAnalyticCostFunction和LidarPlaneAnalyticCostFunction的实现，并指导了源代码的修改，涉及ceres库的CostFunction重构。

雅可比矩阵推导

线特征：

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

面特征

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

 

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

需要修改的代码为

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 将结果图片以此命名保存

LOAM点云匹配算法解析与雅克比矩阵推导
liu3612162的博客
08-16 1736
目录前言算法解析线/面特征与线/面地图的残差与对应优化向量计算LM算法简单介绍基于Tait-Bryan xyz extrinsic rotations的雅克比推导代码剖析问题 前言 LOAM点云匹配部分极为经典，可以说是LOAM整个框架的核心，其运算速度快，精度高，自14年发布，并在后续拿下kitti冠军后，直到现在仍然被广泛使用，但在后续的推广中仍然有一些问题，这里做一些解析并记录下自己应用中的问题。 算法解析 算法具体解析的文章实在太多了，这里不做赘述，只简单介绍下相关流程和原理，大致流程如下： gra
SLAM中线特征的参数化和求导过程（从前端提取，到后端优化）
weixin_42001184的博客
12-29 837
SLAM中线特征的参数化和求导过程
基于Aloam对特征匹配及雷达里程计的学习
brightming的专栏
05-29 941
ALoam使用了ceres库进行优化，得到优化后的里程计信息。 学习总结： 1、明确知道要优化什么： 我们的目的是要知道当前雷达的姿态与上一帧的点云姿态相比的变化，即q_last_curr,t_last_curr. 注意：读作curr到last的q，也就是curr在last坐标系下的旋转和位置（t可以直接看做是位置）。 如果这两个量估计的很准确的话，那么将curr的点云中的每个点进行：q_last_curr*p_curr+t_last_curr会对应上last坐标系下的点。 在这里，是落在last坐标系下的
IBVS雅可比矩阵推导（点和线特征）
weixin_63954541的博客
12-04 633
详细推导IBVS点和线特征的雅可比矩阵
LeGO-LOAM的残差公式及雅克比推导
啥也不懂的研究生也淦SLAM的博客
10-19 1500
LeGO-LOAM的残差公式及雅克比推导
矩阵特征分解介绍及雅克比(Jacobi)方法实现特征值和特征向量的求解(C++/OpenCV/Eigen)
网络资源是无限的
05-29 4万+
矩阵特征分解介绍及雅克比(Jacobi)方法实现特征值和特征向量的求解(C++/OpenCV/Eigen)
【LeGO-LOAM论文阅读（二）--特征提取（三）】
看看就好，IT小白
10-24 1406
本文主要讲的是特征提取中线面特征匹配以及优化的代码理解
【A-LOAM三维地图构建】：构建与优化三维地图的策略
![【A-LOAM三维地图构建】：构建与优化三维地图的策略]...A-LOAM（Advanced Laser Odometry and Mapping）是一种先进的激光雷达里程计和地图构建算法，它通过结合激光雷达数据和惯性传感器数据来构建精准的三维
LOAM论文解读_robinvista的博客解析个人读后感
shuttworth的博客
12-27 2004
对于雷达点云来说，将特征点的概念推广到激光点云领域是LOAM作者zhang ji的一大贡献！
【3D激光SLAM】LOAM源代码解析--laserOdometry.cpp
qq_52785580的博客
08-21 1947
本系列文章将对LOAM源代码进行讲解，在讲解过程中，涉及到论文中提到的部分，会结合论文以及我自己的理解进行解读，尤其是对于其中坐标变换的部分，将会进行详细的讲解。本来是懒得写的，一个是怕自己以后忘了，另外是我在学习过程中，其实没有感觉哪一个博主能讲解的通篇都能让我很明白，特别是坐标变换部分的代码，所以想着自己学完之后，按照自己的理解，也写一个LOAM
Lego-LOAM雅可比矩阵的推导
qq_41949101的博客
01-11 770
求解的问题是当前帧投影到上一帧中点到线的最小距离即F(X),因为优化R,T所以X={R,T}。 首先声明几个符号，为当前帧的点，为当前帧投影到上一帧中的点，为当前帧的点投影到上一帧的中的旋转矩阵，T是当前帧的点投影到上一帧的中的平移矩阵。 进一步得到 为什么要反解出呢？ 因为我们是要找一个R和T使得离线特征距离最近，这就是我们优化的目的。 感谢博主：LOAM SLAM原理之源代码分析附公式推导LaserOdometry - 知乎 (zhihu.com) (12条消息) lio-sam
A-LOAM代码结构分析
weixin_43863870的博客
09-01 1090
A-LOAM代码分析 A-LOAM github网址:https://github.com/HKUST-Aerial-Robotics/A-LOAM 最近看了一下a-loam的代码，在这里分享一下自己的理解。A-LOAM是基于文章LOAM写的，但A-LOAM本身并没有文章。 A-LOAM中程序主体在src文件夹下，有4个cpp文件，分别是kittiHelper.cpp、scanRegistration.cpp、laserOdometry.cpp和laserMapping.cpp。其中kittiHelper.
LOAM中Ceres的使用分析
啥也不懂的研究生也淦SLAM的博客
10-14 1783
LOAM中Ceres的使用分析，包括aloam和floam
雅可比矩阵验证
weixin_41738773的博客
04-11 664
雅可比矩阵验证
矩阵的特征值和特征向量的雅克比算法C/C++实现
热门推荐
心如止水-GISer的成长之路
10-18 5万+
矩阵的特征值和特征向量是线性代数以及矩阵论中非常重要的一个概念。在遥感领域也是经常用到，比如多光谱以及高光谱图像的主成分分析要求解波段间协方差矩阵或者相关系数矩阵的特征值和特征向量。根据普通线性代数中的概念，特征值和特征向量可以用传统的方法求得，但是实际项目中一般都是用数值分析的方法来计算，这里介绍一下雅可比迭代法求解特征值和特征向量。雅克比方法用于求实对称阵的全部特征值、特征向量。对于实对称阵
LOAM论文和程序代码的解读
robinvista的专栏
02-19 3万+
目的 　　LOAM是排名第一的状态估计和激光建图算法，本文分析它的特点，目的是了解它到底厉害在什么地方。 1　利用特征点 　　LOAM采用了特征点提取的策略，让点云帧与帧之间的匹配效率更高、准确度也更高。 　　下图中，绿色的点是velodyne 16线激光雷达的原始点云，扫描环境是笔者的卧室，大概就是一个长方体。图中红色的小圆球是提取出来的边缘点，蓝色的是平面点。可见，边缘点基本上位于房间的角落，...
Eigen位姿表示
人无远虑，必有近忧
12-21 2478
Eigen是一个非常强大的矩阵阵，有各种方式表示矩阵和进行矩阵操作。位姿也是一种矩阵，Eigen中有很多方法来描述位姿，本文进行介绍。
loam算法_LOAM核心算法解析之特征点匹配以及距离计算(二)
weixin_39617405的博客
12-04 2186
LOAM核心算法解析之特征点匹配以及距离计算(二)传统的ICP算法是直接计算点-点之间的距离，进行匹配算法。当前因为激光的点的稀疏性相邻两帧的点云很多程度上不同扫描但相同的位置，只能近似认为是相近位置。所以这里LOAM的匹配没有使用点-点匹配算法，而是使用点-线、点-面的特征匹配算法。下面详细介绍LOAM中的点云匹配算法，以及其loss计算方法。LOAM中的协方差采用如下公式中的计算方法...
3D激光SLAM:LOAM 论文--算法详细解读
月照银海似蛟龙的博客
10-14 4187
LOAM 论文详细解读摘要引言相关工作符号和任务描述系统概括A 硬件部分B 算法系统概述激光雷达里程计A 特征点提取Ｂ 找特征点的匹配对C 运动估计lidar 建图 摘要 提出了一个实时激光里程计并建图的算法,使用的是一个三维空间中运动的两轴单线激光雷达. 两轴单线激光雷达,就是在水平方向和垂直方向的单线雷达在两个方向不断变换形成点云,其中每个点的时间戳则不同. 现在主流用的velodyne 的16线\32线的则是 在一个垂直方向上有 16\32个点,它们有相同的时间戳,在水平方向时间戳则是递增的. 这个问
a-loam点到线/面残差构建
最新发布
05-08
<think>好的，用户现在想了解A-LOAM框架中点到线或面残差的构建方法。我需要详细解释这一部分，并结合之前的引用和之前的回答内容。首先，用户之前的问题是关于特征点分类与提取，现在进一步深入到了残差构建，这是优化位姿的关键步骤。 首先，我应该回顾一下之前的回答，其中提到了边缘点和平面点用于计算点到线（Point-to-Edge）和点到面（Point-to-Plane）的残差。现在需要详细说明这些残差是如何构建的，包括数学公式、具体步骤以及代码实现的逻辑。 根据引用[2]，LOAM算法在匹配特征点时，使用点到线和点到面的距离作为残差，类似于ICP算法。需要解释边缘点如何找到对应的线，平面点如何找到对应的面，然后计算距离，进而优化位姿。 接下来，我需要分步骤来讲解残差的构建： 1. **点到线残差的构建**： - 如何选择当前帧的边缘点。 - 在上一帧或地图中找到对应的边缘线，通常通过最近邻搜索找到两个最近点，形成一条直线。 - 计算当前点到这条直线的距离，作为残差。数学公式需要详细推导，可能涉及向量叉乘。 2. **点到面残差的构建**： - 如何选择当前帧的平面点。 - 在上一帧或地图中找到对应的平面，通常需要三个点来确定一个平面。 - 计算当前点到这个平面的距离，使用平面方程或者向量点积的方式，给出公式。 3. **优化过程**： - 如何将这些残差汇总，使用高斯牛顿法或LM法进行非线性优化，求解位姿变换（旋转和平移）。 - 需要说明雅可比矩阵的推导，因为这是优化中的关键步骤。 此外，引用[1]中提到后端优化中的线面特征提取和问题构建，可能需要结合这部分内容，说明在全局地图中的残差构建与前端有何不同。 还要注意代码层面的实现，比如在A-LOAM的哪个模块处理残差，是否有伪代码或关键函数说明。例如，在odometry和mapping模块中，残差的构建可能有不同的数据源（上一帧vs地图）。 用户可能还想知道为什么选择点到线和面而不是点到点，这涉及到特征匹配的稳定性和计算效率，需要简要说明。 需要确保回答结构清晰，分步骤解释，数学公式正确使用$$...$$格式，变量用斜体，引用正确标注来源。同时，生成的相关问题要围绕残差构建的影响、优化方法比较以及动态环境中的挑战等。</think>### A-LOAM框架中点到线/面残差构建方法详解 在A-LOAM中，**点到线（Point-to-Edge）**和**点到面（Point-to-Plane）**的残差构建是实现激光里程计（Odometry）与建图（Mapping）的核心数学基础。其本质是通过几何约束优化激光雷达的位姿（旋转矩阵$R$和平移向量$t$），具体方法如下： --- #### 1. **点到线残差（Point-to-Edge）** **目标**：将当前帧的**边缘点**与目标结构（上一帧或地图中的边缘线）对齐。 ##### **实现步骤**： 1. **特征点选择**： 从当前帧中选取边缘点$p_i$（曲率前20%的点），并在目标结构（上一帧或全局地图）中搜索其对应的**边缘线**。 2. **边缘线匹配**： 在目标点云中找到$p_i$的最近邻点$p_j$，并在其相邻扫描线（同一扫描环或相邻环）中选择两个点$p_{j1}$、$p_{j2}$，构成一条直线$L$。 - **几何约束**：直线$L$由点$p_j$、$p_{j1}$、$p_{j2}$确定，需满足非共线条件： $$ \| (p_{j1} - p_j) \times (p_{j2} - p_j) \| > \epsilon $$ 3. **残差计算**： 计算点$p_i$到直线$L$的垂直距离作为残差。设直线方向向量为$\boldsymbol{v} = (p_{j1} - p_j) \times (p_{j2} - p_j)$，则残差$d_{\text{edge}}$为： $$ d_{\text{edge}} = \frac{\| (p_i - p_j) \times \boldsymbol{v} \|}{\| \boldsymbol{v} \|} $$ 该残差反映了当前点与目标线之间的几何偏差[^1]。 --- #### 2. **点到面残差（Point-to-Plane）** **目标**：将当前帧的**平面点**与目标结构（上一帧或地图中的平面）对齐。 ##### **实现步骤**： 1. **特征点选择**： 从当前帧中选取平面点$p_i$（曲率后20%的点），并在目标结构中找到其对应的**平面**。 2. **平面匹配**： 在目标点云中找到$p_i$的最近邻点$p_j$，并选择其相邻的3个点$p_{j1}$、$p_{j2}$、$p_{j3}$，通过平面方程$ax + by + cz + d = 0$拟合平面$\Pi$。 - **平面方程计算**： 法向量$\boldsymbol{n} = (p_{j2} - p_j) \times (p_{j3} - p_j)$，归一化后得到$(a, b, c)$，常数项$d = -\boldsymbol{n} \cdot p_j$。 3. **残差计算**： 计算点$p_i$到平面$\Pi$的垂直距离作为残差： $$ d_{\text{plane}} = \frac{| a x_i + b y_i + c z_i + d |}{\sqrt{a^2 + b^2 + c^2}} $$ 该残差反映了当前点与目标平面之间的几何偏差[^1]。 --- #### 3. **残差优化与位姿求解** 所有残差通过非线性优化（高斯-牛顿法或列文伯格-马夸尔特法）最小化，目标函数为： $$ \min_{R, t} \sum \left( d_{\text{edge}}^2 + d_{\text{plane}}^2 \right) $$ - **雅可比矩阵计算**： 对每个残差$d$，计算其关于位姿参数（李代数$\boldsymbol{\xi}$）的导数$\frac{\partial d}{\partial \boldsymbol{\xi}}$，用于迭代更新位姿。 - **位姿更新**： 通过线性方程$H \Delta \boldsymbol{\xi} = -b$求解增量$\Delta \boldsymbol{\xi}$，更新当前位姿$T = \exp(\Delta \boldsymbol{\xi}) \cdot T$[^2]。 --- #### 4. **代码实现逻辑** A-LOAM的残差构建主要位于`laserOdometry.cpp`（帧间匹配）和`laserMapping.cpp`（地图匹配）模块，关键步骤如下： ```cpp // 点到线残差构建（以Odometry为例） for each edge_point p_i in current_frame: find nearest edge_line L in last_frame; compute residual d_edge = distance(p_i, L); accumulate residual and Jacobian; // 点到面残差构建 for each planar_point p_i in current_frame: find nearest planar_patch Π in last_frame; compute residual d_plane = distance(p_i, Π); accumulate residual and Jacobian; // 非线性优化求解位姿 while (not converged): solve HΔξ = -b using L-M method; update T = exp(Δξ) * T; ``` --- #### 5. **与传统ICP的对比** - **A-LOAM的几何特征约束**： 利用线、面特征代替原始点云，减少计算量且提升匹配鲁棒性（避免局部极小值）。 - **ICP的局限性**： 传统点到点ICP在稀疏或非结构化环境中易失效，而点到线/面方法更适合激光雷达的几何特性[^2]。 --- ### 相关问题 1. **如何选择边缘线和平面拟合时的邻近点数量？** > 邻近点数过少会导致结构拟合不稳定，过多会增加计算量。A-LOAM默认选择3个点拟合平面，5个点确定边缘线[^1]。 2. **动态环境中的残差构建如何避免误匹配？** > 可通过运动一致性检测（如残差突然增大时剔除异常点）或结合IMU数据预测位姿变化[^3]。 3. **为什么点到面残差不使用平面方程的简化形式？** > 归一化法向量可消除尺度影响，保证残差计算的几何一致性。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值