讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
文末正下方中心提供了本人 联系方式， 点击本人照片即可显示 W X → 官方认证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证
一、前言
在前面的博客中，对概率栅格地图进行了细致的分析，其中包含了地图跟新的过程，但是对于 2D 激光 SLAM 来说，除了概率栅格地图之外，还有很多地图构建更新的方式，如接下来要分析的 TSDF，该论文全称为： KinectFusion: Real-Time Dense Surface Mapping and Tracking细节这里就不讲解了，有兴趣的朋友可以去看一下论文，激光SLAM（Simultaneous Localization and Mapping）中的Probability Grid Map和TSDF（Truncated Signed Distance Function）是两种常用的方法，用于建立环境地图和进行自主定位。它们的原理和优缺点如下：
P r o b a b i l i t y G r i d M a p （概率栅格地图）： \color{blue} \mathbf {Probability Grid Map（概率栅格地图）：} ProbabilityGridMap（概率栅格地图）：
概率栅格地图是一种常用的2D或3D环境表示方法。它将环境划分为一个个离散的栅格单元，并为每个栅格单元分配一个概率值，表示该栅格单元属于障碍物的可能性。概率值通常是0到1之间的实数，其中0表示自由空间，1表示障碍物，而0.5表示该栅格的状态不确定。通过激光传感器获得的测量数据，可以用来更新栅格地图中的概率值，从而不断优化地图的准确性(Cartographers实现有些区别，这里就不进行展开讨论了)。
优点：
可以高效地处理大规模地图，特别适用于2D环境。
能够表达环境的不确定性，对于处理传感器噪声和不完整数据有一定的鲁棒性。
缺点：
精度受限于栅格的分辨率，分辨率越高，计算量越大。
难以处理动态环境，比如移动的障碍物。
T r u n c a t e d S i g n e d D i s t a n c e F u n c t i o n （截断有符号距离函数）： \color{blue} \mathbf{Truncated Signed Distance Function（截断有符号距离函数）：} TruncatedSignedDistanceFunction（截断有符号距离函数）：
TSDF是一种连续函数的表示方法，用于建立环境的三维地图。它通过在三维空间中为每个点分配一个有符号距离值，表示该点离最近障碍物的距离，并考虑了截断（truncation）以限制地图的范围。如果该点在障碍物内部，则距离为负值；如果在障碍物外部，则距离为正值。
优点：
可以表示高精度的连续环境地图，尤其适用于3D环境。
对于动态环境有较好的建模能力，能够较好地跟踪移动的障碍物。
缺点：
计算复杂度较高，特别是在处理大规模环境时。
不太适合处理传感器噪声和不确定性，对于建模环境的不确定性不够灵活。
综合来说： \color{blue} \mathbf {综合来说：} 综合来说： 概率栅格地图适用于2D环境，具有高效处理大规模地图和处理传感器不确定性的优势，但在精度和对动态环境的处理上有一些限制。而TSDF适用于3D环境，能够表示高精度的地图并处理动态环境，但计算复杂度较高，对于不确定性的建模能力有所限制。具体应用时需要根据场景需求来选择合适的方法。
首先我们找到 src/cartographer/cartographer/mapping/2d/submap_2d.cc 文件 ActiveSubmaps2D::CreateGrid() 函数，该函数在创建栅格地图实例是会被调用：
// 以当前雷达原点为地图原件创建地图
std::unique_ptr<GridInterface> ActiveSubmaps2D::CreateGrid(
const Eigen::Vector2f& origin) {
// 地图初始大小,100个栅格
constexpr int kInitialSubmapSize = 100;
float resolution = options_.grid_options_2d().resolution(); // param: grid_options_2d.resolution
switch (options_.grid_options_2d().grid_type()) {
// 概率栅格地图
case proto::GridOptions2D::PROBABILITY_GRID:
return absl::make_unique<ProbabilityGrid>(
MapLimits(resolution,
// 左上角坐标为坐标系的最大值, origin位于地图的中间
origin.cast<double>() + 0.5 * kInitialSubmapSize *
resolution *
Eigen::Vector2d::Ones(),
CellLimits(kInitialSubmapSize, kInitialSubmapSize)),
&conversion_tables_);
// tsdf地图
case proto::GridOptions2D::TSDF:
return absl::make_unique<TSDF2D>(
MapLimits(resolution,
origin.cast<double>() + 0.5 * kInitialSubmapSize *
resolution *
Eigen::Vector2d::Ones(),
CellLimits(kInitialSubmapSize, kInitialSubmapSize)),
options_.range_data_inserter_options()
.tsdf_range_data_inserter_options_2d()
.truncation_distance(), // 0.3
options_.range_data_inserter_options()
.tsdf_range_data_inserter_options_2d()
.maximum_weight(), // 10.0
&conversion_tables_);
default:
LOG(FATAL) << "Unknown GridType.";
}
}
总体上来说呢，PGM(ProbabilityGridMap) 与 TSDF(TruncatedSignedDistanceFunction) 的原理是差不多的，首先他们的构造函数都接收一个 MapLimits 实例，用于描述地图区域的限制大小，以及对应栅格地图。ProbabilityGrid 还会接收一个转换表的参数 conversion_tables_ 用于地图更新，如果印象的朋友应该记得，ProbabilityGrid::correspondence_cost_cells_ 存储着尚格未被占用概率值对应的尚格值，该尚格值在 ProbabilityGrid::ApplyLookupTable() 函数中利用转换表 conversion_tables_ 进行更新。需要注意 src/cartographer/cartographer/mapping/probability_values.h 文件中如下定义：
constexpr float kMinProbability = 0.1f; // 0.1
constexpr float kMaxProbability = 1.f - kMinProbability; // 0.9
constexpr float kMinCorrespondenceCost = 1.f - kMaxProbability; // 0.1
constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability; // 0.9
其说明由 ProbabilityGrid::correspondence_cost_cells_ 转转而来栅格被占用的概率位于 0.1~0.9之间。传递给 TSDF2D 的后面两个参数，truncation_distance 与 maximum_weight 着两个参数是比较重要的，其中 truncation_distance 作用就是限制尚格取值范围的，具体其是如何进行地图更新的，下来来好好分析一下。
二、TSDValueConverter
首先来看一下 src/cartographer/cartographer/mapping/internal/2d/tsdf_2d.h 这个文件，可知其与 ProbabilityGrid 一直，都继承于 Grid2D，成员变量如下：
ValueConversionTables* conversion_tables_; // 转换表指针
std::unique_ptr<TSDValueConverter> value_converter_; // 数据格式转换
std::vector<uint16> weight_cells_; // Highest bit is update marker.
可知与 ProbabilityGrid 十分相似，其也存在一个转换表 conversion_tables_，以及用于存储尚格值的 value_converter_，不过多了一个权重参数，貌似还会对每个尚格进行权重估算，总的来说是一个十分重要的参数，这里先记者，另外构造函数中形参 truncation_distance 与 max_weight 默认设置分别未 0.3，10.0。
value_converter_ 为一 TSDValueConverter 类型的智能指针对象，该类的主要作用就对 truncation_distance=0.3，max_weight=10.0 做了一个映射，如其比较重要的两个成员变量：
// value到tsd的转换表 将[0, 1~32767] 映射到 [-0.3, -0.3~0.3]
const std::vector<float>* value_to_tsd_;
// value到weight的转换表 将[0, 1~32767] 映射到 [0.0, 0.0~10.0]
const std::vector<float>* value_to_weight_;
另外还设置了一些默认值：
static constexpr float min_weight_ = 0.f; //权重最小值
static constexpr uint16 unknown_tsd_value_ = 0; //未探索区域 tsd 值
static constexpr uint16 unknown_weight_value_ = 0; //未探索区域 权重值
static constexpr uint16 update_marker_ = 1u << 15; //可用用于判断地图点是否被更行过，默认为 32768
关于 TSDValueConverter 构造函数实现于 TSDValueConverter.cc 文件中：
/**
* @brief 构造函数
*
* @param[in] max_tsd 0.3
* @param[in] max_weight 10.0
* @param[in] conversion_tables 转换表
*/
TSDValueConverter::TSDValueConverter(float max_tsd, float max_weight,
ValueConversionTables* conversion_tables)
: max_tsd_(max_tsd),
min_tsd_(-max_tsd),
max_weight_(max_weight),
tsd_resolution_(32766.f / (max_tsd_ - min_tsd_)),
weight_resolution_(32766.f / (max_weight_ - min_weight_)),
value_to_tsd_(
conversion_tables->GetConversionTable(min_tsd_, min_tsd_, max_tsd_)),
value_to_weight_(conversion_tables->GetConversionTable(
min_weight_, min_weight_, max_weight)) {
}
} // namespace mapping
首先要注意到的 ValueConversionTables* conversion_tables 这个变量，该变量的作用就是通过调用其成员函数 ValueConversionTables::GetConversionTable() 构建转换表(如果已经构建，则直接获取，不会构建相同的转换表)，也就是说 conversion_tables 中是可以存储多张转换表的。
源码中的默认配置为： max_tsd=0.3，max_weight_=10.0，这里再重复一下，tsd_resolution_ 表示表示tsd变换的分辨率，也就是说 tsd 增加1，则转换表 value_to_tsd_ 索引则增加 tsd_resolution_。conversion_tables->GetConversionTable(min_tsd_, min_tsd_, max_tsd_)) 与前面的博客 ProbabilityGridMap 中分析的结果差不多，忘记的朋友可以看一下：(02)Cartographer源码无死角解析-(43) 2D栅格地图→ValueConversionTables、Grid2D 回忆一下，总的来说，ValueConversionTables::GetConversionTable() 目的就是根据三个形参获取转换表，如果该转换表不存在，则调用 PrecomputeValueToBoundedFloat() 函数，根据参数构建转换表，然后添加至 bounds_to_lookup_table_ 中，这样下次就可以获取该转换表了。三个形参中的第一个表示未探索区域默认值。获取到转换表之后赋值给 value_to_tsd_。value_to_weight_ 也是同样的道理，转换表的最终作用就是如下：
// value到tsd的转换表 将[0, 1~32767] 映射到 [-0.3, -0.3~0.3]
const std::vector<float>* value_to_tsd_;
// value到weight的转换表 将[0, 1~32767] 映射到 [0.0, 0.0~10.0]
const std::vector<float>* value_to_weight_;
其包含的几个成员函数作用也比较简单，就是根据映射关系 ，如下：
// 输入 [-0.3, -0.3~0.3] 或 [0.0, 0.0~10.0] 的值，映射到 [0, 1~32767]
inline uint16 TSDToValue(const float tsd) {
......}
inline uint16 WeightToValue(const float weight) {
......}
// 与上面的作用相反
inline float ValueToWeight(const uint16 value)
inline float ValueToTSD(const uint16 value)
说明白了就是进行数值映射而已，回忆了 TSDValueConverter 之后，再来分析 TSDF2D 就比较简单了。
三、TSDF基本函数
现在来看看 tsdf_2d.cc 这个文件中，其成员函数相的实现，构造函数如下所示：
/**
* @brief 构造函数
*
* @param[in] limits 地图坐标信息
* @param[in] truncation_distance 0.3
* @param[in] max_weight 10.0
* @param[in] conversion_tables 转换表
*/
TSDF2D::TSDF2D(const MapLimits& limits, float truncation_distance,
float max_weight, ValueConversionTables* conversion_tables)
: Grid2D(limits, -truncation_distance, truncation_distance,
conversion_tables),
conversion_tables_(conversion_tables),
value_converter_(absl::make_unique<TSDValueConverter>(
truncation_distance, max_weight, conversion_tables_)),
weight_cells_(
limits.cell_limits().num_x_cells * limits.cell_limits().num_y_cells,
value_converter_->getUnknownWeightValue()) {
}
传递 ValueConversionTables* conversion_tables 这个参数，千前面提到，该为一个转换表存储器指针，其是可以存储多张转换表的。比如在构造 TSDValueConverter 对象时，通过 ValueConversionTables::GetConversionTable() 构建转换表，构建之后赋值给 value_converter_，该表存储了 TSDValueConverter::value_to_tsd_ 与 TSDValueConverter::value_to_weight_ 这两个映射关系。另外，其对 std::vector<uint16> weight_cells_ 进行了赋值，全部设置为默认值，这里应该时 0。另外 TSDValueConverter 还有几个成员函数，先对来说是比较简单的：
// 把一个 [-0.3~0.3] 之间的一个值映射到 [0, 1~32767]
inline uint16 TSDToValue(const float tsd){
.......}
// 判断指定位置的栅格值是否被更行过，更新过则表示探索过
bool TSDF2D::CellIsUpdated(const Eigen::Array2i& cell_index)
// 获得索引栅格对应的 tsd 值
float TSDF2D::GetTSD(const Eigen::Array2i& cell_index)
// 获得索引栅格对应的权重
float TSDF2D::GetWeight(const Eigen::Array2i& cell_index)
// 同时获得索引栅格对应的 tsd 与权重值
std::pair<float, float> TSDF2D::GetTSDAndWeight(const Eigen::Array2i& cell_index)
// 设置栅格的TSD值与权重，不过最终是修改 Grid2D::correspondence_cost_cells_ 变量,
void TSDF2D::SetCell(const Eigen::Array2i& cell_index, float tsd,float weight)
需要重点提及一下，Grid2D::correspondence_cost_cells_ 是从 32768 开始起始，因为 TSDF2D::SetCell() 函数存在如下代码：
*tsdf_cell =value_converter_->TSDToValue(tsd) + value_converter_->getUpdateMarker();
value_converter_->getUpdateMarker() 在前面提到，其就是 32768。到这里，TSDValueConverter 的作用就十分明显了，说白了 tsd 值的基线就是 32768，可以理解为默认值，如果后续栅格值大于 32768 则表示该栅格被更新过，如函数：
// 指定的索引的栅格是否被更新过
bool TSDF2D::CellIsUpdated(const Eigen::Array2i& cell_index) const {
const int flat_index = ToFlatIndex(cell_index);
uint16 tsdf_cell = correspondence_cost_cells()[flat_index];
return tsdf_cell >= value_converter_->getUpdateMarker();
}
其就是判断栅格值是否大于 value_converter_->getUpdateMarker() = 32768，如果是，则表示被更行过了。
四、TSDF更新流程
通过全面的一系列的介绍，可以知道 TSDF 比 PGM 多了一个权重 TSDF2D::weight_cells_ 的概念，相信大家与我一样都是存在疑惑的，比如①他是如何计算的;②他的作用是什么; 下面顺代码分析，看是否能够找到这两个疑问的答案。核心代码都位于 tsdf_range_data_inserter_2d.cc 这个文件，本质上来来说，可以与前面博客分析的probability_grid_range_data_inserter_2d.cc 进行对比学习。他们的目的都是为了对栅格地图进行更新。在解答这些疑问之前，先来看看 TSDF 地图更新的主体流程是怎么样的，如下所示：
LocalTrajectoryBuilder2D::AddAccumulatedRangeData() // 首先是前端扫描匹配的相关函数为主调函数
// 通过调用该函数把点云数据插入到
insertion_result = InsertIntoSubmap(time, range_data_in_local, filtered_gravity_aligned_point_cloud,pose_estimate, gravity_alignment.rotation());
// 将点云数据写入到submap中
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps=active_submaps_.InsertRangeData(range_data_in_local);
// 将一帧雷达数据同时写入两个子图中，这里注意，该函数是被一个for循环包揽的
submap->InsertRangeData(range_data, range_data_inserter_.get());
// 将雷达数据写到栅格地图中
range_data_inserter->Insert(range_data, grid_.get());
到这个位置就要注意了，调用到 const RangeDataInserterInterface* range_data_inserter 实例的 Insert() 函数，range_data_inserter 是在 ActiveSubmaps2D 构造函数中进行初始化，如下：
// ActiveSubmaps2D构造函数
ActiveSubmaps2D::ActiveSubmaps2D(const proto::SubmapsOptions2D& options)
: options_(options), range_data_inserter_(CreateRangeDataInserter()) {
}
关于 CreateRangeDataInserter() 函数实现于 src/cartographer/cartographer/mapping/2d/submap_2d.cc 文件：
// 创建地图数据写入器
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.";
}
}
所以，可以得知如果使用 TSDF 方式构建地图那么 range_data_inserter_ 就是 TSDFRangeDataInserter2D 类型的实例指针。及对 range_data_inserter->Insert(range_data, grid_.get()); 函数的调用，就是对 TSDFRangeDataInserter2D::Insert() 函数的调用。
五、结语
根据经验，可以知道，最核心的就是 TSDFRangeDataInserter2D::Insert() 这个函数了，TSDF 所有的奥妙应该都与其相关比如前面提到的 TSDF2D::weight_cells_ ①他是如何计算的; ②他的作用是什么; 这些问题或许都能得到直接或者间接的答案，这篇博客就不啰嗦了，下篇博客进行纤细分析。