00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #ifndef COccupancyGridMap2D_H
00030 #define COccupancyGridMap2D_H
00031
00032 #include <mrpt/utils/CSerializable.h>
00033 #include <mrpt/utils/CLoadableOptions.h>
00034 #include <mrpt/utils/CMRPTImage.h>
00035 #include <mrpt/utils/CMRPTImageFloat.h>
00036 #include <mrpt/slam/CMetricMap.h>
00037 #include <mrpt/slam/CLandmarksMap.h>
00038
00039 #include <mrpt/utils/safe_pointers.h>
00040
00041 #include <mrpt/config.h>
00042
00043 #if !defined(OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS) && !defined(OCCUPANCY_GRIDMAP_CELL_SIZE_16BITS)
00044 #error One of OCCUPANCY_GRIDMAP_CELL_SIZE_16BITS or OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS must be defined.
00045 #endif
00046
00047 #if defined(OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS) && defined(OCCUPANCY_GRIDMAP_CELL_SIZE_16BITS)
00048 #error Only one of OCCUPANCY_GRIDMAP_CELL_SIZE_16BITS or OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS must be defined at a time.
00049 #endif
00050
00051
00052 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_16BITS
00053
00055 #define OCCGRID_CELLTYPE_MIN (-32768)
00056 #define OCCGRID_CELLTYPE_MAX (32767)
00057
00058 #define OCCGRID_P2LTABLE_SIZE (32767)
00059 #else
00060
00062 #define OCCGRID_CELLTYPE_MIN (-128)
00063 #define OCCGRID_CELLTYPE_MAX (127)
00064
00065 #define OCCGRID_P2LTABLE_SIZE (128)
00066 #endif
00067
00068
00069 #define OCCGRID_LOGODD_K (16)
00070 #define OCCGRID_LOGODD_K_INV (1.0f/OCCGRID_LOGODD_K)
00071
00072
00073 namespace mrpt
00074 {
00075 namespace poses { class CPose2D; }
00076 namespace slam
00077 {
00078 using namespace mrpt::poses;
00079 using namespace mrpt::utils;
00080
00081 class CObservation2DRangeScan;
00082 class CObservation;
00083 class CPointsMap;
00084
00085
00086 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( COccupancyGridMap2D, CMetricMap )
00087
00088
00109 class MRPTDLLIMPEXP COccupancyGridMap2D : public CMetricMap
00110 {
00111
00112 DEFINE_SERIALIZABLE( COccupancyGridMap2D )
00113
00114 public:
00115 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
00116
00118 typedef int8_t cellType;
00119 typedef uint8_t cellTypeUnsigned;
00120 #endif
00121 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_16BITS
00122
00124 typedef int16_t cellType;
00125 typedef uint16_t cellTypeUnsigned;
00126 #endif
00127
00128 protected:
00129
00130 friend class CMultiMetricMap;
00131 friend class CMultiMetricMapPDF;
00132
00137 std::vector<cellType> map;
00138
00141 uint32_t size_x,size_y;
00142
00145 float x_min,x_max,y_min,y_max;
00146
00149 float resolution;
00150
00153 std::vector<double> precomputedLikelihood;
00154 bool precomputedLikelihoodToBeRecomputed;
00155
00159 std::vector<unsigned char> basis_map;
00160
00165 std::vector<int> voronoi_diagram;
00166
00169 float voroni_free_threshold;
00170
00173 void freeMap();
00174
00177 static double H(double p);
00178
00181 inline void setCell_nocheck(int x,int y,float value)
00182 {
00183 map[x+y*size_x]=p2l(value);
00184 }
00185
00188 inline float getCell_nocheck(int x,int y) const
00189 {
00190 return l2p(map[x+y*size_x]);
00191 }
00192
00195 inline void setRawCell(unsigned int cellIndex, cellType b)
00196 {
00197 if (cellIndex<size_x*size_y)
00198 {
00199 map[cellIndex] = b;
00200 }
00201 }
00202
00205 static std::vector<float> entropyTable;
00206
00209 static std::vector<float> logoddsTable;
00210
00214 static std::vector<uint8_t> logoddsTable_255;
00215
00217 static float* logoddsTablePtr;
00218
00220 static uint8_t* logoddsTable_255Ptr;
00221
00223 static std::vector<cellType> p2lTable;
00224
00226 static cellType* p2lTablePtr;
00227
00228
00231 double computeObservationLikelihood_Consensus(
00232 const CObservation *obs,
00233 const CPose2D &takenFrom );
00234
00238 double computeObservationLikelihood_ConsensusOWA(
00239 const CObservation *obs,
00240 const CPose2D &takenFrom );
00241
00244 double computeObservationLikelihood_CellsDifference(
00245 const CObservation *obs,
00246 const CPose2D &takenFrom );
00247
00250 double computeObservationLikelihood_MI(
00251 const CObservation *obs,
00252 const CPose2D &takenFrom );
00253
00256 double computeObservationLikelihood_rayTracing(
00257 const CObservation *obs,
00258 const CPose2D &takenFrom );
00259
00262 double computeObservationLikelihood_likelihoodField_Thrun(
00263 const CObservation *obs,
00264 const CPose2D &takenFrom );
00265
00268 double computeObservationLikelihood_likelihoodField_II(
00269 const CObservation *obs,
00270 const CPose2D &takenFrom );
00271
00272
00273 public:
00274
00278 void updateCell(int x,int y, float v);
00279
00288 inline static void updateCell_fast_occupied(
00289 const unsigned &x,
00290 const unsigned &y,
00291 const cellType &logodd_obs,
00292 const cellType &thres,
00293 cellType *mapArray,
00294 const unsigned &_size_x)
00295 {
00296 cellType *theCell = mapArray + (x+y*_size_x);
00297 if (*theCell > thres )
00298 *theCell -= logodd_obs;
00299 else *theCell = OCCGRID_CELLTYPE_MIN;
00300 }
00301
00309 inline static void updateCell_fast_occupied(
00310 cellType *theCell,
00311 const cellType &logodd_obs,
00312 const cellType &thres )
00313 {
00314 if (*theCell > thres )
00315 *theCell -= logodd_obs;
00316 else *theCell = OCCGRID_CELLTYPE_MIN;
00317 }
00318
00327 inline static void updateCell_fast_free(
00328 const unsigned &x,
00329 const unsigned &y,
00330 const cellType &logodd_obs,
00331 const cellType &thres,
00332 cellType *mapArray,
00333 const unsigned &_size_x)
00334 {
00335 cellType *theCell = mapArray + (x+y*_size_x);
00336 if (*theCell < thres )
00337 *theCell += logodd_obs;
00338 else *theCell = OCCGRID_CELLTYPE_MAX;
00339 }
00340
00349 inline static void updateCell_fast_free(
00350 cellType *theCell,
00351 const cellType &logodd_obs,
00352 const cellType &thres)
00353 {
00354 if (*theCell < thres )
00355 *theCell += logodd_obs;
00356 else *theCell = OCCGRID_CELLTYPE_MAX;
00357 }
00358
00361 struct MRPTDLLIMPEXP TUpdateCellsInfoChangeOnly
00362 {
00363 TUpdateCellsInfoChangeOnly( bool enabled = false,
00364 double I_change = 0,
00365 int cellsUpdated=0) : enabled(enabled),
00366 I_change(I_change),
00367 cellsUpdated(cellsUpdated),
00368 laserRaysSkip(1)
00369 {
00370 }
00371
00374 bool enabled;
00375
00378 double I_change;
00379
00382 int cellsUpdated;
00383
00386 int laserRaysSkip;
00387 } updateInfoChangeOnly;
00388
00391 COccupancyGridMap2D( float min_x = -20.0f,
00392 float max_x = 20.0f,
00393 float min_y = -20.0f,
00394 float max_y = 20.0f,
00395 float resolution = 0.05f
00396 );
00397
00400 void clear( );
00401
00404 void fill(float default_value = 0.5f );
00405
00408 virtual ~COccupancyGridMap2D();
00409
00419 void setSize(float x_min,float x_max,float y_min,float y_max,float resolution,float default_value = 0.5f);
00420
00430 void resizeGrid(float new_x_min,float new_x_max,float new_y_min,float new_y_max,float new_cells_default_value = 0.5f, bool additionalMargin = true) MRPT_NO_THROWS;
00431
00434 unsigned int getSizeX() const { return size_x; }
00435
00438 unsigned int getSizeY() const { return size_y; }
00439
00442 float getXMin() const { return x_min; }
00443
00446 float getXMax() const { return x_max; }
00447
00450 float getYMin() const { return y_min; }
00451
00454 float getYMax() const { return y_max; }
00455
00458 float getResolution() const { return resolution; }
00459
00462 inline int x2idx(float x) const { return static_cast<int>((x-x_min)/resolution ); }
00463 inline int y2idx(float y) const { return static_cast<int>((y-y_min)/resolution ); }
00464
00465 inline int x2idx(double x) const { return static_cast<int>((x-x_min)/resolution ); }
00466 inline int y2idx(double y) const { return static_cast<int>((y-y_min)/resolution ); }
00467
00470 inline float idx2x(const size_t &cx) const { return x_min+(cx+0.5f)*resolution; }
00471 inline float idx2y(const size_t &cy) const { return y_min+(cy+0.5f)*resolution; }
00472
00475 inline int x2idx(float x,float x_min) const { return static_cast<int>((x-x_min)/resolution ); }
00476 inline int y2idx(float y, float y_min) const { return static_cast<int>((y-y_min)/resolution ); }
00477
00480 static inline float l2p(const cellType &l)
00481 {
00482 return logoddsTablePtr[ -OCCGRID_CELLTYPE_MIN+l ];
00483 }
00484
00487 static inline uint8_t l2p_255(const cellType &l)
00488 {
00489 return logoddsTable_255Ptr[ -OCCGRID_CELLTYPE_MIN+l ];
00490 }
00491
00494 static inline cellType p2l(const float &p)
00495 {
00496 return p2lTablePtr[ (int)(p * OCCGRID_P2LTABLE_SIZE) ];
00497 }
00498
00501 inline void setCell(int x,int y,float value)
00502 {
00503
00504 if (static_cast<unsigned int>(x)>=size_x || static_cast<unsigned int>(y)>=size_y)
00505 return;
00506 else map[x+y*size_x]=p2l(value);
00507 }
00508
00511 inline float getCell(int x,int y) const
00512 {
00513
00514 if (static_cast<unsigned int>(x)>=size_x || static_cast<unsigned int>(y)>=size_y)
00515 return 0.5f;
00516 else return l2p(map[x+y*size_x]);
00517 }
00518
00521 inline cellType *getRow( int cy ) { if (cy<0 || static_cast<unsigned int>(cy)>=size_y) return NULL; else return &map[0+cy*size_x]; }
00522
00525 inline const cellType *getRow( int cy ) const { if (cy<0 || static_cast<unsigned int>(cy)>=size_y) return NULL; else return &map[0+cy*size_x]; }
00526
00529 inline void setPos(float x,float y,float value) { setCell(x2idx(x),y2idx(y),value); }
00530
00533 inline float getPos(float x,float y) const { return getCell(x2idx(x),y2idx(y)); }
00534
00537 inline bool isStaticPos(float x,float y,float threshold = 0.7f) const { return isStaticCell(x2idx(x),y2idx(y),threshold); }
00538 inline bool isStaticCell(int cx,int cy,float threshold = 0.7f) const { return (getCell(cx,cy)<=threshold); }
00539
00542 inline void setBasisCell(int x,int y,unsigned char value) { basis_map[x+y*size_x]=value; };
00543
00546 inline unsigned char getBasisCell(int x,int y) const { return basis_map[x+y*size_x]; };
00547
00550 void computeCriticalPoints( CPointsMap &outPoints, int nPoints = 100 ) const;
00551
00557 void extractPanoramicFeatures(
00558 mrpt::slam::CLandmarksMap &outMap ,
00559 unsigned int nFeatures = 100,
00560 unsigned int nDirectionSectors = 32,
00561 unsigned int nRangeSectors = 8,
00562 float startDistance = 0.1f,
00563 float endDistance = 3.0f ) const;
00564
00568 void extractPanoramicFeaturesCached(
00569 mrpt::slam::CLandmarksMap &outMap ,
00570 unsigned int nFeatures = 100,
00571 unsigned int nDirectionSectors = 32,
00572 unsigned int nRangeSectors = 8,
00573 float startDistance = 0.1f,
00574 float endDistance = 3.0f ) const;
00575
00579 void resetPanoramicFeaturesCache();
00580
00584 struct MRPTDLLIMPEXP TEntropyInfo
00585 {
00586 TEntropyInfo() : H(0),I(0),mean_H(0),mean_I(0),effectiveMappedArea(0),effectiveMappedCells(0)
00587 {
00588 }
00589
00592 double H;
00593
00596 double I;
00597
00600 double mean_H;
00601
00604 double mean_I;
00605
00608 double effectiveMappedArea;
00609
00612 unsigned long effectiveMappedCells;
00613 };
00614
00618 class MRPTDLLIMPEXP TInsertionOptions : public mrpt::utils::CLoadableOptions
00619 {
00620 public:
00623 TInsertionOptions( );
00624
00636 void loadFromConfigFile(
00637 const mrpt::utils::CConfigFileBase &source,
00638 const std::string §ion);
00639
00642 void dumpToTextStream(CStream &out);
00643
00644
00647 float mapAltitude;
00648
00651 bool useMapAltitude;
00652
00655 float maxDistanceInsertion;
00656
00659 float maxOccupancyUpdateCertainty;
00660
00663 bool considerInvalidRangesAsFreeSpace;
00664
00667 uint16_t decimation;
00668
00670 float horizontalTolerance;
00671
00673 float CFD_features_gaussian_size;
00674
00676 float CFD_features_median_size;
00677
00678 bool wideningBeamsWithDistance;
00679
00680 };
00681
00685 TInsertionOptions insertionOptions;
00686
00689 enum TLikelihoodMethod
00690 {
00691 lmMeanInformation = 0,
00692 lmRayTracing,
00693 lmConsensus,
00694 lmCellsDifference,
00695 lmLikelihoodField_Thrun,
00696 lmLikelihoodField_II,
00697 lmConsensusOWA
00698 };
00699
00702 class MRPTDLLIMPEXP TLikelihoodOptions : public mrpt::utils::CLoadableOptions
00703 {
00704 public:
00707 TLikelihoodOptions();
00708
00720 void loadFromConfigFile(
00721 const mrpt::utils::CConfigFileBase &source,
00722 const std::string §ion);
00723
00726 void dumpToTextStream(CStream &out);
00727
00730 TLikelihoodMethod likelihoodMethod;
00731
00734 float LF_stdHit;
00735
00738 float LF_zHit, LF_zRandom;
00739
00742 float LF_maxRange;
00743
00746 uint32_t LF_decimation;
00747
00750 float LF_maxCorrsDistance;
00751
00754 bool LF_alternateAverageMethod;
00755
00758 float MI_exponent;
00759
00762 uint32_t MI_skip_rays;
00763
00766 float MI_ratio_max_distance;
00767
00770 bool rayTracing_useDistanceFilter;
00771
00774 int32_t rayTracing_decimation;
00775
00778 float rayTracing_stdHit;
00779
00782 int32_t consensus_takeEachRange;
00783
00786 float consensus_pow;
00787
00790 std::vector<float> OWA_weights;
00791
00794 bool enableLikelihoodCache;
00795
00796 } likelihoodOptions;
00797
00800 typedef std::pair<double,CPoint2D> TPairLikelihoodIndex;
00801
00804 class TLikelihoodOutput
00805 {
00806 public:
00807 TLikelihoodOutput() : OWA_pairList(), OWA_individualLikValues()
00808 {}
00809
00812 std::vector<TPairLikelihoodIndex> OWA_pairList;
00813
00816 std::vector<double> OWA_individualLikValues;
00817
00818 } likelihoodOutputs;
00819
00822 void subSample( int downRatio );
00823
00833 bool insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL );
00834
00839 void computeEntropy( TEntropyInfo &info ) const;
00840
00843 int getVoroniClearance(int cx,int cy) { return voronoi_diagram[cx+cy*size_x]; }
00844
00847 void setVoroniClearance(int cx,int cy,int dist) { voronoi_diagram[cx+cy*size_x]=dist; }
00848
00858 void buildVoronoiDiagram(float threshold, float robot_size,int x1=0,int x2=0, int y1=0,int y2=0);
00859
00865 void findCriticalPoints( float filter_distance );
00866
00878 int computeClearance( int cx, int cy, int *basis_x, int *basis_y, int *nBasis, bool GetContourPoint = false ) const;
00879
00883 float computeClearance( float x, float y, float maxSearchDistance ) const;
00884
00888 float computePathCost( float x1, float y1, float x2, float y2 ) const;
00889
00902 void laserScanSimulator(
00903 CObservation2DRangeScan &inout_Scan,
00904 const CPose2D &robotPose,
00905 float threshold = 0.5f,
00906 int N = 361,
00907 float noiseStd = 0,
00908 unsigned int decimation = 1,
00909 float angleNoiseStd = DEG2RAD(0) ) const;
00910
00922 double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom );
00923
00928 bool canComputeObservationLikelihood( const CObservation *obs );
00929
00935 double computeLikelihoodField_Thrun( const CPointsMap *pm, const CPose2D *relativePose = NULL);
00936
00942 double computeLikelihoodField_II( const CPointsMap *pm, const CPose2D *relativePose = NULL);
00943
00948 bool saveAsBitmapFile(const std::string &file) const;
00949
00953 bool saveAsBitmapFileWithLandmarks(
00954 const std::string &file,
00955 const mrpt::slam::CLandmarksMap *landmarks ) const;
00956
00961 static bool saveAsBitmapTwoMapsWithCorrespondences(
00962 const std::string &fileName,
00963 const COccupancyGridMap2D *m1,
00964 const COccupancyGridMap2D *m2,
00965 const CMetricMap::TMatchingPairList &corrs);
00966
00971 static bool saveAsEMFTwoMapsWithCorrespondences(
00972 const std::string &fileName,
00973 const COccupancyGridMap2D *m1,
00974 const COccupancyGridMap2D *m2,
00975 const CMetricMap::TMatchingPairList &corrs);
00976
00979 void getAsImage( utils::CMRPTImageFloat &img, bool verticalFlip = false ) const;
00980
00985 void getAsImage( utils::CMRPTImage &img, bool verticalFlip = false, bool forceRGB=false, bool tricolor = false) const;
00986
00991 void getAsImageFiltered( utils::CMRPTImage &img, bool verticalFlip = false, bool forceRGB=false) const;
00992
00995 void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
00996
00999 bool isEmpty() const;
01000
01009 bool loadFromBitmapFile(const std::string &file, float resolution, float xCentralPixel = -1, float yCentralPixel =-1 );
01010
01019 bool loadFromBitmap(const mrpt::utils::CMRPTImage &img, float resolution, float xCentralPixel = -1, float yCentralPixel =-1 );
01020
01026 void computeMatchingWith2D(
01027 const CMetricMap *otherMap,
01028 const CPose2D &otherMapPose,
01029 float maxDistForCorrespondence,
01030 float maxAngularDistForCorrespondence,
01031 const CPose2D &angularDistPivotPoint,
01032 TMatchingPairList &correspondences,
01033 float &correspondencesRatio,
01034 float *sumSqrDist = NULL,
01035 bool onlyKeepTheClosest = false,
01036 bool onlyUniqueRobust = false ) const;
01037
01038
01049 float compute3DMatchingRatio(
01050 const CMetricMap *otherMap,
01051 const CPose3D &otherMapPose,
01052 float minDistForCorr = 0.10f,
01053 float minMahaDistForCorr = 2.0f
01054 ) const;
01055
01058 void saveMetricMapRepresentationToFile(
01059 const std::string &filNamePrefix
01060 ) const;
01061
01066 struct MRPTDLLIMPEXP TCriticalPointsList
01067 {
01068 TCriticalPointsList() : x(),y(),clearance(),x_basis1(),y_basis1(),x_basis2(),y_basis2()
01069 {}
01070
01073 std::vector<int> x,y;
01076 std::vector<int> clearance;
01079 std::vector<int> x_basis1,y_basis1, x_basis2,y_basis2;
01080 } CriticalPointsList;
01081
01085 void auxParticleFilterCleanUp();
01086
01087
01088 private:
01091 mutable mrpt::slam::CLandmarksMapPtr m_panoramicFeaturesCache;
01092
01098 inline unsigned char GetNeighborhood( int cx, int cy ) const;
01099
01104 int direccion_vecino_x[8],direccion_vecino_y[8];
01105
01110 int direction2idx(int dx, int dy);
01111 };
01112
01113
01114 bool operator <(const COccupancyGridMap2D::TPairLikelihoodIndex &e1, const COccupancyGridMap2D::TPairLikelihoodIndex &e2);
01115
01116 }
01117 }
01118
01119 #endif