-
Notifications
You must be signed in to change notification settings - Fork 3
/
smmap.h
56 lines (49 loc) · 1.55 KB
/
smmap.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
#ifndef SMMAP_H
#define SMMAP_H
#include <grid/map.h>
#include <grid/harray2d.h>
#include <utils/point.h>
#define SIGHT_INC 1
namespace GMapping {
struct PointAccumulator{
typedef point<float> FloatPoint;
/* before
PointAccumulator(int i=-1): acc(0,0), n(0), visits(0){assert(i==-1);}
*/
/*after begin*/
PointAccumulator(): acc(0,0), n(0), visits(0){}
PointAccumulator(int i): acc(0,0), n(0), visits(0){assert(i==-1);}
/*after end*/
inline void update(bool value, const Point& p=Point(0,0));
inline Point mean() const {return 1./n*Point(acc.x, acc.y);}
inline operator double() const { return visits?(double)n*SIGHT_INC/(double)visits:-1; }
inline void add(const PointAccumulator& p) {acc=acc+p.acc; n+=p.n; visits+=p.visits; }
static const PointAccumulator& Unknown();
static PointAccumulator* unknown_ptr;
FloatPoint acc;
int n, visits;
inline double entropy() const;
};
// value = false 表示非占用,value = true 表示占用
// 非占用只增加 visits,占用时累加 n 和 visits,并累加占用坐标值
void PointAccumulator::update(bool value, const Point& p){
if (value) {
acc.x+= static_cast<float>(p.x);
acc.y+= static_cast<float>(p.y);
n++;
visits+=SIGHT_INC;
} else
visits++;
}
double PointAccumulator::entropy() const{
// 从未被访问过则置默认值 -log(0.5)
if (!visits)
return -log(.5);
if (n==visits || n==0)
return 0;
double x=(double)n*SIGHT_INC/(double)visits;
return -( x*log(x)+ (1-x)*log(1-x) );
}
typedef Map<PointAccumulator,HierarchicalArray2D<PointAccumulator> > ScanMatcherMap;
};
#endif