讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
 
文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证{\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证
 

一、前言

在上一篇博客中，对 LocalTrajectoryBuilder2D::AddRangeData() 函数中的如下部分进行了讲解：

  // Step: 1 进行多个雷达点云数据的时间同步, 点云的坐标是相对于tracking_frame的auto synchronized_data =range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);if (synchronized_data.ranges.empty()) { //判断同步的点云数据是否为空，//通常在多雷达传感器中，expected_sensor_ids_ 数据没有到齐。LOG(INFO) << "Range data collator filling buffer."; return nullptr;//表示数据已经添加到数据整理器的缓存中。}

之前或许看过之后不是很明白，但是现在应该是比较清楚了，其是把多个雷达数据的点云，按时间戳排序整合到一起，且都是基于 tracking_frame 坐标系的。其返回的数据结构如下所示：

// 时间同步前的点云
struct TimedPointCloudData {common::Time time;        // 点云最后一个点的时间Eigen::Vector3f origin;   // 雷达传感器到 tracking_frame_ 的平移TimedPointCloud ranges;   // 数据点的集合, 每个数据点包含xyz与time, time是负的// 'intensities' has to be same size as 'ranges', or empty.std::vector intensities; // 空的
};// 时间同步后的点云
struct TimedPointCloudOriginData {struct RangeMeasurement {TimedRangefinderPoint point_time;   // 带时间戳的单个数据点的坐标 xyzfloat intensity;                    // 强度值size_t origin_index;                // 属于第几个origins的点};common::Time time;                    // 点云的时间std::vector origins; // 点云是由几个点云组成, 每个点云的原点std::vector ranges; // 数据点的集合
};

为了方便讲解，我们就认为上面的包含的就是一帧数据，只是有包含多个雷达数据而已。后续称为多雷达时间同步数据、时间同步雷达数据、时间同步点云数据都差不多表达一个意思。注意时间同步之后的雷达数据，也就是上面结构体中 TimedPointCloudOriginData:: time 变量表示的是 current_start_，也就是时间同步点云最后一个数据的时间戳，TimedPointCloudOriginData::RangeMeasurement::point_time 表示相对于 TimedPointCloudOriginData:: time 的时间，一般来说该为0或者负数。

TimedPointCloudOriginData::origins 存储了所有雷达传感器到 tracking_frame_ 的平移，然后
RangeMeasurement::origin_index 记录了该点云到 tracking_frame_ 平移的索引，通过索引可在 TimedPointCloudOriginData::origins 找到对应的平移关系。
 

二、AddRangeData() 续接

回到 LocalTrajectoryBuilder2D::AddRangeData 函数，继续往下分析。

  const common::Time& time = synchronized_data.time;// Initialize extrapolator now if we do not ever use an IMU.// 如果不用imu, 就在雷达这初始化位姿推测器if (!options_.use_imu_data()) {InitializeExtrapolator(time);}

因为我们使用了imu，所以这里不初始化位姿推断器。然后做个信息的打印，告诉用户目前还没有进行位姿初始化，本人打印类似如下：

[ INFO] [1669275089.560128501, 1606808654.733059071]: I1124 15:31:29.000000 173872 local_trajectory_builder_2d.cc:168] Extrapolator not yet initialized.

然后执行如下代码：

  CHECK(!synchronized_data.ranges.empty());// TODO(gaschler): Check if this can strictly be 0.CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f);// 计算第一个点的时间const common::Time time_first_point =time +common::FromSeconds(synchronized_data.ranges.front().point_time.time);

保证点云数据不为空，且最后一个点云的数据，时间为0。且用 synchronized_data.time 时间戳，加上第一个点云相对于该时间戳的时间 synchronized_data.ranges.front().point_time.time 得到第一个点云的时间戳。

  // 只有在extrapolator_初始化时, GetLastPoseTime()是common::Time::min()if (time_first_point < extrapolator_->GetLastPoseTime()) {LOG(INFO) << "Extrapolator is still initializing.";return nullptr;}

如果 第一个点云数据的时间，低于位置估计器最新的位姿时间，说明器还没有完成初始化，但是已经再初始化之中了，因为 extrapolator_->GetLastPoseTime() 没有初始化，则其返回是一个很小的时间戳，判断条件不成立，当然也不会打印。进一步会执行如下代码：

  // 预测得到每一个时间点的位姿for (const auto& range : synchronized_data.ranges) {common::Time time_point = time + common::FromSeconds(range.point_time.time);// 如果该时间比上次预测位姿的时间还要早,说明这个点的时间戳往回走了, 就报错if (time_point < extrapolator_->GetLastExtrapolatedTime()) {// 一个循环只报一次错if (!warned) {LOG(ERROR)<< "Timestamp of individual range data point jumps backwards from "<< extrapolator_->GetLastExtrapolatedTime() << " to " << time_point;warned = true;}time_point = extrapolator_->GetLastExtrapolatedTime();}// Step: 2 预测出 每个点的时间戳时刻, tracking frame 在 local slam 坐标系下的位姿range_data_poses.push_back(extrapolator_->ExtrapolatePose(time_point).cast());}

该部分的代码暂且简单理解一下，就是 extrapolator_ 会预测出每个点云的时间戳时刻，其 tracking frame 在 local slam 坐标系下的位姿。这些都后续再进行详细讲解。继续往下看代码：

  if (num_accumulated_ == 0) {//该变量构造时，声明为0// 'accumulated_range_data_.origin' is uninitialized until the last// accumulation.//对accumulated_range_data_进行一个空数据的赋值accumulated_range_data_ = sensor::RangeData{{}, {}, {}};}

sensor::RangeData 的定义如下：

/*** @brief local_slam_data中存储所有雷达点云的数据结构* * @param origin  点云的原点在local坐标系下的坐标* @param returns 所有雷达数据点在local坐标系下的坐标, 记为returns, 也就是hit* @param misses  是在光线方向上未检测到返回的点(nan, inf等等)或超过最大配置距离的点*/
struct RangeData {Eigen::Vector3f origin;PointCloud returns;PointCloud misses; // local坐标系下的坐标
};

三、雷达数据运动畸变

接着下面时比较总要的一块代码。