講解關于slam一系列文章匯總鏈接:史上最全slam從零開始，針對于本欄目講解(02)Cartographer源碼無死角解析-鏈接如下:
(02)Cartographer源碼無死角解析- (00)目錄_最新無死角講解：https://blog.csdn.net/weixin_43013761/article/details/127350885
?

?

一、前言

首先這里對點云的體素濾波進行一個簡單的介紹：體素濾波器是一種下采樣的濾波器，它的作用是使用體素化方法減少點云數量，采用體素格中接近中心點的點替代體素內的所有點云，這種方法比直接使用中心點要慢，但是更加精確。這種方式即減少點云數據，并同時保存點云的形狀特征，在提高配準，曲面重建，形狀識別等算法速度中非常實用，來看下面兩張效果圖：


可以看到，經過體素濾波的點云更加稀疏，其原理也比較簡單：把三維空間按照一定分辨率分割成一個個的小正方體(或者說長方體)，每個小正方體中使用一個點云來表示，這樣就起到下采樣的效果。至于如何處理獲得一個小正方體最具有代表性的點云，不同算法，實現的方式也是不一樣的。另外，這里的小正方體，也稱為體素，所以叫體素濾波。
?

二、濾波函數總覽

首先說明一下，該篇博客主要接著上一篇博客進行講解，也就是對src/cartographer/cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc 文件 中LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter 函數的如下代碼進行細節(jié)分析：

  // Step: 6 對點云進行體素濾波
  return sensor::RangeData{
      cropped.origin,
      sensor::VoxelFilter(cropped.returns, options_.voxel_filter_size()), // param: voxel_filter_size
      sensor::VoxelFilter(cropped.misses, options_.voxel_filter_size())};

也就是進行體素濾波，輸入濾波的點云數據幀已經完成了時間同步、運動畸變校正，以及重力校正與Z軸過濾。不過需要注意的是，src/cartographer/cartographer/sensor/internal/voxel_filter.cc 文件 中定義了很多濾波函數，其中有些為模板函數，如下所示：

1.// 進行體素濾波
PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution){......}

2.
TimedPointCloud VoxelFilter(const TimedPointCloud& timed_point_cloud,const float resolution) {......}

3.
std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement> VoxelFilter(
	const std::vector<sensor::TimedPointCloudOriginData::RangeMeasurement>&range_measurements,
	const float resolution) {....}

為什么定義這么的濾波函數暫時不是很了解，可能是為了處理各種類型的點云(如帶時間與不帶時間)，且需要返回不同的數據格式。下面要講解的是其上的 1，如下：

PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution) {

該濾波輸入與輸出的數據格式都是都是 PointCloud 類型的實例，該類在上一篇博客中提及過，且對其成員函數：

  template <class UnaryPredicate>
  PointCloud copy_if(UnaryPredicate predicate) const {....}

進行了細節(jié)的分析。簡單的可以把該類理解為，存儲一幀點云數據的類，且該類還實現了點云的過濾成員函數→copy_if()。

另外還有一個參數 options_.voxel_filter_size() 表示濾波的體素大小，該變量在 src/cartographer/configuration_files/trajectory_builder_2d.lua 文件中設置，默認為 0.025，也就是每2.5cm為一個體素。

?

三、sensor::VoxelFilter() 總體介紹

/**
 * @brief 對點云數據進行體素濾波
 * 
 * @param[in] point_cloud 該類包含需要處理的所有點云數據
 * @param[in] resolution 濾波分辨率，即每個體素的體積大小
 * @return PointCloud 過濾之后的點云數據
 */
PointCloud VoxelFilter(const PointCloud& point_cloud, const float resolution) {
  
  //得到標記后的點，記錄一個體素中，應該取那個點。(每個體素只取一個點進行代替)
  const std::vector<bool> points_used = RandomizedVoxelFilterIndices(
      point_cloud.points(), resolution,
      [](const RangefinderPoint& point) { return point.position; });

  // 根據標記，把對應的點云存儲在 filtered_points 之中
  std::vector<RangefinderPoint> filtered_points;
  for (size_t i = 0; i < point_cloud.size(); i++) {
    if (points_used[i]) {
      filtered_points.push_back(point_cloud[i]);
    }
  }
  //根據濾波之后的點云標記，把點云對應的強度添加到 filtered_intensities 之中
  std::vector<float> filtered_intensities;
  CHECK_LE(point_cloud.intensities().size(), point_cloud.points().size());
  for (size_t i = 0; i < point_cloud.intensities().size(); i++) {
    if (points_used[i]) {
      filtered_intensities.push_back(point_cloud.intensities()[i]);
    }
  }

  //通過std::move移交所有權，減少數據的拷貝
  return PointCloud(std::move(filtered_points),
                    std::move(filtered_intensities));
}

該函數主要分三個步驟對點云進行處理，這里主要分析第一個部分，即 RandomizedVoxelFilterIndices() 函數。如下所示：

  //得到標記后的點
  const std::vector<bool> points_used = RandomizedVoxelFilterIndices(
      point_cloud.points(), resolution,
      [](const RangefinderPoint& point) { return point.position; });

可見其傳入了三個參數，①需要處理的點云幀數據；②體素濾波分辨率；③lambda表達式，該表示的功能為傳入一個 RangefinderPoint 實例對象，其會返回 Eigen::Vector3f 類型的點云坐標。該函數具體實現于
src/cartographer/cartographer/sensor/internal/voxel_filter.cc 中的，代碼如下：

// 進行體素濾波, 標記體素濾波后的點
template <class T, class PointFunction>
std::vector<bool> RandomizedVoxelFilterIndices(       
    const std::vector<T>& point_cloud, //需要進行處理的點云幀數據，  
     const float resolution, //體素濾波分辨率，即體素大小
    PointFunction&& point_function) { //獲取點云坐標的函數指針
  // According to https://en.wikipedia.org/wiki/Reservoir_sampling
  std::minstd_rand0 generator; //定義一個隨機數生成器

  //std::pair<int, int>> 第一個元素保存該voxel內部的點的個數, 
  //std::pair<int, int>> 第二個元素保存該voxel中選擇的那一個點的序號
  //VoxelKeyType 就是 uint64_t 類型，可以理解為體素的索引
  absl::flat_hash_map<VoxelKeyType, std::pair<int, int>>
      voxel_count_and_point_index;

  // 遍歷所有的點, 計算
  for (size_t i = 0; i < point_cloud.size(); i++) {
    //通過GetVoxelCellIndex()計算該點處于的voxel的序號
    //最終獲取VoxelKeyType對應的value的引用
    auto& voxel = voxel_count_and_point_index[GetVoxelCellIndex(
        point_function(point_cloud[i]), resolution)];

    voxel.first++;//該體素記錄的點云數量+1

    // 如果這個體素格子只有1個點, 那這個體素格子里的點的索引就是i
    if (voxel.first == 1) {
      voxel.second = i; //這里的i為目前遍歷點云數據的索引
    } 
    else {
      // 生成隨機數的范圍是 [1, voxel.first]
      std::uniform_int_distribution<> distribution(1, voxel.first);
      // 生成的隨機數與個數相等, 就讓這個點代表這個體素格子
      if (distribution(generator) == voxel.first) {
        voxel.second = i;
      }
    }
  }

  // 為體素濾波之后的點做標記
  std::vector<bool> points_used(point_cloud.size(), false);
  for (const auto& voxel_and_index : voxel_count_and_point_index) {
    points_used[voxel_and_index.second.second] = true;
  }
  return points_used;
}

注釋比較詳細，這里就不在重復了，其主要是返回變量：

std::vector<bool> points_used(point_cloud.size(), false);

可見其大小與輸入的點云數目一致，默認設置為都為 false，其對應的點云如果被選中會標志為 true。該函數執(zhí)行完成之后則會調用
?

四、sensor::VoxelFilter() 函數返回

其上完成體素濾波之后，這里回到之前的 LocalTrajectoryBuilder2D::AddRangeData() 函數，可見代碼如下：

    return AddAccumulatedRangeData(
        time,
        // 對點云進行重力對齊，也就是讓點云的Z軸與重力方向平行
        TransformToGravityAlignedFrameAndFilter(
            gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
            accumulated_range_data_),
        gravity_alignment, sensor_duration);

可知其又調用了 LocalTrajectoryBuilder2D::AddAccumulatedRangeData() 函數，該函數后面再進行詳細的講解，因為涉及到了點云的掃描匹配，但是其中還有調用濾波相關的代碼，所以先對該部分內容進行講解，如下所示：

  // Step: 7 對 returns點云 進行自適應體素濾波，返回的點云的數據類型是PointCloud
  const 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;
  }

首先其只對 gravity_aligned_range_data.returns 點云進行了自適應體素濾波，而 gravity_aligned_range_data.returns.misses 點云沒有進行濾波，該函數定義如下所示：

// 進行自適應體素濾波
PointCloud AdaptiveVoxelFilter(
    const PointCloud& point_cloud,
    const proto::AdaptiveVoxelFilterOptions& options) {
  return AdaptivelyVoxelFiltered(
      // param: adaptive_voxel_filter.max_range 距遠離原點超過max_range的點被移除
      // 這里的最大距離是相對于local坐標系原點的
      options, FilterByMaxRange(point_cloud, options.max_range()));
}

也就是重點還是 src/cartographer/cartographer/sensor/internal/voxel_filter.cc 中的 AdaptivelyVoxelFiltered() 函數。注意其上的 options 包含如下參數：

  -- 使用固定的voxel濾波之后, 再使用自適應體素濾波器
  -- 體素濾波器 用于生成稀疏點云 以進行 掃描匹配
  adaptive_voxel_filter = {
    max_length = 0.5,             -- 嘗試確定最佳的立方體邊長, 邊長最大為0.5
    min_num_points = 200,         -- 如果存在 較多點 并且大于'min_num_points', 則減小體素長度以嘗試獲得該最小點數
    max_range = 50.,              -- 距遠離local坐標系原點超過 max_range 的點被移除
  },

另外還有一個比較簡單的函數 FilterByMaxRange() , 距遠離 local坐標系原點 超過max_range 的點云被移除。這里需要注意的，其是相對云 local坐標系原點，與前面分析的代碼 LocalTrajectoryBuilder2D::AddRangeData() 函數中的 options_.min_range() 與 options_.mix_range() 是不一樣的，如下：

    // param: min_range max_range
    if (range >= options_.min_range()) {//如果該點云大于等于最近測量距離
      if (range <= options_.max_range()) {//如果該點小于等于最遠測量距離

  //默認配置
  min_range = 0.,                 -- 雷達數據的最近濾波, 保存中間值
  max_range = 30.,                -- 雷達數據的最遠濾波, 保存中間值

在 LocalTrajectoryBuilder2D::AddRangeData() 函數中過濾的是相對于雷達原點的距離，默認 max_range = 30。AdaptivelyVoxelFiltered() 是相對于 local坐標系原點的距離 max_range = 30，通常情況下后者的設置都比前者大。max_range 過濾之后，隨后就是正式進入到 AdaptivelyVoxelFiltered() 函數了。
?

五、AdaptivelyVoxelFiltered()

該函數還是一樣，位于 src/cartographer/cartographer/sensor/internal/voxel_filter.cc 文件中，再復制一下 adaptive_voxel_filter 配置信息：

  -- 使用固定的voxel濾波之后, 再使用自適應體素濾波器
  -- 體素濾波器 用于生成稀疏點云 以進行 掃描匹配
  adaptive_voxel_filter = {
    max_length = 0.5,             -- 嘗試確定最佳的立方體邊長, 邊長最大為0.5
    min_num_points = 200,         -- 如果存在 較多點 并且大于'min_num_points', 則減小體素長度以嘗試獲得該最小點數
    max_range = 50.,              -- 距遠離local坐標系原點超過 max_range 的點被移除
  },

$(01)$ 判斷需要進行自適應體素濾波的幀點云數據，是否已經比較稀疏。如果 point_cloud.size() <= options.min_num_points() 表示目前點云足夠稀疏不需要再進行濾波了。直接返回 point_cloud 即可。

$(03)$ 如果目前點云的非常稠密，嘗試使用把體素的邊長設置為 max_length = 0.5 進行濾波。濾波之后依舊不夠稀疏，那么沒有辦法，說明已經是 max_length = 0.5 參數下最稀疏的狀態(tài)了, 直接返回。

$(04)$ 如果目前的點云不是非常稠密(如果非常稠密第二步就返回了)，那么就使用二分法，找到最合適的體素邊長進行濾波。簡單的說，就是每次體素邊長縮小到原來的1/2 ，進行濾波，如果濾波之后的點云多了，則表示需要增加體素邊長，如果少了，則減少體素的邊長。

$(05)$ 源碼中首先使用一個 for 循環(huán)，對體素邊長進行控制，每次循環(huán)都會把體素邊長縮小為上一次的1/2，如果邊長低于 1e-2f * options.max_length() 則跳出 for 循環(huán)。但是注意，如果 for 循環(huán)執(zhí)行完了，說明其一直不滿足條件 result.size() >= options.min_num_points()，也就時點云一直比較稀疏，最終使用體素邊長為 1e-2f * options.max_length() 的濾波。

$(06)$ 如果for循環(huán)的過程中，發(fā)現點云比較稠密了，即滿足 options.min_num_points() 條件。則進入到 while 循環(huán)進行精細求解，該求解建立在 for循環(huán)的基礎上，即在來自于 for 循環(huán)獲得的 low_length 與 high_length 之間找到最合適的體素邊長。

$結果$ 經過 for 循環(huán)與 while 的最終結果，是希望點云數目保持在 options.min_num_points()=200 最優(yōu)。代碼注釋如下：

// 自適應體素濾波
PointCloud AdaptivelyVoxelFiltered(
    const proto::AdaptiveVoxelFilterOptions& options,
    const PointCloud& point_cloud) {
  // param: adaptive_voxel_filter.min_num_points 滿足小于等于這個值的點云滿足要求, 足夠稀疏
  //也就是說，該幀數據的點云已經比較稀疏了，沒有必要再進行濾波了
  if (point_cloud.size() <= options.min_num_points()) {
    // 'point_cloud' is already sparse enough.
    return point_cloud;
  }

  // param: adaptive_voxel_filter.max_length 進行一次體素濾波
  PointCloud result = VoxelFilter(point_cloud, options.max_length());
  // 如果按照最大邊長進行體素濾波之后還超過這個數了, 就說明已經是這個參數下最稀疏的狀態(tài)了, 直接返回
  if (result.size() >= options.min_num_points()) {
    // Filtering with 'max_length' resulted in a sufficiently dense point cloud.
    return result;
  }

  // Search for a 'low_length' that is known to result in a sufficiently
  // dense point cloud. We give up and use the full 'point_cloud' if reducing
  // the edge length by a factor of 1e-2 is not enough.
  // 將體素濾波的邊長從max_length逐漸減小, 每次除以2
  for (float high_length = options.max_length();
       high_length > 1e-2f * options.max_length(); high_length /= 2.f) {
    // 減小邊長再次進行體素濾波
    float low_length = high_length / 2.f;
    result = VoxelFilter(point_cloud, low_length);
    
    // 重復for循環(huán)直到 濾波后的點數多于min_num_points
    if (result.size() >= options.min_num_points()) {
      // Binary search to find the right amount of filtering. 'low_length' gave
      // a sufficiently dense 'result', 'high_length' did not. We stop when the
      // edge length is at most 10% off.
      // 以二分查找的方式找到合適的過濾邊長, 當邊緣長度最多減少 10% 時停止
      while ((high_length - low_length) / low_length > 1e-1f) {
        const float mid_length = (low_length + high_length) / 2.f;
        const PointCloud candidate = VoxelFilter(point_cloud, mid_length);
        // 如果點數多, 就將邊長變大, 讓low_length變大
        if (candidate.size() >= options.min_num_points()) {
          low_length = mid_length;
          result = candidate;
        } 
        // 如果點數少, 就將邊長變小, 讓high_length變小
        else {
          high_length = mid_length;
        }
      }
      return result;
    }
  }
  return result;
}

?

六、結語

該篇博客主要講解了點云體素濾波的知識，但是講解代碼過程中跳躍得比較多，可能導致不太好理解。不過沒有關系，下一篇博客我們來做一個復盤。這樣就不會覺得比較雜亂了。

?
?
?文章來源地址http://www.zghlxwxcb.cn/news/detail-490322.html

到了這里，關于(02)Cartographer源碼無死角解析-(32) LocalTrajectoryBuilder2D::AddRangeData()→點云的體素濾波的文章就介紹完了。如果您還想了解更多內容，請在右上角搜索TOY模板網以前的文章或繼續(xù)瀏覽下面的相關文章，希望大家以后多多支持TOY模板網！