激光SLAM从理论到实践第二节作业_eigen bdcsvd-优快云博客

本文链接：https://blog.youkuaiyun.com/weixin_49090852/article/details/138131232

一、作业注意事项

本次的作业为用直接线性方法来对机器人的里程计进行校正。
给出的文件中包含有本次作业使用的bag数据，路径为odom_ws/bag/odom.bag。
本次的作业中，需要实现三个函数，分别为：

Main.cpp，第340行中的cal_delta_distance()函数，该函数的功能为给定两个里程计位姿，计算这两个位姿之间的位姿差。
Odom_Calib.cpp，第23行Add_Data()函数，该函数的功能为构建超定方程组Ax=b，具体参考PPT。
Odom_Calib.cpp，第44行 Solve()函数，该函数的功能为对2中构建的超定方程组进行求解。

本次程序的运行过程为：

实现上述的三个函数，并且进行编译。
在odom_ws下，进行source：source devel/setup.bash
运行launch文件:roslaunch calib_odom odomCalib.launch。执行本条指令的时候，必须保证没有任何ros节点在运行，roscore也要关闭。
在3正常的情况下，运行rviz，fix_frame选择为odom_frame。在Add选项卡中增加三条Path消息。一条订阅的topic为：odom_path_pub_;一条订阅的topic为:scan_path_pub_；最后一条为:calib_path_pub_。分别选择不同的颜色。
进入到odom_ws/bag目录下，运行指令：rosbag play --clock odom.bag。
如果一切正常，则能看到运行矫正程序的终端会打印数据，并且rviz中可以看到两条路径。当打印的数据到达一个的数量之后，则可以开始矫正。
矫正的命令为，在calib_flag的topic下发布一个数据：rostopic pub /calib_flag std_msgs/Empty “{}”。
程序矫正完毕会输出对应的矫正矩阵，并且会在rviz中显示出第三条路径，即calib_path。可以观察里程计路径odom_path和矫正路径_calib_path区别来判断此次矫正的效果。

二、代码补充

1.cal_delta_distance()函数

该函数目标求得机器人坐标系下两帧之间的位姿差，即里程计的位姿增量。下图中的dx、dy、dθ这三个参数为机器人坐标系下的值。last_pos和new_pos为世界坐标系的值，因此要将d_pos通过旋转矩阵转化到机器人坐标系。最终能得到里程计位姿增量。

在该函数中，机器人坐标是参照上一时刻的坐标，因此需要求出旋转矩阵T(w->r)。

机器人坐标系与世界坐标系相差θ角

// 创建一个绕 z 轴旋转 θ 的旋转对象

Eigen::AngleAxisd rotation(θ, Eigen::Vector3d::UnitZ());

// 获取旋转矩阵

Eigen::Matrix3d rotation_matrix = rotation.matrix();

由于我们需要的是旋转矩阵T(w->r)，T(r->w)的逆等于T(w->r)。因此求得的T(r->w)需要再转置，及加上inverse()。

2.Add_Data()函数

该函数是为了构建最小二乘方程组，输入为里程计和激光数据的增量。这里应该是增量，因为航迹推算是用增量来计算，从公式可以看出是增量有误差，因此要修正增量。

构造的方程如下图所示，Odom=[uix,uiy,uiθ]，构造为temp_A矩阵，scan构造为temp_b向量。

最终解方程是使用全部的历史数据，存储在A矩阵、b向量中，因此要将temp_A和temp_b添加到A和b当中。

3.Solve()函数

即解方程，使用奇异值分解（SVD）

// 解超定方程组 Ax = b

Eigen::VectorXd x = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);

这行代码中的 bdcSvd 函数用于计算矩阵 A 的奇异值分解（SVD）。Eigen::ComputeThinU | Eigen::ComputeThinV 表示我们需要计算矩阵 A 的左奇异向量矩阵（U）和右奇异向量矩阵（V），这两个矩阵的列数与 A 的秩相同。solve 函数用于求解线性方程组，即解 Ax=b。它使用了 SVD 分解的结果来求解最小二乘问题，返回的结果是一个 Eigen::VectorXd 类型的向量 x，其中包含了方程组的解。

三、运行代码

1.参考这两篇博客。

激光slam学习——第二章（里程计标定）习题_examples/cmakefiles/isam2example_smartfactor.dir/b-优快云博客

深蓝学院，激光slam第二讲作业，踩坑总结_rviz的fixedfram里没有odom-优快云博客

创建新的工作空间

mkdir -p ~/lidar_ws/src #创建工作空间lidar_ws和子目录src，自定义空间名
cd ~/lidar_ws/ #进入到工作空间lidar_ws
catkin_make #编译工作空间lidar_ws

记得添加环境变量

将 source ~/lidar_ws/devel/setup.bash 添加到 .bashrc 文件中是为了在每次打开新的终端时都自动执行这个命令。这个命令的作用是加载 ROS 工作空间的环境变量和相关设置。

具体而言，这个命令会执行 setup.bash 脚本文件，该文件位于 ROS 工作空间的 devel 目录下。这个脚本会设置一些环境变量，包括 PATH、ROS_PACKAGE_PATH、PYTHONPATH 等，以确保 ROS 节点和程序能够正确地找到所需的包和资源。

2.运行结果