karto探秘之open_karto 第五章 --- 栅格地图的生成

目录

1 类简介

1.1 OccupancyGrid类

2 static OccupancyGrid* CreateFromScans()

2.1 static void ComputeDimensions()

2.2 OccupancyGrid()

3 void CreateFromScans()

3.1 kt_bool AddScan()

3.2 kt_bool RayTrace()

3.3  void Grid::TraceLine()

3.4 virtual void Update()

3.5 virtual void UpdateCell()

---

slam_karto中的updateMap()调用了karto::OccupancyGrid::CreateFromScans()生成了栅格地图，这篇文章就讲一下栅格地图如何生成的。

bool
SlamKarto::updateMap()
{
  boost::mutex::scoped_lock lock(map_mutex_);

  karto::OccupancyGrid* occ_grid = 
          karto::OccupancyGrid::CreateFromScans(mapper_->GetAllProcessedScans(), resolution_);
// ...
}

 

1 类简介

1.1 OccupancyGrid类

首先看一下OccupancyGrid类的成员变量

  /**
   * Occupancy grid definition. See GridStates for possible grid values.
   */
  class OccupancyGrid : public Grid<kt_int8u>
  {
    friend class CellUpdater;
    friend class IncrementalOccupancyGrid;

  protected:
    /**
     * Counters of number of times a beam passed through a cell
     */
    Grid<kt_int32u>* m_pCellPassCnt;

    /**
     * Counters of number of times a beam ended at a cell
     */
    Grid<kt_int32u>* m_pCellHitsCnt;

  private:
    /**
     * Restrict the copy constructor
     */
    OccupancyGrid(const OccupancyGrid&);

    /**
     * Restrict the assignment operator
     */
    const OccupancyGrid& operator=(const OccupancyGrid&);

  private:
    CellUpdater* m_pCellUpdater;

    
    // NOTE: These two values are dependent on the resolution.  If the resolution is too small,
    // then not many beams will hit the cell!

    // Number of beams that must pass through a cell before it will be considered to be occupied
    // or unoccupied.  This prevents stray beams from messing up the map.
    Parameter<kt_int32u>* m_pMinPassThrough;

    // Minimum ratio of beams hitting cell to beams passing through cell for cell to be marked as occupied
    Parameter<kt_double>* m_pOccupancyThreshold;
  };  // OccupancyGrid

1.2 CellUpdater类（未进行使用）

  class KARTO_EXPORT CellUpdater : public Functor
  {
  public:
    CellUpdater(OccupancyGrid* pGrid)
      : m_pOccupancyGrid(pGrid)
    {
    }

    /**
     * Updates the cell at the given index based on the grid's hits and pass counters
     * @param index
     */
  void CellUpdater::operator() (kt_int32u index)
  {
    kt_int8u* pDataPtr = m_pOccupancyGrid->GetDataPointer();
    kt_int32u* pCellPassCntPtr = m_pOccupancyGrid->m_pCellPassCnt->GetDataPointer();
    kt_int32u* pCellHitCntPtr = m_pOccupancyGrid->m_pCellHitsCnt->GetDataPointer();

    m_pOccupancyGrid->UpdateCell(&pDataPtr[index], pCellPassCntPtr[index], pCellHitCntPtr[index]);
  }

  private:
    OccupancyGrid* m_pOccupancyGrid;
  };  // CellUpdater

 

2 static OccupancyGrid* CreateFromScans()

    /**
     * Create an occupancy grid from the given scans using the given resolution
     * @param rScans
     * @param resolution
     */
    static OccupancyGrid* CreateFromScans(const LocalizedRangeScanVector& rScans, kt_double resolution)
    {
      if (rScans.empty())
      {
        return NULL;
      }

      kt_int32s width, height;
      Vector2<kt_double> offset;
      ComputeDimensions(rScans, resolution, width, height, offset);
      OccupancyGrid* pOccupancyGrid = new OccupancyGrid(width, height, offset, resolution);
      pOccupancyGrid->CreateFromScans(rScans);

      return pOccupancyGrid;
    }

2.1 static void ComputeDimensions()

    /**
     * Calculate grid dimensions from localized range scans
     * @param rScans
     * @param resolution
     * @param rWidth
     * @param rHeight
     * @param rOffset
     */
    static void ComputeDimensions(const LocalizedRangeScanVector& rScans,
                                  kt_double resolution,
                                  kt_int32s& rWidth,
                                  kt_int32s& rHeight,
                                  Vector2<kt_double>& rOffset)
    {
      BoundingBox2 boundingBox;
      // 得到所有scan的总体boundingBox
      const_forEach(LocalizedRangeScanVector, &rScans)
      {
        boundingBox.Add((*iter)->GetBoundingBox());
      }

      kt_double scale = 1.0 / resolution;
      Size2<kt_double> size = boundingBox.GetSize();

      // 坐标值除以分辨率等于栅格的个数
      rWidth = static_cast<kt_int32s>(math::Round(size.GetWidth() * scale));
      rHeight = static_cast<kt_int32s>(math::Round(size.GetHeight() * scale));
      rOffset = boundingBox.GetMinimum(); // 左下角的坐标值
    }

  2.1.1 BoundingBox2

BoundingBox2 只是储存了矩形左下角坐标与右上角坐标

  /**
   * Defines a bounding box in 2-dimensional real space.
   */
  class BoundingBox2
  {
  public:
    /**
     * Get bounding box minimum
     */
    inline const Vector2<kt_double>& GetMinimum() const
    {
      return m_Minimum;
    }

    /**
     * Get the size of the bounding box 获取2坐标的差值
     */
    inline Size2<kt_double> GetSize() const
    {
      Vector2<kt_double> size = m_Maximum - m_Minimum;

      return Size2<kt_double>(size.GetX(), size.GetY());
    }

    /**
     * Add vector to bounding box
     */
    inline void Add(const Vector2<kt_double>& rPoint)
    {
      m_Minimum.MakeFloor(rPoint);// m_Minimum的x和y坐标 如果比 rPoint 大，则用 rPoint 的x或y赋值
      m_Maximum.MakeCeil(rPoint); // m_Maximum的x和y坐标 如果比 rPoint 小，则用 rPoint 的x或y赋值
    }

    /**
     * Add other bounding box to bounding box
     */
    inline void Add(const BoundingBox2& rBoundingBox)
    {
      Add(rBoundingBox.GetMinimum());
      Add(rBoundingBox.GetMaximum());
    }

  private:
    Vector2<kt_double> m_Minimum;
    Vector2<kt_double> m_Maximum;
  };  // BoundingBox2

2.2 OccupancyGrid()

    /**
     * Constructs an occupancy grid of given size
     * @param width
     * @param height
     * @param rOffset
     * @param resolution
     */
    OccupancyGrid(kt_int32s width, kt_int32s height, const Vector2<kt_double>& rOffset, kt_double resolution)
      : Grid<kt_int8u>(width, height)
      , m_pCellPassCnt(Grid<kt_int32u>::CreateGrid(0, 0, resolution))
      , m_pCellHitsCnt(Grid<kt_int32u>::CreateGrid(0, 0, resolution))
      , m_pCellUpdater(NULL)
    {
      m_pCellUpdater = new CellUpdater(this);

      if (karto::math::DoubleEqual(resolution, 0.0))
      {
        throw Exception("Resolution cannot be 0");
      }

      m_pMinPassThrough = new Parameter<kt_int32u>("MinPassThrough", 2);
      m_pOccupancyThreshold = new Parameter<kt_double>("OccupancyThreshold", 0.1);

      GetCoordinateConverter()->SetScale(1.0 / resolution);
      GetCoordinateConverter()->SetOffset(rOffset);  // 左下角坐标值
    }

 

3 void CreateFromScans()

    /**
     * Create grid using scans
     * @param rScans
     */
    virtual void CreateFromScans(const LocalizedRangeScanVector& rScans)
    {
      // 设置 pass 网格的size与左下角坐标
      m_pCellPassCnt->Resize(GetWidth(), GetHeight());
      m_pCellPassCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());

      // 设置 Hit 网格的size与左下角坐标
      m_pCellHitsCnt->Resize(GetWidth(), GetHeight());
      m_pCellHitsCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());

      const_forEach(LocalizedRangeScanVector, &rScans)
      {
        LocalizedRangeScan* pScan = *iter;
        AddScan(pScan);
      }

      Update();
    }


    /**
     * Resizes the grid (deletes all old data)
     * @param width
     * @param height
     */
    virtual void Resize(kt_int32s width, kt_int32s height)
    {
      Grid<kt_int8u>::Resize(width, height);    // 同时也将基类中的 Grid resize 了
      m_pCellPassCnt->Resize(width, height);
      m_pCellHitsCnt->Resize(width, height);
    }

3.1 kt_bool AddScan()

    /**
     * Adds the scan's information to this grid's counters (optionally
     * update the grid's cells' occupancy status)
     * @param pScan
     * @param doUpdate whether to update the grid's cell's occupancy status
     * @return returns false if an endpoint fell off the grid, otherwise true
     */
    virtual kt_bool AddScan(LocalizedRangeScan* pScan, kt_bool doUpdate = false)
    {
      kt_double rangeThreshold = pScan->GetLaserRangeFinder()->GetRangeThreshold();
      kt_double maxRange = pScan->GetLaserRangeFinder()->GetMaximumRange(); // 0
      kt_double minRange = pScan->GetLaserRangeFinder()->GetMinimumRange(); // 80

      Vector2<kt_double> scanPosition = pScan->GetSensorPose().GetPosition();

      // get scan point readings,  false 代表未经过滤波的scan
      const PointVectorDouble& rPointReadings = pScan->GetPointReadings(false);

      kt_bool isAllInMap = true;

      // draw lines from scan position to all point readings 从雷达坐标向着scan画线
      int pointIndex = 0;
      const_forEachAs(PointVectorDouble, &rPointReadings, pointsIter)
      {
        Vector2<kt_double> point = *pointsIter; // 雷达数据点的坐标
        kt_double rangeReading = pScan->GetRangeReadings()[pointIndex];
        kt_bool isEndPointValid = rangeReading < (rangeThreshold - KT_TOLERANCE); // 是否小于12米

        if (rangeReading <= minRange || rangeReading >= maxRange || std::isnan(rangeReading))
        {
          // ignore these readings
          pointIndex++;
          continue;
        }
        else if (rangeReading >= rangeThreshold) // 大于12米的点进行裁剪，距离越远有效距离越近
        {
          // trace up to range reading
          kt_double ratio = rangeThreshold / rangeReading;
          kt_double dx = point.GetX() - scanPosition.GetX();
          kt_double dy = point.GetY() - scanPosition.GetY();
          point.SetX(scanPosition.GetX() + ratio * dx);
          point.SetY(scanPosition.GetY() + ratio * dy);
        }

        kt_bool isInMap = RayTrace(scanPosition, point, isEndPointValid, doUpdate);
        if (!isInMap)
        {
          isAllInMap = false;
        }

        pointIndex++;
      }

      return isAllInMap;
    }

3.2 kt_bool RayTrace()

    /**
     * Traces a beam from the start position to the end position marking
     * the bookkeeping arrays accordingly.
     * @param rWorldFrom start position of beam
     * @param rWorldTo end position of beam
     * @param isEndPointValid is the reading within the range threshold?
     * @param doUpdate whether to update the cells' occupancy status immediately
     * @return returns false if an endpoint fell off the grid, otherwise true
     */
    virtual kt_bool RayTrace(const Vector2<kt_double>& rWorldFrom,
                             const Vector2<kt_double>& rWorldTo,
                             kt_bool isEndPointValid,
                             kt_bool doUpdate = false)
    {
      assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL);

      Vector2<kt_int32s> gridFrom = m_pCellPassCnt->WorldToGrid(rWorldFrom);
      Vector2<kt_int32s> gridTo = m_pCellPassCnt->WorldToGrid(rWorldTo);

      CellUpdater* pCellUpdater = doUpdate ? m_pCellUpdater : NULL;
      m_pCellPassCnt->TraceLine(gridFrom.GetX(), gridFrom.GetY(), gridTo.GetX(), gridTo.GetY(), pCellUpdater);

      // for the end point
      if (isEndPointValid)
      {
        if (m_pCellPassCnt->IsValidGridIndex(gridTo))
        {
          kt_int32s index = m_pCellPassCnt->GridIndex(gridTo, false);

          kt_int32u* pCellPassCntPtr = m_pCellPassCnt->GetDataPointer();
          kt_int32u* pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer();

          // increment cell pass through and hit count
          pCellPassCntPtr[index]++;
          pCellHitCntPtr[index]++;

          if (doUpdate)
          {
            (*m_pCellUpdater)(index);
          }
        }
      }

      return m_pCellPassCnt->IsValidGridIndex(gridTo);
    }

3.3  void Grid::TraceLine()

    /**
     * Increments all the grid cells from (x0, y0) to (x1, y1);
     * if applicable, apply f to each cell traced
     * @param x0
     * @param y0
     * @param x1
     * @param y1
     * @param f
     */
    void Grid::TraceLine(kt_int32s x0, kt_int32s y0, kt_int32s x1, kt_int32s y1, Functor* f = NULL)
    {
      kt_bool steep = abs(y1 - y0) > abs(x1 - x0);
      if (steep)           // 如果steep为true，将坐标关于y=x做对称，保持斜率小于45度
      {
        std::swap(x0, y0);
        std::swap(x1, y1);
      }
      if (x0 > x1)         // 坐标调换位置，保持x0在左边
      {
        std::swap(x0, x1);
        std::swap(y0, y1);
      }

      kt_int32s deltaX = x1 - x0;
      kt_int32s deltaY = abs(y1 - y0);
      kt_int32s error = 0;
      kt_int32s ystep;     // 向上走还是向下走
      kt_int32s y = y0;

      if (y0 < y1)
      {
        ystep = 1;
      }
      else
      {
        ystep = -1;
      }

      kt_int32s pointX;
      kt_int32s pointY;
      for (kt_int32s x = x0; x <= x1; x++)
      {
        if (steep)
        {
          pointX = y;
          pointY = x;
        }
        else
        {
          pointX = x;
          pointY = y;
        }

        error += deltaY;

        if (2 * error >= deltaX)
        {
          y += ystep;
          error -= deltaX;
        }

        Vector2<kt_int32s> gridIndex(pointX, pointY);
        if (IsValidGridIndex(gridIndex))
        {
          kt_int32s index = GridIndex(gridIndex, false);  // 二维坐标变为1维索引
          T* pGridPointer = GetDataPointer();             
          pGridPointer[index]++;                          // index处的栅格储存的值+1

          if (f != NULL)
          {
            (*f)(index);
          }
        }
      }
    }

    /**
     * Checks whether the given coordinates are valid grid indices
     * @param rGrid
     */
    inline kt_bool Grid::IsValidGridIndex(const Vector2<kt_int32s>& rGrid) const
    {
      return (math::IsUpTo(rGrid.GetX(), m_Width) && math::IsUpTo(rGrid.GetY(), m_Height));
    }


    /**
     * Checks whether value is in the range [0;maximum)
     * @param value
     * @param maximum
     */
    template<typename T>
    inline kt_bool math::IIsUpTo(const T& value, const T& maximum)
    {
      return (value >= 0 && value < maximum);
    }


    /**
     * Gets the index into the data pointer of the given grid coordinate
     * @param rGrid
     * @param boundaryCheck default value is true
     * @return grid index
     */
    virtual kt_int32s Grid::GridIndex(const Vector2<kt_int32s>& rGrid, kt_bool boundaryCheck = true) const
    {
      if (boundaryCheck == true)
      {
        if (IsValidGridIndex(rGrid) == false)
        {
          std::stringstream error;
          error << "Index " << rGrid << " out of range.  Index must be between [0; "
                << m_Width << ") and [0; " << m_Height << ")";
          throw Exception(error.str());
        }
      }

      kt_int32s index = rGrid.GetX() + (rGrid.GetY() * m_WidthStep);

      if (boundaryCheck == true)
      {
        assert(math::IsUpTo(index, GetDataSize()));
      }

      return index;
    }

3.4 virtual void Update()

调用这个函数将更新整个栅格地图，并设置栅格地图的占用状态。更新时是通过pass栅格地图与hit栅格地图中对应栅格的比值进行判断的，hit中的1个值需要pass中的10个值来进行取消（占用的比例为0.1），Grid中的地图，与pass地图，hit地图的大小都一样，Resize()中实现。

Update()在CreateFromScans()中只调用1次，同时维护3个等大小的地图，2个中间地图，一个最终地图。

    /**
     * Update the grid based on the values in m_pCellHitsCnt and m_pCellPassCnt
     */
    virtual void Update()
    {
      assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL);

      // clear grid
      Clear();

      // set occupancy status of cells
      kt_int8u* pDataPtr = GetDataPointer();
      kt_int32u* pCellPassCntPtr = m_pCellPassCnt->GetDataPointer();
      kt_int32u* pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer();

      kt_int32u nBytes = GetDataSize();
      for (kt_int32u i = 0; i < nBytes; i++, pDataPtr++, pCellPassCntPtr++, pCellHitCntPtr++)
      {
        UpdateCell(pDataPtr, *pCellPassCntPtr, *pCellHitCntPtr);
      }
    }


    /**
     * Clear out the grid data
     */
    void Grid::Clear()
    {
      memset(m_pData, 0, GetDataSize() * sizeof(T));
    }

3.5 virtual void UpdateCell()

    /**
     * Updates a single cell's value based on the given counters
     * @param pCell
     * @param cellPassCnt
     * @param cellHitCnt
     */
    virtual void UpdateCell(kt_int8u* pCell, kt_int32u cellPassCnt, kt_int32u cellHitCnt)
    {
      if (cellPassCnt > m_pMinPassThrough->GetValue())    // pass栅格图中的值是否大于2
      {
        kt_double hitRatio = static_cast<kt_double>(cellHitCnt) / static_cast<kt_double>(cellPassCnt);

        if (hitRatio > m_pOccupancyThreshold->GetValue()) // hitRatio 是否大于 0.1
        {                                                 // hit中的1个值需要pass中的10个值来取消掉
          *pCell = GridStates_Occupied;     // 100 
        }
        else
        {
          *pCell = GridStates_Free;         // 255
        }
      }
    }