smmap.h
Go to the documentation of this file.
00001 #ifndef SMMAP_H
00002 #define SMMAP_H
00003 #include <gmapping/grid/map.h>
00004 #include <gmapping/grid/harray2d.h>
00005 #include <gmapping/utils/point.h>
00006 #define SIGHT_INC 1
00007 
00008 namespace GMapping {
00009 
00010 struct PointAccumulator{
00011         typedef point<float> FloatPoint;
00012         /* before 
00013         PointAccumulator(int i=-1): acc(0,0), n(0), visits(0){assert(i==-1);}
00014         */
00015         /*after begin*/
00016         PointAccumulator(): acc(0,0), n(0), visits(0){}
00017         PointAccumulator(int i): acc(0,0), n(0), visits(0){assert(i==-1);}
00018         /*after end*/
00019         inline void update(bool value, const Point& p=Point(0,0));
00020         inline Point mean() const {return 1./n*Point(acc.x, acc.y);}
00021         inline operator double() const { return visits?(double)n*SIGHT_INC/(double)visits:-1; }
00022         inline void add(const PointAccumulator& p) {acc=acc+p.acc; n+=p.n; visits+=p.visits; }
00023         static const PointAccumulator& Unknown();
00024         static PointAccumulator* unknown_ptr;
00025         FloatPoint acc;
00026         int n, visits;
00027         inline double entropy() const;
00028 };
00029 
00030 void PointAccumulator::update(bool value, const Point& p){
00031         if (value) {
00032                 acc.x+= static_cast<float>(p.x);
00033                 acc.y+= static_cast<float>(p.y); 
00034                 n++; 
00035                 visits+=SIGHT_INC;
00036         } else
00037                 visits++;
00038 }
00039 
00040 double PointAccumulator::entropy() const{
00041         if (!visits)
00042                 return -log(.5);
00043         if (n==visits || n==0)
00044                 return 0;
00045         double x=(double)n*SIGHT_INC/(double)visits;
00046         return -( x*log(x)+ (1-x)*log(1-x) );
00047 }
00048 
00049 
00050 typedef Map<PointAccumulator,HierarchicalArray2D<PointAccumulator> > ScanMatcherMap;
00051 
00052 };
00053 
00054 #endif 


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Fri Aug 28 2015 11:56:21