在Cartographer中机器人的运动轨迹是由一个个前后串联的节点(TrajectionNode)构成的。每当有新的扫描数据输入, LocalTrajectoryBuilder2D
都会 先通过scan matcher生成一个MatchingResult
的对象,但是它的insertion_result
字段是留空的。 只有当通过motion filter
成功插入到子图中之后,才会将插入的结果记录下来, 于此同时子图就得到了更新。
LocalTrajectoryBuilder2D类的成员变量
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18
| struct InsertionResult { std::shared_ptr<const TrajectoryNode::Data> constant_data; std::vector<std::shared_ptr<const Submap2D>> insertion_submaps; };
struct MatchingResult { common::Time time; transform::Rigid3d local_pose; sensor::RangeData range_data_in_local; std::unique_ptr<const InsertionResult> insertion_result; };
|
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
|
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult> LocalTrajectoryBuilder2D::AddRangeData( const std::string& sensor_id, const sensor::TimedPointCloudData& unsynchronized_data) { 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; if (!options_.use_imu_data()) InitializeExtrapolator(time);
if (extrapolator_ == nullptr) { LOG(INFO) << "Extrapolator not yet initialized."; return nullptr; } CHECK(!synchronized_data.ranges.empty()); 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); 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 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22
| std::vector<transform::Rigid3f> range_data_poses; range_data_poses.reserve(synchronized_data.ranges.size() ); bool warned = false; 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(); } range_data_poses.push_back( extrapolator_->ExtrapolatePose(time_point).cast<float>() ); }
|
处理运动畸变
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
|
if (num_accumulated_ == 0) accumulated_range_data_ = sensor::RangeData{ {}, {}, {} }; for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) { const sensor::TimedRangefinderPoint& hit = synchronized_data.ranges[i].point_time;
const Eigen::Vector3f origin_in_local = range_data_poses[i] * synchronized_data.origins.at(synchronized_data.ranges[i].origin_index); sensor::RangefinderPoint hit_in_local = range_data_poses[i] * sensor::ToRangefinderPoint(hit); const Eigen::Vector3f delta = hit_in_local.position - origin_in_local; const float range = delta.norm();
if (range >= options_.min_range() ) { if (range <= options_.max_range()) accumulated_range_data_.returns.push_back(hit_in_local);
else { hit_in_local.position = origin_in_local + options_.missing_data_ray_length() / range * delta; accumulated_range_data_.misses.push_back(hit_in_local); } } }
|
运动畸变的来源:雷达静止时,扫描结果没什么问题。雷达放在车上运动时,由于雷达扫描频率不高(比如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_; 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; const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation( extrapolator_->EstimateGravityOrientation(time) );
accumulated_range_data_.origin = range_data_poses.back().translation(); return AddAccumulatedRangeData( time, 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坐标系原点,这时候点云有可能是斜着的,先通过机器人的姿态把点云变成水平的,然后再对射程做过滤
按重力方向对点云数据进行变换,让点云的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 38
| sensor::RangeData LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter( const transform::Rigid3f& transform_to_gravity_aligned_frame, const sensor::RangeData& range_data) const { const sensor::RangeData cropped = sensor::CropRangeData(sensor::TransformRangeData( range_data, transform_to_gravity_aligned_frame), options_.min_z(), options_.max_z()); 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算法一致的功能。