OccupancyGrid.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2006-2011, SRI International (R)
00003  *
00004  * This program is free software: you can redistribute it and/or modify
00005  * it under the terms of the GNU Lesser General Public License as published by
00006  * the Free Software Foundation, either version 3 of the License, or
00007  * (at your option) any later version.
00008  *
00009  * This program is distributed in the hope that it will be useful,
00010  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  * GNU Lesser General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU Lesser General Public License
00015  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016  */
00017 
00018 #pragma once
00019 
00020 #ifndef __OpenKarto_OccupancyGird_h__
00021 #define __OpenKarto_OccupancyGird_h__
00022 
00023 #include <OpenKarto/Grid.h>
00024 #include <OpenKarto/SensorData.h>
00025 
00026 namespace karto
00027 {
00028 
00030 
00031 
00035 
00039   typedef enum
00040   {
00041     GridStates_Unknown = 0,
00042     GridStates_Occupied = 100,
00043     GridStates_Free = 255
00044   } GridStates;
00045 
00049 
00050   class CellUpdater;
00051   class OpenMapper;
00052 
00056   class KARTO_EXPORT OccupancyGrid : public Grid<kt_int8u>
00057   {
00058     KARTO_RTTI();
00059 
00060   private:
00061     friend class CellUpdater;
00062     friend class IncrementalOccupancyGrid;
00063     
00064   public:
00072     OccupancyGrid(kt_int32s width, kt_int32s height, const Vector2d& rOffset, kt_double resolution);
00073     
00074   protected:
00075     //@cond EXCLUDE
00079     virtual ~OccupancyGrid();
00080     //@endcond
00081     
00082   public:
00090     static OccupancyGrid* CreateFromScans(const LocalizedLaserScanList& rScans, kt_double resolution);
00091 
00101     static KARTO_DEPRECATED  OccupancyGrid* CreateFromScans(const std::vector< SmartPointer<LocalizedRangeScan> >& rScans, kt_double resolution);
00102 
00110     static OccupancyGrid* CreateFromMapper(OpenMapper* pMapper, kt_double resolution);
00111 
00112   public:
00117     OccupancyGrid* Clone() const;
00118 
00124     inline kt_bool IsFree(const Vector2i& rGridIndex) const
00125     {
00126       kt_int8u* pOffsets = (kt_int8u*)GetDataPointer(rGridIndex);
00127       return (*pOffsets == GridStates_Free);
00128     }
00129     
00137     kt_double RayCast(const Pose2& rPose2, kt_double maxRange) const;
00138 
00139   protected:
00144     Grid<kt_int32u>* GetCellHitsCounts()
00145     {
00146       return m_pCellHitsCnt;
00147     }
00148 
00153     Grid<kt_int32u>* GetCellPassCounts()
00154     {
00155       return m_pCellPassCnt;
00156     }
00157     
00166     static void ComputeDimensions(const LocalizedLaserScanList& rScans, kt_double resolution, kt_int32s& rWidth, kt_int32s& rHeight, Vector2d& rOffset);
00167     
00172     virtual void CreateFromScans(const LocalizedLaserScanList& rScans);
00173         
00181     kt_bool AddScan(LocalizedLaserScan* pScan, kt_bool doUpdate = false);
00182     
00192     kt_bool RayTrace(const Vector2d& rWorldFrom, const Vector2d& rWorldTo, kt_bool isEndPointValid, kt_bool doUpdate = false);
00193     
00200     void UpdateCell(kt_int8u* pCell, kt_int32u cellPassCnt, kt_int32u cellHitCnt);
00201     
00205     void UpdateGrid();
00206     
00212     virtual void Resize(kt_int32s width, kt_int32s height);
00213     
00214   protected:
00218     SmartPointer< Grid<kt_int32u> > m_pCellPassCnt;
00219     
00223     SmartPointer< Grid<kt_int32u> >  m_pCellHitsCnt;
00224 
00225   private:
00226     CellUpdater* m_pCellUpdater;
00227 
00229     // NOTE: These two values are dependent on the resolution.  If the resolution is too small,
00230     // then not many beams will hit the cell!
00231 
00232     // Number of beams that must pass through a cell before it will be considered to be occupied
00233     // or unoccupied.  This prevents stray beams from messing up the map.
00234     SmartPointer<Parameter<kt_int32u> > m_pMinPassThrough;
00235 
00236     // Minimum ratio of beams hitting cell to beams passing through cell for cell to be marked as occupied
00237     SmartPointer<Parameter<kt_double> > m_pOccupancyThreshold;
00238 
00239   private:
00240     // restrict the following functions
00241     OccupancyGrid(const OccupancyGrid&);
00242     const OccupancyGrid& operator=(const OccupancyGrid&);
00243   }; // OccupancyGrid
00244 
00248   KARTO_TYPE(OccupancyGrid);
00249 
00253   typedef SmartPointer<OccupancyGrid> OccupancyGridPtr;
00254 
00258 
00262   class KARTO_EXPORT CellUpdater : public Functor
00263   {
00264   public:
00269     CellUpdater(OccupancyGrid* pOccupancyGrid)
00270       : m_pOccupancyGrid(pOccupancyGrid)
00271     {
00272     }
00273     
00278     virtual void operator() (kt_int32u index);
00279     
00280   private:
00281     OccupancyGrid* m_pOccupancyGrid;
00282   }; // CellUpdater
00283 
00285 
00286 }
00287 
00288 
00289 #endif // __OpenKarto_OccupancyGird_h__


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Thu Aug 27 2015 14:07:25