(02)Cartographer源码无死角解析-(86) 地图构建与更新→TSDF(TruncatedSignedDistanceFunction)、TSDValueConverter

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

一、前言

在前面的博客中，对概率栅格地图进行了细致的分析，其中包含了地图跟新的过程，但是对于 2D 激光 SLAM 来说，除了概率栅格地图之外，还有很多地图构建更新的方式，如接下来要分析的 TSDF，该论文全称为： KinectFusion: Real-Time Dense Surface Mapping and Tracking细节这里就不讲解了，有兴趣的朋友可以去看一下论文，激光SLAM（Simultaneous Localization and Mapping）中的Probability Grid Map和TSDF（Truncated Signed Distance Function）是两种常用的方法，用于建立环境地图和进行自主定位。它们的原理和优缺点如下：

$\mathbf{ProbabilityGridMap}（概率栅格地图）：$
概率栅格地图是一种常用的2D或3D环境表示方法。它将环境划分为一个个离散的栅格单元，并为每个栅格单元分配一个概率值，表示该栅格单元属于障碍物的可能性。概率值通常是0到1之间的实数，其中0表示自由空间，1表示障碍物，而0.5表示该栅格的状态不确定。通过激光传感器获得的测量数据，可以用来更新栅格地图中的概率值，从而不断优化地图的准确性(Cartographers实现有些区别，这里就不进行展开讨论了)。

优点：
可以高效地处理大规模地图，特别适用于2D环境。
能够表达环境的不确定性，对于处理传感器噪声和不完整数据有一定的鲁棒性。

缺点：
精度受限于栅格的分辨率，分辨率越高，计算量越大。
难以处理动态环境，比如移动的障碍物。

$\mathbf{TruncatedSignedDistanceFunction}（截断有符号距离函数）：$
TSDF是一种连续函数的表示方法，用于建立环境的三维地图。它通过在三维空间中为每个点分配一个有符号距离值，表示该点离最近障碍物的距离，并考虑了截断（truncation）以限制地图的范围。如果该点在障碍物内部，则距离为负值；如果在障碍物外部，则距离为正值。

优点：
可以表示高精度的连续环境地图，尤其适用于3D环境。
对于动态环境有较好的建模能力，能够较好地跟踪移动的障碍物。

缺点：
计算复杂度较高，特别是在处理大规模环境时。
不太适合处理传感器噪声和不确定性，对于建模环境的不确定性不够灵活。

$综合来说：$ 概率栅格地图适用于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 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_ ①他是如何计算的; ②他的作用是什么; 这些问题或许都能得到直接或者间接的答案，这篇博客就不啰嗦了，下篇博客进行纤细分析。