gmapping代码学习阅读
这里借读的代码是白茶清欢的gmapping手写板，仅作阅读学习
基于滤波器的SLAM算法-《gmapping算法的删减版》+《加入激光雷达运动畸变去除》
在原来gmapping源码的基础之上，公众号：小白学移动机器人的作者对其进行了大刀阔斧的更改。
（1）删除几乎所有不需要的代码，对代码的运行结构也进行了调整
（2）对该代码进行详细中文注释，以及对核心代码进行更改
（3）将激光雷达运动畸变去除算法，直接加入删减版的gmapping算法中，算法文件在part_data文件夹
目录结构
下面是目录结构
robot@robot-virtual-machine:~/my_slam_gmapping$ tree
.
├── CMakeLists.txt
├── include
│ └── my_slam_gmapping
├── launch
│ └── my_slam_gmapping.launch
├── package.xml
└── src
├── part_data
│ └── lidar_undistortion
│ ├── lidar_undistortion.cpp
│ └── lidar_undistortion.h
├── part_ros
│ ├── main.cpp
│ ├── my_slam_gmapping.cpp
│ └── my_slam_gmapping.h
└── part_slam
├── grid
│ ├── array2d.h
│ ├── harray2d.h
│ └── map.h
├── gridfastslam
│ ├── gridslamprocessor.cpp
│ └── gridslamprocessor.h
├── motionmodel
│ ├── motionmodel.cpp
│ └── motionmodel.h
├── particlefilter
│ └── particlefilter.h
├── scanmatcher
│ ├── gridlinetraversal.h
│ ├── scanmatcher.cpp
│ └── scanmatcher.h
├── sensor_range
│ ├── rangereading.cpp
│ └── rangereading.h
└── utils
├── macro_params.h
└── point.h
part_data里面的lidar_undistortion是实现雷达运动去畸变
part_slam的motionmodel是通过odom的数据推算出机器人下一个时刻的大致位置
scanmatcher是之前提到通过odom得到P1的大致位置之后，通过与地地图的匹配，得到P1的最优位置，scanmatcher是做最优匹配的，
sensor_range里的rangereading是由于每一个激光雷达激光束是角度相同的，但是是有畸变的，RangeReading结构体来存储每一个激光束对应的角度和距离
函数讲解
初始化参数
main函数
先从main函数讲起，位于src\part_ros\main.cpp
int main(int argc, char ** argv)
{
ros::init(argc, argv, "my_slam_gmapping");
MySlamGMapping slamer;
slamer.startLiveSlam();
ros::spin();
return 0;
}初始化了一个MySlamGMapping的对象，
slamer.startLiveSlam()启动slam，接下来看一下MySlamGMapping类
MySlamGMapping类
构造函数
//构造函数-初始化相关变量，比如指针的初始化
MySlamGMapping::MySlamGMapping():
map_to_odom_(tf::Transform(tf::createQuaternionFromRPY( 0, 0, 0 ), tf::Point(0, 0, 0 ))),//默认两个坐标系重合
private_nh_("~"),
scan_filter_sub_(NULL),
scan_filter_(NULL),
transform_thread_(NULL)
{
seed_ = time(NULL);
init();
}初始化的复制，先看一下map_to_odom
map_to_odom是tf transform的一个变换，描述的是map到odom的变化，这里初始化全为0
scan_filter_sub_(NULL), scan_filter_(NULL),是我们要对输入的scan数据和odom数据进行一个同步处理
message_filters::Subscriber<sensor_msgs::LaserScan>* scan_filter_sub_;
tf::MessageFilter<sensor_msgs::LaserScan>* scan_filter_;scan_filter_sub_是scan数据的一个订阅器，但是这个订阅器加了一个message_filters,加了一个数据过滤，这个和普通的订阅器相比较，就是加了一个过滤的功能
scan_filter_是核心过滤器，后面会讲
transform_thread_
boost::thread* transform_thread_; //发布转换关系的线程他是一个线程变量，实时发布map到odom的变换的线程
在MySlamGMapping::startLiveSlam()里
/*发布map到odom的转换关系的线程*/
transform_thread_ = new boost::thread(boost::bind(&MySlamGMapping::publishLoop, this, transform_publish_period_));这里transform_thread_是由做发布变换的工作的，实例化了一个线程，实时发布
回到构造函数，看到有一个seed，是高斯噪声的随机种子，
最后进入初始化init函数
init初始化函数
//slamgmapping的初始化，主要用来读取配置文件中写入的参数以及初始化一些对象
void MySlamGMapping::init()
{
if(!private_nh_.getParam("map_frame", map_frame_))
map_frame_ = "map";
if(!private_nh_.getParam("odom_frame", odom_frame_))
odom_frame_ = "odom";
if(!private_nh_.getParam("scan_topic", scan_topic_))
scan_topic_ = "scan";
if(!private_nh_.getParam("laser_frame", laser_frame_))
laser_frame_ = "laser_link";
//new一个激光雷达运动畸变的对象
lmc_ = new LidarMotionCalibrator(laser_frame_,odom_frame_);
//new一个GridSlamProcessor对象，也是ros和gridslam的连接
gsp_ = new GMapping::GridSlamProcessor(); //这里需要跳进去看，第一次不要看
//new一个TransformBroadcaster对象，用来发布map和odom的关系
tfB_ = new tf::TransformBroadcaster();
got_first_scan_ = false;
got_map_ = false;
private_nh_.param("transform_publish_period", transform_publish_period_, 0.05);
double tmp;
if(!private_nh_.getParam("map_update_interval", tmp))//地图更新的秒数间隔
tmp = 5.0;
map_update_interval_.fromSec(tmp);
//GMapping算法本身使用的参数
maxUrange_ = 0.0; maxRange_ = 0.0;
if(!private_nh_.getParam("particles", particles_))
particles_ = 30;
if(!private_nh_.getParam("xmin", xmin_))
xmin_ = -100.0;
if(!private_nh_.getParam("ymin", ymin_))
ymin_ = -100.0;
if(!private_nh_.getParam("xmax", xmax_))
xmax_ = 100.0;
if(!private_nh_.getParam("ymax", ymax_))
ymax_ = 100.0;
if(!private_nh_.getParam("delta", delta_))
delta_ = 0.05;
if(!private_nh_.getParam("occ_thresh", occ_thresh_))
occ_thresh_ = 0.25;
if(!private_nh_.getParam("minimumScore", minimum_score_))
minimum_score_ = 0;
if(!private_nh_.getParam("sigma", sigma_))
sigma_ = 0.05;
if(!private_nh_.getParam("kernelSize", kernelSize_))
kernelSize_ = 1;
if(!private_nh_.getParam("lstep", lstep_))//默认一个栅格距离大小变化
lstep_ = delta_;
if(!private_nh_.getParam("astep", astep_))
astep_ = delta_;
if(!private_nh_.getParam("iterations", iterations_))
iterations_ = 5;
if(!private_nh_.getParam("lsigma", lsigma_))
lsigma_ = 0.075;
if(!private_nh_.getParam("ogain", ogain_))
ogain_ = 3.0;
if(!private_nh_.getParam("lskip", lskip_))//计算scan与地图匹配得分时，跳过的部分，默认为0
lskip_ = 0;
if(!private_nh_.getParam("srr", srr_))
srr_ = 0.1;
if(!private_nh_.getParam("srt", srt_))
srt_ = 0.2;
if(!private_nh_.getParam("str", str_))
str_ = 0.1;
if(!private_nh_.getParam("stt", stt_))
stt_ = 0.2;
if(!private_nh_.getParam("linearUpdate", linearUpdate_))
linearUpdate_ = 1.0;
if(!private_nh_.getParam("angularUpdate", angularUpdate_))
angularUpdate_ = 0.5;
if(!private_nh_.getParam("temporalUpdate", temporalUpdate_))
temporalUpdate_ = -1.0;
if(!private_nh_.getParam("resampleThreshold", resampleThreshold_))
resampleThreshold_ = 0.5;
if(!private_nh_.getParam("tf_delay", tf_delay_))
tf_delay_ = transform_publish_period_;
ROS_DEBUG("MySlamGMapping::init finish");
}前面是做参数初始化，map、odom、laser坐标系的初始化，以及scan话题初始化
//new一个激光雷达运动畸变的对象
lmc_ = new LidarMotionCalibrator(laser_frame_,odom_frame_);初始化对象，去畸变对象、
tfB_是用来发布map到odom之间的tf变换，与上面map_to_odom的区别是什么呢?
tf::Transform map_to_odom_：存坐标变换数据的 “容器”
tf::TransformBroadcaster *tfB_：广播器，负责把变换发给整个 ROS 系统
作用只有一个： 把你存在 tf::Transform 里的坐标变换，打包成 ROS 消息，发布到 /tf 话题上。 其他节点（雷达、导航、可视化 Rviz）订阅 /tf 就能读取坐标系关系。两者核心区别（一句话分清）
map_to_odom_ = 数据盒子
只存平移、旋转数值，纯内存变量，其他节点看不到。
tfB_ = 快递员
把盒子里的数据打包广播，让全系统所有节点读取坐标系变换。got_first_scan_表示是否得到了第一帧，我们要决定是否来进行地图初始化（地图需要第一帧来进行初始化）
got_map_表示是否拿到了第一帧初始化好的地图
如果没有拿到第一帧，以及第一帧初始化好的地图的话，后续的帧都不会进行处理，而是反复的等待地图
private_nh_.param("transform_publish_period", transform_publish_period_, 0.05);transform_publish_period_表示的是时间差，多久发布一次map到odom的tf变换
double tmp;
if(!private_nh_.getParam("map_update_interval", tmp))//地图更新的秒数间隔
tmp = 5.0;
map_update_interval_.fromSec(tmp);这里的map_update_interval表示的是地图更新的时间间隔，单位是秒
后面就是地图相关参数，从launch的参数读取
地图参数
particles初始化粒子：每一次得到一个大致位置（odom发出的）的时候，我们就要在这个大致位置附近初始化很多有可能的位姿，这里每一个位姿称之为一个粒子。每一次初始化多少个粒子就代表的这个意思。
地图尺寸xmin ymin xmax ymax，后面会进行扩展，在launch里面所以设置的是5m到-5m之间
地图分辨率delta，最终形成的栅格地图的分辨率，每一个像素就是一个栅格，每一个栅格表示物理世界的表示多少米，
occ_thresh占用概率，超过这个概率，才是被占用
下面是scan_match相关参数
scan_match参数
minimumScore匹配阈值，
sigma粒子得分，每一个粒子就是每一个位置，表示每一个位置和地图的匹配程度得分
kernelSize搜索框，在找到最优粒子（位置）的时候，会向左右前后四个方向进行位置偏移会不会得到最优位置，这个是偏移程度
linearUpdate angularUpdate更新的情况，当前激光帧和现有地图距离比较远的时候才进行地图更新
初始化讲完了，下面讲解SLAM相关的函数
开始实时SLAM
startLiveSlam函数()
void MySlamGMapping::startLiveSlam()
{
sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
ss_ = node_.advertiseService("dynamic_map", &MySlamGMapping::mapCallback, this);
{
//用message_filters来订阅scan_topic_，进而初始化scan_filter_，
scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, scan_topic_, 5);
//tf::MessageFilter，订阅激光数据同时和odom_frame之间转换时间同步
scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
//scan_filter_注册回调函数laserCallback
scan_filter_->registerCallback(boost::bind(&MySlamGMapping::laserCallback, this, _1));
ROS_DEBUG("Start Subscribe LaserScan & odom!!!");
}
/*发布map到odom的转换关系的线程*/
transform_thread_ = new boost::thread(boost::bind(&MySlamGMapping::publishLoop, this, transform_publish_period_));
ROS_DEBUG("Start transform_thread ");
}发布地图，
map_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
map_info_pub_ = nh_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);gmapping最终输出的建图结果就是一张栅格地图图片+一个对应的yaml配置文件，如下:
可以理解第一个发布的是pgm格式的，第二个发布的是yaml格式类型
下面是代码段
Scan和Odom同步
收到一帧LaserScan后，filter 会拿着这帧激光的时间戳，去 TF 里查询：在该时刻，激光坐标系 → odom 坐标系 的 TF 变换是否已经可用
- TF 存在、查询成功 → 放行消息，触发回调
- TF 还没收到 / 延迟、查不到变换 → 缓存消息，等待一段时间，超时直接丢弃，不进回调
{
//用message_filters来订阅scan_topic_，进而初始化scan_filter_，
scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, scan_topic_, 5);
//tf::MessageFilter，订阅激光数据同时和odom_frame之间转换时间同步
scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
//scan_filter_注册回调函数laserCallback
scan_filter_->registerCallback(boost::bind(&MySlamGMapping::laserCallback, this, _1));
ROS_DEBUG("Start Subscribe LaserScan & odom!!!");
}message_filter的使用
message_filters + tf::MessageFilter 做时间 + TF 坐标同步，经典用法
首先是实例化了一个订阅，订阅scan话题，跟不同的订阅不同的是，增加了一个消息过滤message_filter，消息过滤器message_filters类似一个消息缓存，当消息到达消息过滤器的时候，可能并不会立即输出，而是在稍后的时间点里满足一定条件下输出。
所以这里第一个是订阅器的实例化，绑定了雷达scan话题
第二个是过滤器的实例化，同步处理，
对订阅的scgn话题按照指定tf对消息进行过滤 (话题订阅器，监听坐标变换，消息将会被转换到的目的帧，queue_size)，同步上了，就进入第三行的回调函数
实时发布map-odom
new boost::thread对线程实例化
/*发布map到odom的转换关系的线程*/
transform_thread_ = new boost::thread(boost::bind(&MySlamGMapping::publishLoop, this, transform_publish_period_));void MySlamGMapping::publishLoop(double transform_publish_period)
//发布map->odom的转换关系
void MySlamGMapping::publishLoop(double transform_publish_period)
{
//发布时间间隔不正确，直接返回
if(transform_publish_period == 0)
return;
ros::Rate r(1.0 / transform_publish_period);
while(ros::ok())
{
publishTransform(); //发布
r.sleep(); //延时r ms
}
}while循环持续发布
MySlamGMapping::publishLoop(double transform_publish_period)
//发布map到odom的转换关系
void MySlamGMapping::publishTransform()
{
// 加锁（因为会对 map_to_odom_ 内容进行更新）
map_to_odom_mutex_.lock();
//默认情况下 tf_delay_ = transform_publish_period_;
//默认情况下ros::Duration(tf_delay_)时间长度，等于 r.sleep();的时间长度
ros::Time tf_expiration = ros::Time::now() + ros::Duration(tf_delay_);//这个没搞明白为啥要加这一点时间，感觉没有必要
// tf::StampedTransform是ROS中用于表示带有时间戳的坐标变换的类。
// （坐标的具体变换，这个变换的时间戳，表示这个变换发生的时间，父坐标系名称，子坐标系名称）
tfB_->sendTransform( tf::StampedTransform (map_to_odom_, tf_expiration, map_frame_, odom_frame_));
map_to_odom_mutex_.unlock();
}持续发布map_to_odom_，发布前需要上锁操作
接下来就看回调函数
回调函数--Scan和Odom的同步
1. `MySlamGMapping::laserCallback`：ROS 激光回调入口
2. `LidarMotionCalibrator::LidarMotionCalibrator`：类构造函数
3. `LidarMotionCalibrator::lidarCalibration`：畸变矫正总调度（分段逻辑）
4. `LidarMotionCalibrator::lidarMotionCalibration`：单段内部逐点矫正核心计算MySlamGMapping::laserCallback
输入参数
const sensor_msgs::LaserScan::ConstPtr& scan ----ROS 原始激光扫描消息
输出：无返回值
内部成员变量laser_ranges_、laser_angles_被填充，传给lidarCalibration；这两个 vector 会被去畸变函数直接修改（引用）。
首先拿到激光雷达数据，需要进行一个运动去畸变处理
//每当到达一帧scan数据，就将调用laserCallback函数
void MySlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
//===========================激光雷达数据运动畸变处理部分===============================
ros::Time startTime, endTime;
//一帧scan的时间戳就代表一帧数据的开始时间
startTime = scan->header.stamp;// 一帧scan的时间戳就代表一帧数据的开始时间(第一个激光束的时间)
sensor_msgs::LaserScan laserScanMsg = *scan;// 拷贝scan数据
int beamNum = laserScanMsg.ranges.size();// 激光束数量
// endTime = startTime + 每束之间的时间差*激光束数量
//根据激光时间分割和激光束个数的乘积+startTime得到endTime（最后一束激光束的时间）
endTime = startTime + ros::Duration(laserScanMsg.time_increment * beamNum);
laser_ranges_.clear();
laser_angles_.clear();
//拷贝scan数据到laser_ranges_,laser_angles_
double lidar_dist,lidar_angle;
for(int i = 0; i < beamNum;i++)
{
lidar_dist = laserScanMsg.ranges[i];//单位米
lidar_angle = laserScanMsg.angle_min + laserScanMsg.angle_increment * i;//单位弧度
laser_ranges_.push_back(lidar_dist);
laser_angles_.push_back(lidar_angle);
}
//激光雷达运动畸变去除
lmc_->lidarCalibration(laser_ranges_,laser_angles_,startTime,endTime,&tf_);
------------------
}1.首先定义两个变量startTime endTime表示激光束的开始和结束时间，一帧scan的时间戳就是开始时间，可以直接获取，
2.拷贝scan数据，laserScanMsg 是拷贝的激光数据，
3.获取一帧scan里激光束总数beamNum，这也可以从scan里面拿到，
4.计算帧结束时间endTime:每一个激光束是有时间差的，时间差*beamNum + startTime
5.清空laser_rangers_、laser_angles_
std::vector<double> laser_ranges_; //存储每一个激光点距离
std::vector<double> laser_angles_; //存储每一个激光点的角度6.循环每一束，填充距离、角度数组
7.调用 `lmc_->lidarCalibration(laser_ranges_, laser_angles_, startTime, endTime, &tf_)`，执行畸变矫正
//激光雷达运动畸变去除
lmc_->lidarCalibration(laser_ranges_,laser_angles_,startTime,endTime,&tf_);在头文件定义了lmc
LidarMotionCalibrator* lmc_; //激光雷达运动畸变去除对象在初始化的时候实例化了lmc_
8.函数返回；后续 SLAM 使用矫正完成的laser_ranges_、laser_angles_
这里看一下类LidarMotionCalibrator
LidarMotionCalibrator::LidarMotionCalibrator
作用：初始化运动畸变器对象，保存坐标系名字。
输入
std::string scan_frame_name：激光雷达坐标系名，如laser_link
std::string odom_name：里程计坐标系名，如odom
输出：无输出
给成员变量赋值：scan_frame_name_、odom_name_
//构造函数
LidarMotionCalibrator::LidarMotionCalibrator(std::string scan_frame_name,std::string odom_name)
{
scan_frame_name_ = scan_frame_name;
odom_name_ = odom_name;
}雷达运动去畸变LidarMotionCalibrator::lidarCalibration
类里面的方法
作用：本函数不做坐标变换计算，只做分段、TF 查询、调度子函数。
把一帧激光按时间切分成多段（每段最大 5ms）
每段的起止时刻调用 TF 获取雷达在 odom 下的位姿
调用lidarMotionCalibration对每一段执行逐点矫正输入：
| 参数 | 类型 | 说明 |
|---|---|---|
ranges |
std::vector<double>& |
【输入输出】原始激光距离数组，引用，函数内会被修改 |
angles |
std::vector<double>& |
【输入输出】原始激光角度数组，引用，函数内会被修改 |
startTime |
ros::Time |
整帧第一束激光时间戳 |
endTime |
ros::Time |
整帧最后一束激光时间戳 |
tf_ |
tf::TransformListener* |
TF 监听器指针，用于查询位姿 |
输出
返回值：void
输出（引用改写）：ranges、angles，完成畸变矫正后的极坐标数组
//激光雷达运动畸变去除函数
void LidarMotionCalibrator::lidarCalibration(std::vector<double>& ranges,std::vector<double>& angles,ros::Time startTime,ros::Time endTime,tf::TransformListener * tf_)
{
//激光束的数量
int beamNumber = ranges.size();
//分段时间间隔，单位us
int interpolation_time_duration = 5 * 1000;//单位us
tf::Stamped<tf::Pose> frame_base_pose; //基准坐标系原点位姿
tf::Stamped<tf::Pose> frame_start_pose;
tf::Stamped<tf::Pose> frame_mid_pose;
double start_time = startTime.toSec() * 1000 * 1000; //*1000*1000转化时间单位为us
double end_time = endTime.toSec() * 1000 * 1000;
double time_inc = (end_time - start_time) / beamNumber; //每相邻两束激光数据的时间间隔，单位us
//得到start_time时刻，laser_link在里程计坐标下的位姿，存放到frame_start_pose
if(!getLaserPose(frame_start_pose, ros::Time(start_time /1000000.0), tf_))
{
ROS_WARN("Not Start Pose,Can not Calib");
return ;
}
//分段个数计数
int cnt = 0;
//当前插值的段的起始坐标
int start_index = 0;
//默认基准坐标系就是第一个位姿的坐标系
frame_base_pose = frame_start_pose;
for(int i = 0; i < beamNumber; i++)
{
//按照分割时间分段，分割时间大小为interpolation_time_duration
double mid_time = start_time + time_inc * (i - start_index);
//这里的mid_time、start_time多次重复利用
if(mid_time - start_time > interpolation_time_duration || (i == beamNumber - 1))
{
cnt++;
//得到临时结束点的laser_link在里程计坐标系下的位姿，存放到frame_mid_pose
if(!getLaserPose(frame_mid_pose, ros::Time(mid_time/1000000.0), tf_))
{
ROS_ERROR("Mid %d Pose Error",cnt);
return ;
}
//计算该分段需要插值的个数
int interp_count = i + 1 - start_index ;
//对本分段的激光点进行运动畸变的去除
lidarMotionCalibration(frame_base_pose, //对于一帧激光雷达数据，传入参数基准坐标系是不变的
frame_start_pose, //每一次的传入，都代表新分段的开始位姿，第一个分段，根据时间戳，在tf树上获得，其他分段都为上一段的结束点传递
frame_mid_pose, //每一次的传入，都代表新分段的结束位姿，根据时间戳，在tf树上获得
ranges, //引用对象，需要被修改的距离数组
angles, //引用对象，需要被修改的角度数组
start_index, //每一次的传入，都代表新分段的开始序号
interp_count); //每一次的传入，都代表该新分段需要线性插值的个数
//更新时间
start_time = mid_time;//⚠️ start_time被改写，变成这一段的结束时间，也就是下一段的起始时间
start_index = i;
frame_start_pose = frame_mid_pose; //将上一分段的结束位姿，传递为下一分段的开始位姿
}
}
}1.获取激光线束，和定义单段最大时间5000μs（5ms）
2.把起始时间和终止时间转成微秒
3.计算每一束激光时间间隔time_inc（μs）
4.调用类方法getLaserPose()获取帧起始时刻雷达在odom位姿frame_start_pose,如果 TF 查不到位姿，直接 return，放弃本帧矫正
5.frame_base_pose = frame_start_pose：报错基准位姿（每一帧 真头时刻雷达在odom的位姿），所有的点最终都会对齐到此坐标系
6.初始化分段变量：start_index = 0,分段计数cnt=0
7.for循环遍历全部激光束i:
计算当前点相当于本段起点时间mid_time
判断条件 mid_time - start_time > 5ms或者遍历到最后一束
--执行分段：
调用getLaserPose()获取本段结束时刻雷达 odom 位姿frame_mid_pose；失败直接 return
计算本段点数量 interp_count
调用 lidarMotionCalibration ()，传入本段参数，完成本段所有点矫正
更新分段起始时间、起始索引；把本段结束位姿赋值给下一段起始位姿8.for 循环结束，函数返回，ranges/angles 已经全部矫正完成。
大坑：这里变量名很迷惑
刚进入lidarCalibration，start_time = 整帧第一束激光的时间（us）
每完成一个分段，start_time = mid_time，被重新赋值！ 循环里面的start_time不再代表整帧开头，而是当前这个分段的起始时刻（us）。 start_index是当前分段第一个点在 ranges 数组的下标。
为什么叫mid_time?名字容易误导！！！
名字叫 mid_time，并不是 “中间时刻”，是本段末尾点的时刻。 作者命名不好，容易以为是中点。 实际含义：当前 i 号激光束的真实采集时间（us）
下面举例子：
假设10Hz 雷达，一帧 1000 束激光，演示一下for循环
雷达频率：10Hz，一帧扫描周期 = 100 ms = 100000 μs
每帧光束数：1000 束
每束时间间隔：time_inc = 100μs
分段阈值：interpolation_time_duration = 5 ms = 5000 μs
start_time = 0 us // 当前分段起始时间
start_index = 0 // 当前分段第一个光束下标
frame_start_pose = 帧头TF位姿
循环 i 从 0 ~ 999（全部 1000 束）
\(mid\_time = start\_time + time\_inc \times (i-start\_index)\)
判断条件： mid_time - start_time > 5000 || i == 999
第一段处理
start_time=0，start_index=0，time_inc=100us
i=0：mid_time = 0 +100*(0‑0)=0 μs；差值 0 ≤5000，不触发
……
i=50：mid_time =0 +100*(50‑0)=**5000 μs**
mid_time‑start_time = 5000`，条件`>5000`不成立，不触发分段
i=51：mid_time =0 +100*(51‑0)=**5100 μs**
mid_time‑start_time =5100 > 5000` ✔触发分段条件
第一段执行动作
1. 当前 i=51，mid_time = 5100 us 作为本段结束时刻
2. 调用getLaserPose( frame_mid_pose, ros::Time(5100e‑6) , tf_ )，查询 5100us 时刻雷达 odom 位姿。失败直接 return 放弃整帧。
3. 本段光束数量 interp_count = i - start_index + 1 =51‑0+1 =52个点：下标 0 ~ 51
4. 调用lidarMotionCalibration，传入：
- frame_base_pose：帧头基准位姿不变
- frame_start_pose：本段起点位姿（0us 时刻）
- frame_mid_pose：本段终点位姿（5100us 时刻）
- start_index=0，beam_number=52
函数内部：对 0‑51 号全部激光束，在 0us~5100us 之间插值做畸变矫正。
5.更新分段变量，准备下一段：
start_time = mid_time; // start_time =5100 us，下一段的起点时间
start_index = i; // start_index =51，下一段第一个点下标
frame_start_pose = frame_mid_pose; //下一段起点位姿=本段终点位姿
第二段处理
现在状态：
start_time=5100 us，start_index=51，time_inc=100us
继续循环 i=52
(mid_time = 5100 + 100*(i‑51))
i=52：mid_time = 5100 + 100*(1)=5200；差值 100 ≤5000
i=53：mid_time=5300；差值 200 ≤5000
……
i=102：mid_time = 5100 + 100*(102‑51)= 5100+5100 =10200
差值：10200‑5100 =5100 >5000 ✔触发分段
1. 本段结束时刻 mid_time=10200us；调用 getLaserPose 拿该时刻 odom 位姿 frame_mid_pose
2. interp_count =102‑51+1 =52；点下标 **51~102**
3. lidarMotionCalibration 矫正 51‑102
4. 更新变量：
start_time=10200，start_index=102`，frame_start_pose = frame_mid_pose
✅规律：每一段大概 52 个光束，时间跨度约 5.1ms，略大于设定 5ms 阈值。
为什么不是刚好 5ms？因为光束是离散的，不能切在光束中间，只能在某一束 i 的位置切分。
---
持续循环下去
每一轮：
每 52 束触发一次分段；
查询一次 TF 获取本段终点位姿；
执行本段点矫正；
更新 start_time、start_index、frame_start_pose。
一直走到 i=999（最后一束光束）
最后一段（i=999，命中 i==beamNumber‑1条件）
假设此时：
start_time =97900 us，start_index = 948
i=999
mid_time = 97900 +100*(999‑948) = 97900 + 5100 =103000 us
触发条件：i ==999，到达最后一束。
1. getLaserPose查询 mid_time=103000us 时刻雷达 odom 位姿 frame_mid_pose
2. interp_count =999‑948+1 =52，点下标948‑999
3. lidarMotionCalibration 矫正这最后一批点
4. 更新分段变量（for 循环马上结束，更新已经无意义）
for 循环结束，整帧 1000 束全部分段矫正完毕。
总共有多少段？1000 束 ÷52 束每段 ≈ 19‑20 个分段。
每一段都调用一次 getLaserPose 查 TF；每一段调用一次 lidarMotionCalibration 做本段点矫正。关键要点总结（结合例子）
-
不是每一束激光都去查 TF！ 如果每束 1000 个点都调用
getLaserPose查询 TF，10Hz 下一秒就要查 10000 次 TF，CPU 压力巨大。 ✅方案：按时间 5ms 分段，只查每一段的起点、终点两个时刻的 TF 位姿；段内几百个点，代码内部做线性插值，不需要再查 TF。第一段：起点 0us，终点 5100us，查 2 次 TF；段内 52 个点靠插值。
start_time变量一直在被改写！ 不是固定整帧 0us，每分完一段，被赋值为本段结束mid_time，作为下一段分段的起点。- 分段切分只能落在光束 i 上，不能切在两束激光之间，所以实际时间跨度会略微大于 5ms 阈值。
- 边界：最后一束 i=999 强制触发分段，保证最后剩余的点一定被处理，不会漏掉尾部光束。
- 上层
lidarMotionCalibration拿到本段起止两个 odom 位姿，在段内部，根据每个点在本段内的时间比例，做平移 lerp、旋转 slerp 插值，算出该束激光发射瞬间雷达的真实位姿。
插值函数LidarMotionCalibrator::lidarMotionCalibration
外层：lidarCalibration 按 5ms 时间阈值把整帧激光切分成多段,查询每一段起止时刻的 TF 位姿
循环调用 lidarMotionCalibration，把每一段送进去插值，每一段只调用一次TF查询获得laser在odom坐标系的位姿
内层(本函数)：lidarMotionCalibration 单段内执行：位姿插值 + 坐标变换
输入：当前这一小段的起点、终点雷达位姿
内部：对本段内每个激光点，按序号权重 beam_step*i 做 Lerp (平移)+Slerp (姿态) → 得到该点瞬时雷达位姿
lidarMotionCalibration 的职责：接收已经切好的一小段的首尾位姿，在本段内部做位姿插值 + 坐标变换 + 畸变修正。
1. 根据分段起止位姿，对每个点时间比例插值，得到该束激光采集时刻雷达真实 odom 位姿
2. 将原始极坐标点转到 odom 世界坐标系
3. 将 odom 下的点反向变换回**帧头基准雷达坐标系 frame_base_pose**
4. 重新计算距离、角度，写回 ranges、angles 数组输入：
| 参数 | 类型 | 说明 |
|---|---|---|
frame_base_pose |
tf::Stamped<tf::Pose> |
【整帧帧头时刻雷达odom位姿】矫正完成后所有点要对齐到这个坐标系 |
frame_start_pose |
tf::Stamped<tf::Pose> |
本分段起始时刻雷达 odom 位姿 |
frame_end_pose |
tf::Stamped<tf::Pose> |
本分段结束时刻雷达 odom 位姿 |
ranges |
std::vector<double>& |
【in/out】全局激光距离数组，引用修改 |
angles |
std::vector<double>& |
【in/out】全局激光角度数组，引用修改 |
startIndex |
int |
本段第一个点在数组中的下标 |
beam_number |
int& |
本段包含多少个激光点（引用） |
输出
返回：void
直接改写ranges[startIndex ... startIndex+beam_number‑1]、angles[...]，得到矫正后的极坐标。
//根据传入参数，对任意一个分段进行插值
void LidarMotionCalibrator::lidarMotionCalibration(tf::Stamped<tf::Pose> frame_base_pose,tf::Stamped<tf::Pose> frame_start_pose,tf::Stamped<tf::Pose> frame_end_pose,
std::vector<double>& ranges,std::vector<double>& angles,
int startIndex,int& beam_number)
{
//beam_step插值函数所用的步长
double beam_step = 1.0 / (beam_number-1);
//该分段中，在里程计坐标系下，laser_link位姿的起始角度 和 结束角度，四元数表示
tf::Quaternion start_angle_q = frame_start_pose.getRotation();
tf::Quaternion end_angle_q = frame_end_pose.getRotation();
//该分段中，在里程计坐标系下，laser_link位姿的起始角度、该帧激光数据在里程计坐标系下基准坐标系位姿的角度，弧度表示
double start_angle_r = tf::getYaw(start_angle_q);
double base_angle_r = tf::getYaw(frame_base_pose.getRotation());
//该分段中，在里程计坐标系下，laser_link位姿的起始位姿、结束位姿，以及该帧激光数据在里程计坐标系下基准坐标系的位姿
tf::Vector3 start_pos = frame_start_pose.getOrigin(); start_pos.setZ(0);
tf::Vector3 end_pos = frame_end_pose.getOrigin(); end_pos.setZ(0);
tf::Vector3 base_pos = frame_base_pose.getOrigin(); base_pos.setZ(0);
//临时变量
double mid_angle;
tf::Vector3 mid_pos;
tf::Vector3 mid_point;
double lidar_angle, lidar_dist;
//beam_number为该分段中需要插值的激光束的个数
for(int i = 0; i< beam_number;i++)
{
//得到第i个激光束的角度插值，线性插值需要步长、起始和结束数据,与该激光点坐标系和里程计坐标系的夹角
mid_angle = tf::getYaw(start_angle_q.slerp(end_angle_q, beam_step * i)); //slerp（）角度线性插值函数
//得到第i个激光束的近似的里程计位姿线性插值
mid_pos = start_pos.lerp(end_pos, beam_step * i); //lerp（），位姿线性插值函数
//如果激光束距离不等于无穷,则需要进行矫正
if( std::isinf(ranges[startIndex + i]) == false)
{
//取出该分段中该束激光距离和角度
lidar_dist = ranges[startIndex+i];
lidar_angle = angles[startIndex+i];
//在当前帧的激光雷达坐标系下，该激光点的坐标 （真实的、实际存在的、但不知道具体数值）
double laser_x,laser_y;
laser_x = lidar_dist * cos(lidar_angle);
laser_y = lidar_dist * sin(lidar_angle);
//该分段中的该激光点，变换的里程计坐标系下的坐标，（这里用插值的激光雷达坐标系近似上面哪个真实存在的激光雷达坐标系，因为知道数值，可以进行计算）
double odom_x,odom_y;
double cos_ , sin_;
cos_ = cos(mid_angle);
sin_ = sin(mid_angle);
odom_x = laser_x * cos_ - laser_y * sin_ + mid_pos.x();
odom_y = laser_x * sin_ + laser_y * cos_ + mid_pos.y();
mid_point.setValue(odom_x, odom_y, 0);
//在里程计坐标系下，基准坐标系的参数
double x0,y0,a0,s,c;
x0 = base_pos.x();
y0 = base_pos.y();
a0 = base_angle_r;
s = sin(a0);
c = cos(a0);
//把该激光点都从里程计坐标系下，变换的基准坐标系下
double tmp_x,tmp_y;
tmp_x = mid_point.x()*c + mid_point.y()*s - x0*c - y0*s;
tmp_y = -mid_point.x()*s + mid_point.y()*c + x0*s - y0*c;
mid_point.setValue(tmp_x,tmp_y,0);
//然后计算该激光点以起始坐标为起点的 dist angle
double dx,dy;
dx = (mid_point.x());
dy = (mid_point.y());
lidar_dist = sqrt(dx*dx + dy*dy);
lidar_angle = atan2(dy,dx);
//激光雷达被矫正
ranges[startIndex+i] = lidar_dist;
angles[startIndex+i] = lidar_angle;
}
//如果等于无穷,则随便计算一下角度
else
{
double tmp_angle;
lidar_angle = angles[startIndex+i];
//里程计坐标系的角度
tmp_angle = mid_angle + lidar_angle;
tmp_angle = tfNormalizeAngle(tmp_angle);
//如果数据非法 则只需要设置角度就可以了。把角度换算成start_pos坐标系内的角度
lidar_angle = tfNormalizeAngle(tmp_angle - start_angle_r);
angles[startIndex+i] = lidar_angle;
}
}
}1.计算插值步长 beam_step =1.0/(beam_number‑1)
//beam_step插值函数所用的步长
double beam_step = 1.0 / (beam_number-1);2.取出分段起点、终点雷达在odom下的旋转四元素，用于球面插值slerp
取出分段起止雷达 xy 位置；取出基准 frame_base_pose 的 xy、yaw
//该分段中，在里程计坐标系下，laser_link位姿的起始角度 和 结束角度，四元数表示
tf::Quaternion start_angle_q = frame_start_pose.getRotation();
tf::Quaternion end_angle_q = frame_end_pose.getRotation();
提取 yaw 角；
start_angle_r：分段起点雷达 yaw 角 (rad)，odom 坐标系
base_angle_r：整帧帧头基准雷达的 yaw 角（目标坐标系姿态）
//该分段中，在里程计坐标系下，laser_link位姿的起始角度、该帧激光数据在里程计坐标系下基准坐标系位姿的角度，弧度表示
double start_angle_r = tf::getYaw(start_angle_q);
double base_angle_r = tf::getYaw(frame_base_pose.getRotation());取出三个位姿的 xy 平移；z 全部置 0，忽略高度。
start_pos：分段起点雷达 odom 坐标
end_pos：分段终点雷达 odom 坐标
base_pos：整帧帧头基准雷达 odom 坐标（目标坐标系原点）
//该分段中，在里程计坐标系下，laser_link位姿的起始位姿、结束位姿，以及该帧激光数据在里程计坐标系下基准坐标系的位姿
tf::Vector3 start_pos = frame_start_pose.getOrigin(); start_pos.setZ(0);
tf::Vector3 end_pos = frame_end_pose.getOrigin(); end_pos.setZ(0);
tf::Vector3 base_pos = frame_base_pose.getOrigin(); base_pos.setZ(0);临时变量：
mid_angle：段内第 i 束激光发射时刻，插值得到雷达 yaw 角（odom 坐标系）
mid_pos：段内第 i 束激光发射时刻，插值得到雷达 xy 位置（odom 坐标系）
mid_point：存放激光点世界 (odom) 坐标，之后变换回基准雷达坐标系
lidar_dist / lidar_angle：矫正后的极坐标
3.段内 for 循环，遍历本段每一束激光
利用球面插值slerp和线性插值lerp得到本段第i个激光束的角度插值mid_angle和位姿插值mid_angle
//旋转：四元数，球面插值slerp
//得到第i个激光束的角度插值，线性插值需要步长、起始和结束数据,与该激光点坐标系和里程计坐标系的夹角
mid_angle = tf::getYaw(start_angle_q.slerp(end_angle_q, beam_step * i)); //slerp（）角度线性插值函数
//平移：Vector3位置，普通线性插值lerp
//得到第i个激光束的近似的里程计位姿线性插值
mid_pos = start_pos.lerp(end_pos, beam_step * i); //lerp（），位姿线性插值函数为什么：平移向量用lerp，旋转四元数用slerp？？
平移代表三维欧氏空间里的坐标点 \((x,y,z)\)。 欧几里得空间，两点之间最短路径就是直线。
lerp 公式：
\(P(t) = (1-t)\cdot P_0 + t\cdot P_1,\quad t\in[0,1]\)
- t=0 返回起点；t=1 返回终点；
- t 均匀变化，空间位置匀速直线移动。
举例子： 起点 \((0,0)\)，终点 \((10,0)\)；t=0.5，得到 \((5,0)\)，正好走到中间。完全符合物理世界机器人直线运动。
✅ 位置向量属于普通欧氏空间，直接线性插值完全正确，没有问题。
旋转（四元数 Quaternion）不能直接 lerp，要用 Slerp 球面插值
旋转不是普通三维向量。 三维空间任意旋转，用单位四元数表达；所有合法旋转全部落在四维单位超球面\(S^3\)上面。
如果对四元数直接做普通 Lerp 会发生什么？
\(q(t)=(1-t)q_0 + t q_1\)
- 插值出来的结果不再是单位四元数，不在球面上，跑到球体内部；非单位四元数不能代表正确旋转。
- t 均匀变化，角速度不是匀速：两头慢，中间转得飞快。机器人匀速旋转，插值出来角度忽快忽慢。
- 大角度旋转，会走错误路径，甚至反向绕一大圈。
类比地球：从北京飞到纽约，lerp 相当于钻穿地球内部直线，不是地球表面航线；真实旋转必须沿着球面表面弧线走。Slerp 就是沿着球面大圆弧插值，也叫测地线插值。
Slerp 球面线性插值
\(q(t)=\frac{\sin((1-t)\theta)}{\sin\theta}q_0+\frac{\sin(t\theta)}{\sin\theta}q_1\)
\(\theta\)：起点旋转与终点旋转之间总角度。
- t 从 0 到 1 均匀增加 → 旋转角度匀速增加，角速度恒定。
- 始终保持单位四元数，始终在旋转球面上；走最短旋转路径。
在我们运动畸变代码中，隐含假设：在这 5ms 分段之内，机器人是匀速转动、匀速平移。
平移匀速 → Vector3::lerp 匹配匀速直线运动
旋转匀速 → Quaternion::slerp 匹配匀速转动
如果不用 slerp，大角度转动场景，每一束激光的雷达姿态算错，运动畸变矫正直接失效，点云会扭曲变形。
一个重要概念：位姿 Pose =【平移 Vector3】+【旋转 Quaternion】
一个完整 pose 不是单一向量，它是两套完全不同数学空间的数据拼起来的：
- 平移部分：欧氏空间 \(\mathbb R^3\) → lerp 线性插值
- 旋转部分：旋转群 SO (3)，对应四维单位球面 \(S^3\) → slerp 球面插值
❗没有直接对整个 pose 做插值的函数！ tf、ROS、大部分库都是拆开做： 位置用 lerp，姿态四元数用 slerp，之后再把插值得到的位置 + 旋转组装成中间位姿。 我们的
lidarMotionCalibration就是这么做的。
记住一句话： 位置在普通空间，走直线 lerp；旋转在球面上，走弧线 slerp。位姿插值必须拆开两部分分别插值。
四步和源代码一一对应（有效点分支）
顺带看代码的完整流程 只看 if(std::isinf(... )==false) 内部的代码。
第一步（插值）：该束激光发射时刻 雷达本体在 odom 的位姿 mid_pos，mid_angle
//1.旋转四元数球面插值
mid_angle = tf::getYaw( start_angle_q.slerp(end_angle_q, t) );
//2.平移向量线性插值
mid_pos = start_pos.lerp(end_pos, t);
//组合得到该束激光发射时刻雷达完整odom位姿(mid_pos,mid_angle)
//再用这个位姿，做激光点坐标变换，完成畸变矫正
注意：代码里 slerp 返回四元数，再用
tf::getYaw()提取出 2D 平面的 yaw 角；因为是地面机器人，只关心绕 Z 轴旋转。
这只是第一步，是把这一束雷达坐标系在 odom 求出来了，这一步算的是【雷达自己】的位置姿态，不是障碍物点！
每一个激光点都有自己专属的瞬时雷达位姿！得到了这一束激光发射瞬间雷达在 odom 下的位姿 (mid_pos,mid_angle)。
第二步障碍物点：瞬时雷达坐标系 → odom 世界坐标系
原始激光数据 r,θ：是障碍物相对于这个瞬时雷达的局部极坐标。
-
极坐标转局部笛卡尔：
laser_x, laser_y（障碍物相对于瞬时雷达） -
使用
mid_pos、mid_angle做旋转平移，得到odom_x,odom_y✅输出：障碍物在 odom 里程计世界里面真实坐标 👉对象：障碍物点。
到这里，我们终于知道障碍物真实世界在哪。
// --------第二步：瞬时雷达坐标系 → odom世界坐标--------
//取出该分段中该束激光距离和角度
//读取原始极坐标（相对于瞬时雷达）
lidar_dist = ranges[startIndex+i];
lidar_angle = angles[startIndex+i];
//在当前帧的激光雷达坐标系下，该障碍点的坐标
//极坐标转笛卡尔坐标 x=Rcosθ y=Rsinθ
// (lidar_dist,lidar_angle) -> (laser_x, laser_y)
double laser_x,laser_y;
laser_x = lidar_dist * cos(lidar_angle);
laser_y = lidar_dist * sin(lidar_angle);
//利用mid_pos、mid_angle做旋转平移，变换到odom世界
double odom_x,odom_y;
double cos_ , sin_;
cos_ = cos(mid_angle);
sin_ = sin(mid_angle);
// --------旋转部分--------
rot_x = laser_x * cos_ - laser_y * sin_;
rot_y = laser_x * sin_ + laser_y * cos_;
// --------再加上平移--------
odom_x = rot_x + mid_pos.x();
odom_y = rot_y + mid_pos.y();
mid_point.setValue(odom_x, odom_y, 0);
(laser_x, laser_y)：障碍物点相对于雷达自身的坐标（雷达坐标系）
雷达本身转了一个角度 \(\boldsymbol{\theta}=mid\_angle\) 旋转矩阵就是用来：把雷达坐标系上的点，旋转对齐到 odom 世界坐标系的朝向
二维旋转矩阵乘法展开
\(R= \begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix}\)
向量 \(\boldsymbol{P}_{local}= \begin{bmatrix}laser_x\\laser_y\end{bmatrix}\)
矩阵乘法：
\(R\cdot P_{local}= \begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix} \begin{bmatrix}laser_x\\laser_y\end{bmatrix}\)
矩阵乘法运算规则：
第一行结果 = 第一行每个元素 × 对应列元素，相加 第二行结果 = 第二行每个元素 × 对应列元素，相加
\(\begin{align} \text{rot}_x &= \cos\theta \cdot laser_x \;+\; (-\sin\theta)\cdot laser_y \\ &=\boldsymbol{laser_x\cdot \cos\theta \;-\; laser_y\cdot \sin\theta}\\[4pt] \text{rot}_y &= \sin\theta \cdot laser_x \;+\; \cos\theta\cdot laser_y\\ &=\boldsymbol{laser_x\cdot \sin\theta \;+\; laser_y\cdot \cos\theta} \end{align}\)
这两行，就是旋转运算的全部结果！还没有加上平移！
坐标系变换完整两步（非常关键）
变换公式 \(\boldsymbol{P_w=R\cdot P_l+T}\)，物理含义严格顺序：
- 旋转 \(R\cdot P_l\)： 将激光点从雷达的局部坐标轴，旋转到和 odom 世界坐标轴方向一致。
只改方向，原点还没有移动。
- 加上平移 T（mid_pos）： 把旋转后的整个点，搬运到雷达原点在世界中的真实位置。
mid_pos：发射该束激光瞬间，雷达中心点在 odom 的 XY 位置mid_angle：发射该束激光瞬间，雷达面朝哪个方向 (yaw)(laser_x,laser_y)：激光点相对于雷达中心，在雷达自己朝前方向下的位置
经过旋转 + 平移，就得到：该激光打到障碍物那一刻，障碍物在世界 odom 坐标系的真实坐标。
第三步：障碍物点：odom 世界坐标系 → 帧头时刻雷达局部坐标系
//帧头基准雷达在odom下的位姿 x0,y0,a0
double x0,y0,a0,s,c;
x0 = base_pos.x();
y0 = base_pos.y();
a0 = base_angle_r;
s = sin(a0);
c = cos(a0);
//逆坐标变换，odom点转到帧头雷达本体坐标系
double tmp_x,tmp_y;
tmp_x = mid_point.x()*c + mid_point.y()*s - x0*c - y0*s;
tmp_y = -mid_point.x()*s + mid_point.y()*c + x0*s - y0*c;
mid_point.setValue(tmp_x,tmp_y,0);
✔输出：mid_point(tmp_x,tmp_y)。障碍物点统一投影回整帧第一束激光时刻的雷达坐标系。 这一步是运动畸变矫正的核心，所有点统一参考时刻。对象：障碍物。
之前正向变换（雷达局部 → odom 世界）：
\(P_w = R\cdot P_l + T\)
- \(P_l\)：雷达局部坐标
- R：旋转矩阵，\(R=\begin{bmatrix}\cos a_0 & -\sin a_0 \\ \sin a_0 & \cos a_0\end{bmatrix}\)
- \(T=\begin{bmatrix}x_0 \\ y_0\end{bmatrix}\)：雷达原点在世界坐标的平移
现在我们已知世界点 \(P_w\)，反过来求局部点 \(P_l\)，解上面方程：
\(\begin{align*} P_w &= R P_l + T\\ R P_l &= P_w - T\\ P_l &= R^{-1}(P_w-T) \end{align*}\)
二维旋转矩阵的逆 = 转置矩阵
\(R^{-1}=R^\top= \begin{bmatrix} \cos a_0 & \sin a_0\\ -\sin a_0 & \cos a_0 \end{bmatrix}\)
这里的X_l就是tmp_x变量，Y_l就是tmp_y变量，也是障碍点在基准雷达坐标系下的笛卡尔坐标
X_w在这里就是mid_pose.x()，Y_w在这里就是mid_pose.y()是障碍点在odom世界坐标系下的笛卡尔坐标
把这个展开就是上面的式子了
第四步：笛卡尔 XY 转回极坐标，写回 ranges、angles 数组
double dx,dy;
dx = (mid_point.x());
dy = (mid_point.y());
lidar_dist = sqrt(dx*dx + dy*dy);
lidar_angle = atan2(dy,dx);
//覆盖原始数据，矫正完成
ranges[startIndex+i] = lidar_dist;
angles[startIndex+i] = lidar_angle;
输出：矫正后的极坐标，给 SLAM 使用
for(int i = 0; i< beam_number;i++)
{
// ==========第一步：插值得到该束发射时刻雷达odom位姿============
mid_angle = tf::getYaw(start_angle_q.slerp(end_angle_q, beam_step * i));
mid_pos = start_pos.lerp(end_pos, beam_step * i);
if( std::isinf(ranges[startIndex + i]) == false)
{
// --------第二步：瞬时雷达坐标系 → odom世界坐标--------
lidar_dist = ranges[startIndex+i];
lidar_angle = angles[startIndex+i];
double laser_x,laser_y;
laser_x = lidar_dist * cos(lidar_angle);
laser_y = lidar_dist * sin(lidar_angle);
double odom_x,odom_y;
double cos_ , sin_;
cos_ = cos(mid_angle);
sin_ = sin(mid_angle);
odom_x = laser_x * cos_ - laser_y * sin_ + mid_pos.x();
odom_y = laser_x * sin_ + laser_y * cos_ + mid_pos.y();
mid_point.setValue(odom_x, odom_y, 0);
// --------第三步：odom世界坐标 → 帧头基准雷达坐标系--------
double x0,y0,a0,s,c;
x0 = base_pos.x();
y0 = base_pos.y();
a0 = base_angle_r;
s = sin(a0);
c = cos(a0);
double tmp_x,tmp_y;
tmp_x = mid_point.x()*c + mid_point.y()*s - x0*c - y0*s;
tmp_y = -mid_point.x()*s + mid_point.y()*c + x0*s - y0*c;
mid_point.setValue(tmp_x,tmp_y,0);
// --------第四步：笛卡尔转回极坐标写回数组--------
double dx,dy;
dx = (mid_point.x());
dy = (mid_point.y());
lidar_dist = sqrt(dx*dx + dy*dy);
lidar_angle = atan2(dy,dx);
ranges[startIndex+i] = lidar_dist;
angles[startIndex+i] = lidar_angle;
}
else
{
//无效点（inf）分支，不执行4步，只修正光束角度
...
}
}
先理清 4 个关键坐标系（重中之重，看懂这个就看懂全部）
- \(L_t\)：瞬时雷达系（该激光点打出那一刻雷达本体坐标系，每个点不一样，原始 ranges/angles 就是在这个系下）
- O：odom 里程计坐标系
- \(L_0\)：基准雷达系（整帧统一基准 = 帧首时刻雷达位姿
frame_base_pose，校正后所有点要统一落到这个坐标系） - 分段起点：\(T_s\) 雷达位姿
frame_start_pose；分段终点：\(T_e\) 雷达位姿frame_end_pose
核心变换链（代码严格按这个顺序）
类方法getLaserPose
作用：给定一个时间戳dt，从TF缓存查询lase_link（scan_frame_name_）在odom（odom_name_）坐标系下的位姿
去畸变模块的依赖函数：lidarCalibration多次调用它获取各个时刻雷达在里程计坐标系的位姿，用于后续插值。
输入输出
| 参数 | 类型 | 作用 |
|---|---|---|
odom_pose |
tf::Stamped<tf::Pose>& |
输出参数（引用），存放查询结果：odom 坐标系下，laser_link在dt时刻的位姿（包含位置 + 旋转 + 时间戳 frame_id） |
dt |
ros::Time |
输入，需要查询的目标时间点（某一束 / 某分段的激光时间） |
tf_ |
tf::TransformListener* |
tf 监听对象指针，用来查 tf 树 |
| 返回值 | bool |
true：查询成功；false：等待变换失败（waitForTransform 超时） |
⚠️注意：catch 捕获异常后，函数依旧 return true！这是原代码的一个 bug
//从tf缓存数据中，寻找laser_link对应时间戳的里程计位姿
bool LidarMotionCalibrator::getLaserPose(tf::Stamped<tf::Pose> &odom_pose,ros::Time dt,tf::TransformListener * tf_)
{
//初始化
odom_pose.setIdentity();
//定义一个 tf::Stamped 对象，构造函数的入口参数（const T& input, const ros::Time& timestamp, const std::string & frame_id）
tf::Stamped <tf::Pose> tmp(odom_pose, dt, scan_frame_name_);
try
{
//阻塞直到可能进行转换或超时，解决时间不同步问题
if(!tf_->waitForTransform(odom_name_, scan_frame_name_, dt, ros::Duration(0.5))) // 0.15s 的时间可以修改
{
ROS_ERROR("LidarMotion-Can not Wait Transform()");
return false;
}
//转换一个带时间戳的位姿到目标坐标系odom_name_的位姿输出到odom_pose
tf_->transformPose(odom_name_, tmp, odom_pose);
}
catch(const std::exception& e)
{
std::cerr << e.what() << '\n';
}
return true;
}补充关键概念
waitForTransform四个参数含义：
waitForTransform(
target_frame, //目标坐标系 odom
source_frame, //源坐标系 laser_link
time, //查询哪个时刻的变换
timeout //最大等待时间
)
transformPose做的事情： 不是简单拿最新 tf，会根据 time 戳，在 tf 缓存做插值，得到该历史时刻的变换矩阵。
tf 内部保存历史一段时间的变换缓存，不仅仅保存当前时刻。这也是运动去畸变可以查询过去时刻雷达位姿的基础。
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
6
0- 0
已为社区贡献3条内容
所有评论(0)