OccupancyGrid.h

Go to the documentation of this file.

/*
 * Copyright (C) 2006-2011, SRI International (R)
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#pragma once

#ifndef __OpenKarto_OccupancyGird_h__
#define __OpenKarto_OccupancyGird_h__

#include <OpenKarto/Grid.h>
#include <OpenKarto/SensorData.h>

namespace karto
{




  typedef enum
  {
    GridStates_Unknown = 0,
    GridStates_Occupied = 100,
    GridStates_Free = 255
  } GridStates;


  class CellUpdater;
  class OpenMapper;

  class KARTO_EXPORT OccupancyGrid : public Grid<kt_int8u>
  {
    KARTO_RTTI();

  private:
    friend class CellUpdater;
    friend class IncrementalOccupancyGrid;
    
  public:
    OccupancyGrid(kt_int32s width, kt_int32s height, const Vector2d& rOffset, kt_double resolution);
    
  protected:
    //@cond EXCLUDE
    virtual ~OccupancyGrid();
    //@endcond
    
  public:
    static OccupancyGrid* CreateFromScans(const LocalizedLaserScanList& rScans, kt_double resolution);

    static KARTO_DEPRECATED  OccupancyGrid* CreateFromScans(const std::vector< SmartPointer<LocalizedRangeScan> >& rScans, kt_double resolution);

    static OccupancyGrid* CreateFromMapper(OpenMapper* pMapper, kt_double resolution);

  public:
    OccupancyGrid* Clone() const;

    inline kt_bool IsFree(const Vector2i& rGridIndex) const
    {
      kt_int8u* pOffsets = (kt_int8u*)GetDataPointer(rGridIndex);
      return (*pOffsets == GridStates_Free);
    }
    
    kt_double RayCast(const Pose2& rPose2, kt_double maxRange) const;

  protected:
    Grid<kt_int32u>* GetCellHitsCounts()
    {
      return m_pCellHitsCnt;
    }

    Grid<kt_int32u>* GetCellPassCounts()
    {
      return m_pCellPassCnt;
    }
    
    static void ComputeDimensions(const LocalizedLaserScanList& rScans, kt_double resolution, kt_int32s& rWidth, kt_int32s& rHeight, Vector2d& rOffset);
    
    virtual void CreateFromScans(const LocalizedLaserScanList& rScans);
        
    kt_bool AddScan(LocalizedLaserScan* pScan, kt_bool doUpdate = false);
    
    kt_bool RayTrace(const Vector2d& rWorldFrom, const Vector2d& rWorldTo, kt_bool isEndPointValid, kt_bool doUpdate = false);
    
    void UpdateCell(kt_int8u* pCell, kt_int32u cellPassCnt, kt_int32u cellHitCnt);
    
    void UpdateGrid();
    
    virtual void Resize(kt_int32s width, kt_int32s height);
    
  protected:
    SmartPointer< Grid<kt_int32u> > m_pCellPassCnt;
    
    SmartPointer< Grid<kt_int32u> >  m_pCellHitsCnt;

  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.
    SmartPointer<Parameter<kt_int32u> > m_pMinPassThrough;

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

  private:
    // restrict the following functions
    OccupancyGrid(const OccupancyGrid&);
    const OccupancyGrid& operator=(const OccupancyGrid&);
  }; // OccupancyGrid

  KARTO_TYPE(OccupancyGrid);

  typedef SmartPointer<OccupancyGrid> OccupancyGridPtr;


  class KARTO_EXPORT CellUpdater : public Functor
  {
  public:
    CellUpdater(OccupancyGrid* pOccupancyGrid)
      : m_pOccupancyGrid(pOccupancyGrid)
    {
    }
    
    virtual void operator() (kt_int32u index);
    
  private:
    OccupancyGrid* m_pOccupancyGrid;
  }; // CellUpdater


}


#endif // __OpenKarto_OccupancyGird_h__