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

 

 

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 

    /**
     * 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;
    }

1.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;
      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();
    }

1.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);
    }

2 void CreateFromScans()

    /**
     * Create grid using scans
     * @param rScans
     */
    virtual void CreateFromScans(const LocalizedRangeScanVector& rScans)
    {
      m_pCellPassCnt->Resize(GetWidth(), GetHeight());
      m_pCellPassCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());

      m_pCellHitsCnt->Resize(GetWidth(), GetHeight());
      m_pCellHitsCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());

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

      Update();
    }

 

2.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();
      kt_double minRange = pScan->GetLaserRangeFinder()->GetMinimumRange();

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

      // get scan point readings
      const PointVectorDouble& rPointReadings = pScan->GetPointReadings(false);

      kt_bool isAllInMap = true;

      // draw lines from scan position to all point readings
      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);

        if (rangeReading <= minRange || rangeReading >= maxRange || std::isnan(rangeReading))
        {
          // ignore these readings
          pointIndex++;
          continue;
        }
        else if (rangeReading >= rangeThreshold)
        {
          // 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;
    }

2.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);
    }

2.3 

    /**
     * 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);
      }
    }

2.4

    /**
     * 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())
      {
        kt_double hitRatio = static_cast<kt_double>(cellHitCnt) / static_cast<kt_double>(cellPassCnt);

        if (hitRatio > m_pOccupancyThreshold->GetValue())
        {
          *pCell = GridStates_Occupied;
        }
        else
        {
          *pCell = GridStates_Free;
        }
      }
    }