在Cartographer中机器人的运动轨迹是由一个个前后串联的节点(TrajectionNode)构成的。每当有新的扫描数据输入， LocalTrajectoryBuilder2D都会 先通过scan matcher生成一个MatchingResult的对象，但是它的insertion_result字段是留空的。 只有当通过motion filter成功插入到子图中之后，才会将插入的结果记录下来， 于此同时子图就得到了更新。
1 | struct InsertionResult { |
Local SLAM的业务主线是LocalTrajectoryBuilder2D::AddRangeData，它在核心函数GlobalTrajectoryBuilder::AddSensorData里调用1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46// 把来自不同传感器的TimePointCloudData进行同步。TimePointCloudData是带时间的点云数据
// 3D情况下，前3个元素是点的坐标，第4个元素是测量到每个点的相对时间。
// 时间以s为单位，以最后一个点的捕获时间为0，越早捕获的点时间值越大。
// 对于2d情况，第3个元素始终为0，第4个元素同样表示时间
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& unsynchronized_data)
{
// 把输入的索引和数据记录在range_data_collator_中，同时做了时间同步
// RangeDataCollator::AddRangeData
// TimedPointCloudOriginData range_data_collator_
auto synchronized_data =
range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);
if (synchronized_data.ranges.empty() )
{
LOG(INFO) << "Range data collator filling buffer.";
return nullptr;
}
// 记录下同步时间
const common::Time& time = synchronized_data.time;
// 如果不使用IMU,那么直接初始化 extrapolator
// 取点云获取的时间为基准
if (!options_.use_imu_data())
InitializeExtrapolator(time);
// 如果使用IMU进行估计，那么将在接收到IMU消息之后，再初始化 extrapolator
if (extrapolator_ == nullptr)
{
LOG(INFO) << "Extrapolator not yet initialized.";
return nullptr;
}
CHECK(!synchronized_data.ranges.empty());
// 确保最后一个数据的时间偏移量 <= 0
CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f);
// 获取第一个数据点的绝对时间，第一个点的时间就等于点云集获取的时间加上第一个点记录的相对时间
// 将它与 extrapolator的时间对比，来判定 extrapolator 是否完成了初始化
const common::Time time_first_point =
time +
common::FromSeconds(synchronized_data.ranges.front().point_time.time);
// 上一帧位姿如果没有计算完，则还在初始化的过程中
if (time_first_point < extrapolator_->GetLastPoseTime())
{
LOG(INFO) << "Extrapolator is still initializing.";
return nullptr;
}
第一次收到传感器数据时， GetLastPoseTime 和 time = synchronized_data.time相同。其余情况，都应当满足关系： time = synchronized_data.time > time_first_point > extrapolator_->GetLastPoseTime
1 | std::vector<transform::Rigid3f> range_data_poses; |
处理运动畸变
1 | // 如果还没有累积过扫描数据，就重置对象 accumulated_range_data_ |
运动畸变的来源：雷达静止时，扫描结果没什么问题。雷达放在车上运动时，由于雷达扫描频率不高(比如20Hz)，一些激光点就会有偏差。 注意不是指车运动时的激光往返距离的不同导致的， 激光速度为光速，车速与之相比可以忽略不计。
如上图所示，雷达的扫描是从右往左，应当扫描的结果是AB，但是扫描到B时，雷达已经运动了一段距离(假设匀速运动)，此时扫描的是 ，但是扫描的结果仍然算作运动前的坐标系里，也就是，最终的结果是， 这就造成了畸变。
向障碍物前进，获得的障碍距离偏小；远离障碍物，获得的障碍距离偏大。注意雷达扫描方向的不同，畸变的情况也不同。
雷达产生运动畸变，而相机不同，不管运动不运动都有畸变。实际中，平移产生的畸变非常小，可以忽略不计。但旋转产生的畸变，不能忽略了。
运动补偿通常有几种做法
1、如果有高频里程记，可以获取每个点相对起始扫描时刻的位姿
2、如果有imu，可以求出每个点相对起始点的旋转
3、如果没有其他传感器，可以使用匀速模型假设，使用上一个帧间里程记的结果作为当前两帧之间的运动，同时假设当前帧也是匀速运动，也可以估计出每个点相对起始时刻的位姿
cartographer对每个点的坐标左乘range_data_poses[i]，而它来自位姿估计器，后面可以看到，用了1和2两种方法。
代码中把点云从tracking坐标系转到local坐标系1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33 // 有了一帧有效点云数据了
++num_accumulated_;
// 每当累积的有效激光帧数 超过了配置值，一般为1
if (num_accumulated_ >= options_.num_accumulated_range_data() )
{
const common::Time current_sensor_time = synchronized_data.time;
absl::optional<common::Duration> sensor_duration;
if (last_sensor_time_.has_value())
{
sensor_duration = current_sensor_time - last_sensor_time_.value();
}
last_sensor_time_ = current_sensor_time;
num_accumulated_ = 0; // 计数清零
// 获取重力的方向，后面将点云z轴和重力方向对齐
const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
extrapolator_->EstimateGravityOrientation(time) );
// 计算origin，在插入子图时非常重要
// 最后一个点的时间戳估计的坐标，作为原点
accumulated_range_data_.origin = range_data_poses.back().translation();
return AddAccumulatedRangeData(
time,
// 点云对齐到重力方向后，对return集合体素化滤波
// 点云变换到原点处
TransformToGravityAlignedFrameAndFilter(
gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
accumulated_range_data_),
gravity_alignment,
sensor_duration );
}
return nullptr;
}
origin会有不断增加，因为车是动的，点云不断更新。origin_index是在点云时间同步那里赋值的，可以不关心细节。range_data_poses是tracking坐标系在local坐标系的坐标，也就是二者的坐标系变换。大小是激光点的个数，或者稍微少一点
最后不仅把点云转到local坐标系和重力对齐，还把点云变换到原点处 点云转到local坐标系原点，这时候点云有可能是斜着的，先通过机器人的姿态把点云变成水平的，然后再对射程做过滤
TransformToGravityAlignedFrameAndFilter
按重力方向对点云数据进行变换，让点云的z轴和重力方向重合1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38sensor::RangeData
LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter(
const transform::Rigid3f& transform_to_gravity_aligned_frame,
const sensor::RangeData& range_data) const
{
const sensor::RangeData cropped =
// 限定点云z坐标
sensor::CropRangeData(sensor::TransformRangeData(
range_data, transform_to_gravity_aligned_frame),
options_.min_z(), options_.max_z());
// 对 returns 和 misses点云做体素滤波
return sensor::RangeData{
cropped.origin,
sensor:: (options_.voxel_filter_size()).Filter(cropped.returns),
sensor::VoxelFilter(options_.voxel_filter_size()).Filter(cropped.misses) };
}
RangeData TransformRangeData(const RangeData& range_data,
const transform::Rigid3f& transform)
{
return RangeData{
transform * range_data.origin,
TransformPointCloud(range_data.returns, transform),
TransformPointCloud(range_data.misses, transform),
};
}
// 对点云中每一个点做变换
PointCloud TransformPointCloud(const PointCloud& point_cloud,
const transform::Rigid3f& transform)
{
PointCloud result;
result.reserve(point_cloud.size() );
for (const RangefinderPoint& point : point_cloud)
{
result.emplace_back(transform * point);
}
return result;
}
LocalTrajectoryBuilder2D是一个没有闭环处理的完整的slam过程，修改submap的帧的阈值（无限大），则完成了与开源hector slam算法一致的功能。