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

### 关于 A-LOAM `laserOdometry.cpp` 编译错误 `'LocalPar' 未定义` 在处理此类编译错误时,通常是因为变量 `LocalPar` 在当前作用域中未被正确定义或声明。以下是可能的原因以及解决方案: #### 可能原因分析 1. **头文件缺失** 如果 `LocalPar` 是由某个特定的头文件定义,则可能是该头文件未被包含到源码文件中。这可能导致编译器无法找到 `LocalPar` 的定义[^3]。 2. **命名空间冲突** 若 `LocalPar` 定义在一个特定的命名空间下而未显式指定,则可能会引发未定义错误。例如,如果 `LocalPar` 属于某命名空间 `MyNamespace`,则需通过 `MyNamespace::LocalPar` 来访问它[^4]。 3. **链接库配置不正确** 当使用外部库(如 OpenCV 或其他第三方库)时,若链接阶段缺少必要的库文件,也可能导致类似的未定义错误。此情况常见于跨平台开发环境中使用的工具链设置不当[^2]。 4. **CMakeLists.txt 配置问题** 假设项目依赖 CMake 构建系统,在构建过程中未能正确传递目标所需的编译标志或者遗漏了某些必需的目标链接项,也会造成上述现象[^1]。 #### 解决方案 针对以上提到的各种可能性,可以采取如下措施逐一排查并解决问题: ##### 方法一:确认头文件是否已引入 检查 `laserOdometry.cpp` 文件顶部是否有适当导入对应定义 `LocalPar` 的头文件。如果没有,请补充相应头文件路径。例如: ```cpp #include "local_par_definition.h" ``` ##### 方法二:验证命名空间归属 假如知道 `LocalPar` 应属于某一具体命名空间,那么应确保每次调用前加上完整的限定名;或者采用 `using namespace MyNamespace;` 方便后续操作。比如: ```cpp // 使用完全限定名称 MyNamespace::LocalPar localVariable; // 或者引入整个命名空间 using namespace MyNamespace; LocalPar localVariable; ``` ##### 方法三:调整 CMake 配置 仔细审查项目的 `CMakeLists.txt` 文件,保证所有涉及的静态/动态库都被正确添加至目标链接列表之中。对于多平台支持场景尤其重要的是考虑不同操作系统间差异化的编译参数设定方式: ```cmake if(WIN32) target_link_libraries(your_target PRIVATE ${WINDOWS_SPECIFIC_LIBRARIES}) elseif(UNIX OR APPLE) target_link_libraries(your_target PRIVATE ${LINUX_OR_MACOS_LIBRARIES}) endif() ``` ##### 方法四:重新审视全局范围内的宏定义 有时开发者会借助预处理器指令来控制部分功能实现与否。因此有必要查看是否存在影响 `LocalPar` 存在性的条件编译逻辑。像这样子的例子: ```cpp #ifdef ENABLE_LOCAL_PAR_FEATURE #define LocalPar SomeTypeOfValue #endif ``` 此时需要依据实际需求决定开启还是关闭相关特性开关。 --- ### 示例代码修正片段 假设经过初步诊断发现问题是由于忘记加入自定义数据结构所在的头文件引起的话,修改后的版本大致如下所示: ```cpp // 添加必要头文件 #include "custom_data_structures.h" int main() { // 正确初始化对象实例 CustomStructs::LocalPar myParameterSet; return 0; } ``` 同时更新关联的 `CMakeLists.txt` 如下: ```cmake add_executable(example_project main.cpp custom_data_structures.cpp) target_include_directories(example_project PUBLIC "${PROJECT_SOURCE_DIR}/include") find_package(PkgConfig REQUIRED) pkg_check_modules(PC_EIGEN Eigen3 REQUIRED) target_link_libraries(example_project PRIVATE PC_EIGEN::Eigen3) ``` ---
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值