【圖解 cartographer】 之雷達模型CastRay

---

前言：
本文主要對google的開源SLAM框架 Cartographer 建圖過程中的鐳射雷達對柵格地圖的更新模型CastRay進行詳細講解。目前網上對這部分的講解比較少，只是大致提一下其使用的是 Bresenham快速畫直線演算法。本質上是沒有問題的，但是 Cartographer 的具體實現上還是有一些變化。之前我直接結合Bresenham和原始碼對照看，就越看越暈，比較難看懂，後面自己推導了一遍才完全明白，藉此做個記錄分享。

鐳射雷達更新柵格地圖的目的： 根據最新採集到的雷達點，在與地圖匹配後，把雷達點插入到當前地圖中，其本質是對柵格地圖概率的更新。這裡的更新包括兩部分，其一是對hit點的概率更新，其二是對CastRay完成鐳射射線狀地圖更新，即對一條直線所經過的柵格進行概率值更新。

地圖更新的方式：

1. 計算hit點的索引值，並直接呼叫ApplyLookupTable更新對應柵格概率。
2. 採用Bresenham演算法思想對雷達射線掃過miss的點進行概率更新。

如下所示是雷達掃描的模型模擬：

其中紅色線是雷達的miss部分，線段的端點就是hit點。在更新過程中，hit點更新比較簡單，可直接計算對應的柵格座標並更新；而對於紅線的miss部分，則需要根據紅線的起始點和終止點計算所經過的柵格才能更新，這裡先看看檔案 ray_casting.cc 的實現：

// ray_casting.cc
// 傳入雷達點，子圖大小資訊，hit和miss的更新函式指標
void CastRays(const sensor::LaserFan& laser_fan, const MapLimits& limits,
              const std::function<void(const Eigen::Array2i&)>& hit_visitor,
              const std::function<void(const Eigen::Array2i&)>& miss_visitor) {
  // 這裡把要更新的子圖的解析度再縮小1000倍，用於再精細化起始點和終止點的座標
  const double superscaled_resolution = limits.resolution() / kSubpixelScale;
  // 注意這裡為更新子圖重新構建一個資訊結構，地圖的limits.max()沒有變化
  const MapLimits superscaled_limits(
      superscaled_resolution, limits.max(),
      CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale,
                 limits.cell_limits().num_y_cells * kSubpixelScale));
  // 獲取雷達在map座標系中的座標作為雷達更新的起始點
  const Eigen::Array2i begin =
      superscaled_limits.GetXYIndexOfCellContainingPoint(laser_fan.origin.x(),
                                                         laser_fan.origin.y());

  // Compute and add the end points.
  // 更新所有hit點的概率值，直接呼叫hit_visitor即可
  std::vector<Eigen::Array2i> ends;
  ends.reserve(laser_fan.point_cloud.size());
  for (const Eigen::Vector2f& laser_return : laser_fan.point_cloud) {
    ends.push_back(superscaled_limits.GetXYIndexOfCellContainingPoint(
        laser_return.x(), laser_return.y()));
    hit_visitor(ends.back() / kSubpixelScale);
  }

  // Now add the misses. 更新miss射線部分的概率柵格
  for (const Eigen::Array2i& end : ends) {
    CastRay(begin, end, miss_visitor);
  }

  // Finally, compute and add empty rays based on missing echos in the scan.
  // 更新無反饋的雷達射線，借用miss模型
  for (const Eigen::Vector2f& missing_echo :
       laser_fan.missing_echo_point_cloud) {
    CastRay(begin, superscaled_limits.GetXYIndexOfCellContainingPoint(
                       missing_echo.x(), missing_echo.y()),
            miss_visitor);
  }
}

可以明確看出，更新hit點的概率很簡單，直接呼叫 hit_visitor(ends.back() / kSubpixelScale); 即可，但更新miss部分則呼叫了 CastRay(begin, end, miss_visitor); 為每條射線進行單獨更新，這也是本篇博文主要想講的部分。

-> 該問題的核心是求出直線所經過的所有柵格。

在介紹如何快速計算該柵格時，我們需要先介紹一下在計算機圖形影像學中的經典演算法：Bresenham演算法，它是一種典型的直線光柵化演算法，也顯示器中顯示直線的實現形式。其核心思想是：已知當前點座標及曲（直）線方程，遞推下一個（x+1）點的y座標（或者（y+1）點的x座標）。如下是顯示器顯示直線的離散圖：

Cartographer中的具體實現方式如下：

// Factor for subpixel accuracy of start and end point.
constexpr int kSubpixelScale = 1000;

// We divide each pixel in kSubpixelScale x kSubpixelScale subpixels. 'begin'
// and 'end' are coordinates at subpixel precision. We compute all pixels in
// which some part of the line segment connecting 'begin' and 'end' lies.
void CastRay(const Eigen::Array2i& begin, const Eigen::Array2i& end,
             const std::function<void(const Eigen::Array2i&)>& visitor) {
  // For simplicity, we order 'begin' and 'end' by their x coordinate.
  // 保證dx > 0，方便後面計算
  if (begin.x() > end.x()) {
    CastRay(end, begin, visitor);
    return;
  }

  CHECK_GE(begin.x(), 0);
  CHECK_GE(begin.y(), 0);
  CHECK_GE(end.y(), 0);

  // Special case: We have to draw a vertical line in full pixels, as 'begin'
  // and 'end' have the same full pixel x coordinate.
  // 處理當射線為一條豎線的情況
  if (begin.x() / kSubpixelScale == end.x() / kSubpixelScale) {
    Eigen::Array2i current(begin.x() / kSubpixelScale,
                           std::min(begin.y(), end.y()) / kSubpixelScale);
    const int end_y = std::max(begin.y(), end.y()) / kSubpixelScale;
    for (; current.y() <= end_y; ++current.y()) {
      visitor(current);
    }
    return;
  }

  const int64 dx = end.x() - begin.x();
  const int64 dy = end.y() - begin.y();
  const int64 denominator = 2 * kSubpixelScale * dx;

  // The current full pixel coordinates. We begin at 'begin'.
  Eigen::Array2i current = begin / kSubpixelScale;

  // To represent subpixel centers, we use a factor of 2 * 'kSubpixelScale' in
  // the denominator.
  // +-+-+-+ -- 1 = (2 * kSubpixelScale) / (2 * kSubpixelScale)
  // | | | |
  // +-+-+-+
  // | | | |
  // +-+-+-+ -- top edge of first subpixel = 2 / (2 * kSubpixelScale)
  // | | | | -- center of first subpixel = 1 / (2 * kSubpixelScale)
  // +-+-+-+ -- 0 = 0 / (2 * kSubpixelScale)

  // The center of the subpixel part of 'begin.y()' assuming the
  // 'denominator', i.e., sub_y / denominator is in (0, 1).
  int64 sub_y = (2 * (begin.y() % kSubpixelScale) + 1) * dx;

  // The distance from the from 'begin' to the right pixel border, to be divided by 2 * 'kSubpixelScale'.
  const int first_pixel = 2 * kSubpixelScale - 2 * (begin.x() % kSubpixelScale) - 1;
  // The same from the left pixel border to 'end'.
  const int last_pixel = 2 * (end.x() % kSubpixelScale) + 1;

  // The full pixel x coordinate of 'end'.
  const int end_x = std::max(begin.x(), end.x()) / kSubpixelScale;

  // Move from 'begin' to the next pixel border to the right.
  sub_y += dy * first_pixel;
  if (dy > 0) {
      // 下面詳細介紹
      return;
  }
}

Cartographer中為了提升地圖更新的精度，把起始點和終止點都放在了更細的解析度上，這樣一來，就需要考慮起始點和終止點相對於所佔柵格的偏移，這也是該演算法與經典Bresenham演算法的差別。如下，我們對該問題進行抽象：

現在問題變成了問題：已知起始點和終止點座標（1000倍細分），求該兩點連線經過的柵格？

  const int64 dx = end.x() - begin.x();
  const int64 dy = end.y() - begin.y();
  const int64 denominator = 2 * kSubpixelScale * dx;

這裡首先計算了dx和dy，denominator是用於Bresenham演算法的避免了浮點運算的因子，提前計算好。denominator的本質上是表示一個原始柵格的邊細分個數，這裡是kSubpixelScale = 1000；為了更好的解釋如下計算，我們把2*dx除去，便得:$$\frac{denominator}{2\ast dx}=kSubpixelScale$$
後面的 sub_y 直接比較難理解，但當其除以 $2\ast dx$ 時，則可以很容易得出如下示意：

$$sub\_y=2\ast B\ast dx$$

$$first\_pixel=2\ast A$$

是後面遞推更新柵格的核心，如下對其進行詳細解讀。

// sub_y的初始化
sub_y += dy * first_pixel;

我們把sub_y的全等式寫出來
$$\begin{array}{rl}sub\_y& =sub\_y+dy\ast first\_pixel\\ & =2\ast B\ast dx+2\ast A\ast dy\\ & =2\ast B\ast dx+2\ast C\ast dx\\ & =(B+C)\ast (2\ast dx)\end{array}$$

如下開始進行柵格概率更新，主要是用 $sub\_y$ 與 $denominator$ 進行比較，而其實際上是 $sub\_y$ 與 $kSubpixelScale$ 進行對比。

  // Move from 'begin' to the next pixel border to the right.
  sub_y += dy * first_pixel;
  if (dy > 0) {
    while (true) {
      visitor(current);
      while (sub_y > denominator) {
        sub_y -= denominator;
        ++current.y();
        visitor(current);
      }
      ++current.x();
      if (sub_y == denominator) {
        sub_y -= denominator;
        ++current.y();
      }
      if (current.x() == end_x) {
        break;
      }
      // Move from one pixel border to the next.
      sub_y += dy * 2 * kSubpixelScale;
    }
    // Move from the pixel border on the right to 'end'.
    sub_y += dy * last_pixel;
    visitor(current);
    while (sub_y > denominator) {
      sub_y -= denominator;
      ++current.y();
      visitor(current);
    }
    CHECK_NE(sub_y, denominator);
    CHECK_EQ(current.y(), end.y() / kSubpixelScale);
    return;
  }

• 1.當$sub\_y$ > $denominator$ ，取當前點上方的點；
• 2.當$sub\_y$ = $denominator$ ，取當前點右上方的點；
• 3.當$sub\_y$ < $denominator$ ，取當前點右方的點；

注意：這裡需注意起點和終止點的處理。

如下我們開始更新地圖，第一步 $sub\_y$ < $denominator$ ，取當前點右方的點；

這裡需重新計算$sub\_y=sub\_y+dy\ast 2\ast kSubpixelScale$，加了一個斜率單位的高度。
這裡會連續出現多次$sub\_y$ > $denominator$。

如此到達x的終點，需要再次更新$sub\_y=sub\_y+dy\ast las{t}_{p}ixel$

最終完成此條射線的柵格更新。

這裡我們只講解了dy>0的情況，其實當dy<0時，只是方向變了，其他更新方式都一樣。

我們再次對 Cartographer 的實際建圖過程進行了分析，與我們的理解一致。


到此，CastRay運算元的實現算是基本上理解了。