cartographer代码学习-概率栅格地图（栅格地图的更新）

本文主要是介绍cartographer代码学习-概率栅格地图（栅格地图的更新），希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

在cartographer中，地图的更新是很重要的一部分内容，如何将一帧一帧的激光点云转换成子图，则是其中的核心部分。

栅格地图的更新流程

根据前面所学，我们知道在local_trajectory_builder_2d中，函数在对点云预处理后调用了扫描匹配模块：

  // Step: 7 对 returns点云 进行自适应体素滤波，返回的点云的数据类型是PointCloudconst sensor::PointCloud& filtered_gravity_aligned_point_cloud =sensor::AdaptiveVoxelFilter(gravity_aligned_range_data.returns,options_.adaptive_voxel_filter_options());if (filtered_gravity_aligned_point_cloud.empty()) {return nullptr;}// local map frame <- gravity-aligned frame// 扫描匹配, 进行点云与submap的匹配std::unique_ptr<transform::Rigid2d> pose_estimate_2d =ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);

扫描匹配可以获取到一个局部最优解，即当前机器人最有可能所在的实际位姿，该位姿会被用于后面栅格地图的更新，也就是下面的InsertIntoSubmap：

// 将二维坐标旋转回之前的姿态const transform::Rigid3d pose_estimate =transform::Embed3D(*pose_estimate_2d) * gravity_alignment;// 校准位姿估计器extrapolator_->AddPose(time, pose_estimate);// Step: 8 将 原点位于local坐标系原点处的点云 变换成 原点位于匹配后的位姿处的点云sensor::RangeData range_data_in_local =TransformRangeData(gravity_aligned_range_data,transform::Embed3D(pose_estimate_2d->cast<float>()));// 将校正后的雷达数据写入submapstd::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(time, range_data_in_local, filtered_gravity_aligned_point_cloud,pose_estimate, gravity_alignment.rotation());

而实际上InsertIntoSubmap这个函数它本身是调用了active_submaps类下的InsertRangeData：

std::unique_ptr<LocalTrajectoryBuilder2D::InsertionResult>
LocalTrajectoryBuilder2D::InsertIntoSubmap(const common::Time time, const sensor::RangeData& range_data_in_local,const sensor::PointCloud& filtered_gravity_aligned_point_cloud,const transform::Rigid3d& pose_estimate,const Eigen::Quaterniond& gravity_alignment) {// 如果移动距离过小, 或者时间过短, 不进行地图的更新if (motion_filter_.IsSimilar(time, pose_estimate)) {return nullptr;}// 将点云数据写入到submap中std::vector<std::shared_ptr<const Submap2D>> insertion_submaps =active_submaps_.InsertRangeData(range_data_in_local);// 生成InsertionResult格式的数据进行返回return absl::make_unique<InsertionResult>(InsertionResult{std::make_shared<const TrajectoryNode::Data>(TrajectoryNode::Data{time,gravity_alignment,filtered_gravity_aligned_point_cloud,  // 这里存的是体素滤波后的点云, 不是校准后的点云{},  // 'high_resolution_point_cloud' is only used in 3D.{},  // 'low_resolution_point_cloud' is only used in 3D.{},  // 'rotational_scan_matcher_histogram' is only used in 3D.pose_estimate}),std::move(insertion_submaps)});
}

而这个函数对于点云的处理又分为了两个部分：新增子图以及更新子图：

// 将点云数据写入到submap中
std::vector<std::shared_ptr<const Submap2D>> ActiveSubmaps2D::InsertRangeData(const sensor::RangeData& range_data) {// 如果第二个子图插入节点的数据等于num_range_data时,就新建个子图// 因为这时第一个子图应该已经处于完成状态了if (submaps_.empty() ||submaps_.back()->num_range_data() == options_.num_range_data()) {AddSubmap(range_data.origin.head<2>());}// 将一帧雷达数据同时写入两个子图中for (auto& submap : submaps_) {submap->InsertRangeData(range_data, range_data_inserter_.get());}// 第一个子图的节点数量等于2倍的num_range_data时,第二个子图节点数量应该等于num_range_dataif (submaps_.front()->num_range_data() == 2 * options_.num_range_data()) {submaps_.front()->Finish();}return submaps();
}

在当前子图容器为空时或者前一张子图的插入数量达到阈值时，会新开一张子图。否则会调用InsertRangeData对当前子图进行雷达数据的插入。而InsertRangeData中实际调用的是RangeDataInserterInterface类中的Insert函数进行的插入操作：

// 将雷达数据写到栅格地图中
void Submap2D::InsertRangeData(const sensor::RangeData& range_data,const RangeDataInserterInterface* range_data_inserter) {CHECK(grid_);CHECK(!insertion_finished());// 将雷达数据写到栅格地图中range_data_inserter->Insert(range_data, grid_.get());// 插入到地图中的雷达数据的个数加1set_num_range_data(num_range_data() + 1);
}

注意到Insert函数中的grid_.get()是一个指针，这个指针是ActiveSubmaps2D在构造的时候根据传入的参数CreateRangeDataInserter进行的构造。

// ActiveSubmaps2D构造函数
ActiveSubmaps2D::ActiveSubmaps2D(const proto::SubmapsOptions2D& options): options_(options), range_data_inserter_(CreateRangeDataInserter()) {}// 返回指向 Submap2D 的 shared_ptr指针 的vector
std::vector<std::shared_ptr<const Submap2D>> ActiveSubmaps2D::submaps() const {return std::vector<std::shared_ptr<const Submap2D>>(submaps_.begin(),submaps_.end());
}

而这个CreateRangeDataInserter函数本身是创建了一个地图数据写入器，根据初始化参数决定使用的是概率栅格地图写入器还是tsdf地图的写入器。

// 创建地图数据写入器
std::unique_ptr<RangeDataInserterInterface>
ActiveSubmaps2D::CreateRangeDataInserter() {switch (options_.range_data_inserter_options().range_data_inserter_type()) {// 概率栅格地图的写入器case proto::RangeDataInserterOptions::PROBABILITY_GRID_INSERTER_2D:return absl::make_unique<ProbabilityGridRangeDataInserter2D>(options_.range_data_inserter_options().probability_grid_range_data_inserter_options_2d());// tsdf地图的写入器case proto::RangeDataInserterOptions::TSDF_INSERTER_2D:return absl::make_unique<TSDFRangeDataInserter2D>(options_.range_data_inserter_options().tsdf_range_data_inserter_options_2d());default:LOG(FATAL) << "Unknown RangeDataInserterType.";}
}

对于我们这边来说，CreateRangeDataInserter是建立了ProbabilityGridRangeDataInserter2D类的一个指针。

ProbabilityGridRangeDataInserter2D

简单看一下ProbabilityGridRangeDataInserter2D这个类，它包含了三个成员变量：

  const proto::ProbabilityGridRangeDataInserterOptions2D options_;const std::vector<uint16> hit_table_;const std::vector<uint16> miss_table_;

options_是传入的配置参数，hit_table_是指按照占用概率0.55更新之后的值，miss_table_是按照空闲概率0.49更新之后的值。

在ProbabilityGridRangeDataInserter2D中初始化了hit_table_与miss_table_这两个参数：

// 写入器的构造, 新建了2个查找表
ProbabilityGridRangeDataInserter2D::ProbabilityGridRangeDataInserter2D(const proto::ProbabilityGridRangeDataInserterOptions2D& options): options_(options),// 生成更新占用栅格时的查找表 // param: hit_probabilityhit_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(options.hit_probability()))),    // 0.55// 生成更新空闲栅格时的查找表 // param: miss_probabilitymiss_table_(ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(options.miss_probability()))) {} // 0.49

这边主要调用了ComputeLookupTableToApplyCorrespondenceCostOdds函数，但是传入的参数是不一样的，前者传入的是options.hit_probability()（0.55），后者传入的是options.miss_probability()（0.49）。对于ComputeLookupTableToApplyCorrespondenceCostOdds函数的作用，是将栅格是未知状态与odds状态下, 将更新时的所有可能结果预先计算出来：

// 将栅格是未知状态与odds状态下, 将更新时的所有可能结果预先计算出来
std::vector<uint16> ComputeLookupTableToApplyCorrespondenceCostOdds(float odds) {//预先申请一个32768的空间std::vector<uint16> result;result.reserve(kValueCount); // 32768// 当前cell是unknown情况下直接把odds转成value存进来result.push_back(CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(ProbabilityFromOdds(odds))) +kUpdateMarker); // 加上kUpdateMarker作为一个标志, 代表这个栅格已经被更新了// 计算更新时 从1到32768的所有可能的 更新后的结果 for (int cell = 1; cell != kValueCount; ++cell) {result.push_back(CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(ProbabilityFromOdds(odds * Odds(CorrespondenceCostToProbability((*kValueToCorrespondenceCost)[cell]))))) +kUpdateMarker);}return result;
}

ProbabilityFromOdds(odds)是将odds的值转换成概率：

inline float ProbabilityFromOdds(const float odds) {return odds / (odds + 1.f);
}

这个概率代表的占用的概率，而ProbabilityToCorrespondenceCost函数则是将其转换成空闲的概率：

inline float ProbabilityToCorrespondenceCost(const float probability) {return 1.f - probability;
}

再上层则是CorrespondenceCostToValue函数，其中调用的是BoundedFloatToValue函数，作用是将浮点的概率值转换成0-32767的整数计算。

此外上面还有一个kUpdateMarker参数，该参数是作为一个标志, 代表这个栅格已经被更新了。kUpdateMarker本身是一个32768这么一个值，这样就可以通过一个数据来判断这个栅格是否被更新了。为什么要增加这个标志，主要是为了防止同一个栅格在一次更新中被多次更新，这样子本栅格如果已经被更新了，那么本次数据的后续点云将不会再更新该栅格。

再看一下后面的：

  // 计算更新时 从1到32768的所有可能的 更新后的结果 for (int cell = 1; cell != kValueCount; ++cell) {result.push_back(CorrespondenceCostToValue(ProbabilityToCorrespondenceCost(ProbabilityFromOdds(odds * Odds(CorrespondenceCostToProbability((*kValueToCorrespondenceCost)[cell]))))) +kUpdateMarker);}

这部分，kValueToCorrespondenceCost指代的是映射表：

// [0, 1~32767] 映射成 [0.9, 0.1~0.9]转换表
const std::vector<float>* const kValueToCorrespondenceCost =PrecomputeValueToCorrespondenceCost().release();

从这里得到的是一个空闲的概率，然后通过CorrespondenceCostToProbability转换成占用的概率，然后再转换成odds的值进行乘法操作，得到栅格新的概率值。然后再通过ProbabilityFromOdds函数将其从odds转换成概率值，ProbabilityToCorrespondenceCost会将占用概率转成空闲概率，CorrespondenceCostToValue则是将空闲概率转换成Value。通过这样子一系列的操作，就可以根据传入的odds将栅格的概率进行更新。注意这里同样添加了kUpdateMarker标记。
这个标记添加后是在哪里进行删除的呢？在Grid_2d中对雷达结束后的数据进行了恢复：

// Finishes the update sequence.
// 插入雷达数据结束
void Grid2D::FinishUpdate() {while (!update_indices_.empty()) {DCHECK_GE(correspondence_cost_cells_[update_indices_.back()],kUpdateMarker);// 更新的时候加上了kUpdateMarker, 在这里减去correspondence_cost_cells_[update_indices_.back()] -= kUpdateMarker;update_indices_.pop_back();}
}

可以看到这里在结束的时候对每个栅格去除了kUpdateMarker。

然后再看一下ProbabilityGridRangeDataInserter2D中的另外一个函数Insert，这个函数就是第一部分中InsertRangeData所调用的函数实现了：

/*** @brief 将点云写入栅格地图* * @param[in] range_data 要写入地图的点云* @param[in] grid 栅格地图*/
void ProbabilityGridRangeDataInserter2D::Insert(const sensor::RangeData& range_data, GridInterface* const grid) const {ProbabilityGrid* const probability_grid = static_cast<ProbabilityGrid*>(grid);CHECK(probability_grid != nullptr);// By not finishing the update after hits are inserted, we give hits priority// (i.e. no hits will be ignored because of a miss in the same cell).// param: insert_free_spaceCastRays(range_data, hit_table_, miss_table_, options_.insert_free_space(),probability_grid);probability_grid->FinishUpdate();
}

可以看到这个函数主要是调用了CastRays函数，这个函数实现了将点云写入栅格地图的具体操作。展开看一下这个函数的具体实现：
第一步其调用了一个GrowAsNeeded函数，该函数的作用主要是对于地图的扩展。在cartographer中，子图的大小并不是固定的，会随着运动逐渐增大，其增大的处理方式就是按照这里的代码实现。
第二步对地图进行了分辨率的放大：

const MapLimits& limits = probability_grid->limits();const double superscaled_resolution = limits.resolution() / kSubpixelScale;const MapLimits superscaled_limits(superscaled_resolution, limits.max(),CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale,limits.cell_limits().num_y_cells * kSubpixelScale));

这边的操作相当于将原有的分辨率放大了1000倍，获取了一个更加精细的高精度地图，这样可以使点云映射时画线画的更加细致。
第三步是将机器人姿态作为画线的原点放入到地图中：

// 雷达原点在地图中的像素坐标, 作为画线的起始坐标const Eigen::Array2i begin =superscaled_limits.GetCellIndex(range_data.origin.head<2>());

第四步是建立一个雷达终点所在栅格的容器，并更新终点所在地图栅格的占用值：

// Compute and add the end points.std::vector<Eigen::Array2i> ends;ends.reserve(range_data.returns.size());for (const sensor::RangefinderPoint& hit : range_data.returns) {// 计算hit点在地图中的像素坐标, 作为画线的终止点坐标ends.push_back(superscaled_limits.GetCellIndex(hit.position.head<2>()));// 更新hit点的栅格值probability_grid->ApplyLookupTable(ends.back() / kSubpixelScale, hit_table);}

更新是通过上述代码中的ApplyLookupTable函数实现的，这个函数函数中的更新主要是通过查找表的方式进行的更新，不需要再次进行计算，具体的查找表更新方式后续再单独整理。但是它的原理是跟原论文中的更新方式是一样的，只是实现方式的不同而以。

第五步是根据起点与点云的终点进行连线，并对连线上的栅格进行更新：

// Now add the misses.for (const Eigen::Array2i& end : ends) {std::vector<Eigen::Array2i> ray =RayToPixelMask(begin, end, kSubpixelScale);for (const Eigen::Array2i& cell_index : ray) {// 从起点到end点之前, 更新miss点的栅格值probability_grid->ApplyLookupTable(cell_index, miss_table);}}

RayToPixelMask函数是用于获取所有起点到终点所经过的所有栅格。然后再调用ApplyLookupTable函数，传入miss_table查找表进行栅格的更新。
第六步是对所有点云中超过范围的点的连线栅格的处理，在cartographer中，对于超出范围的点云，会用一个固定的值去替代（例如5米）。对于这些点云不会进行占用值的更新，但是会对连线上的所有点进行空闲值的更新：

  // Finally, compute and add empty rays based on misses in the range data.for (const sensor::RangefinderPoint& missing_echo : range_data.misses) {std::vector<Eigen::Array2i> ray = RayToPixelMask(begin, superscaled_limits.GetCellIndex(missing_echo.position.head<2>()),kSubpixelScale);for (const Eigen::Array2i& cell_index : ray) {// 从起点到misses点之前, 更新miss点的栅格值probability_grid->ApplyLookupTable(cell_index, miss_table);}}

这篇关于cartographer代码学习-概率栅格地图（栅格地图的更新）的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---