GridMapLogOdds.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
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
|
//=================================================================================================
// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
// All rights reserved.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of the Simulation, Systems Optimization and Robotics
// group, TU Darmstadt nor the names of its contributors may be used to
// endorse or promote products derived from this software without
// specific prior written permission.
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================
#ifndef __GridMapLogOdds_h_
#define __GridMapLogOdds_h_
#include <cmath>
/**
* Provides a log odds of occupancy probability representation for cells in a occupancy grid map.
*/
class LogOddsCell
{
public:
/*
void setOccupied()
{
logOddsVal += 0.7f;
};
void setFree()
{
logOddsVal -= 0.4f;
};
*/
/**
* Sets the cell value to val.
* @param val The log odds value.
*/
void set(float val)
{
logOddsVal = val;
}
/**
* Returns the value of the cell.
* @return The log odds value.
*/
float getValue() const
{
return logOddsVal;
}
/**
* Returns wether the cell is occupied.
* @return Cell is occupied
*/
bool isOccupied() const
{
return logOddsVal > 0.0f;
}
bool isFree() const
{
return logOddsVal < 0.0f;
}
/**
* Reset Cell to prior probability.
*/
void resetGridCell()
{
logOddsVal = 0.0f;
updateIndex = -1;
}
//protected:
public:
float logOddsVal; ///< The log odds representation of occupancy probability.
int updateIndex;
};
/**
* Provides functions related to a log odds of occupancy probability respresentation for cells in a occupancy grid map.
*/
class GridMapLogOddsFunctions
{
public:
/**
* Constructor, sets parameters like free and occupied log odds ratios.
*/
GridMapLogOddsFunctions()
{
this->setUpdateFreeFactor(0.4f);
this->setUpdateOccupiedFactor(0.6f);
/*
//float probOccupied = 0.6f;
float probOccupied = 0.9f;
float oddsOccupied = probOccupied / (1.0f - probOccupied);
logOddsOccupied = log(oddsOccupied);
float probFree = 0.4f;
float oddsFree = probFree / (1.0f - probFree);
logOddsFree = log(oddsFree);
*/
}
/**
* Update cell as occupied
* @param cell The cell.
*/
void updateSetOccupied(LogOddsCell& cell) const
{
if (cell.logOddsVal < 50.0f){
cell.logOddsVal += logOddsOccupied;
}
}
/**
* Update cell as free
* @param cell The cell.
*/
void updateSetFree(LogOddsCell& cell) const
{
cell.logOddsVal += logOddsFree;
}
void updateUnsetFree(LogOddsCell& cell) const
{
cell.logOddsVal -= logOddsFree;
}
/**
* Get the probability value represented by the grid cell.
* @param cell The cell.
* @return The probability
*/
float getGridProbability(const LogOddsCell& cell) const
{
float odds = exp(cell.logOddsVal);
return odds / (odds + 1.0f);
/*
float val = cell.logOddsVal;
//prevent #IND when doing exp(large number).
if (val > 50.0f) {
return 1.0f;
} else {
float odds = exp(val);
return odds / (odds + 1.0f);
}
*/
//return 0.5f;
}
//void getGridValueMap( const LogOddsCell& cell) const{};
//void isOccupied(LogOddsCell& cell) {};
//void resetGridCell() {};
void setUpdateFreeFactor(float factor)
{
logOddsFree = probToLogOdds(factor);
}
void setUpdateOccupiedFactor(float factor)
{
logOddsOccupied = probToLogOdds(factor);
}
protected:
float probToLogOdds(float prob)
{
float odds = prob / (1.0f - prob);
return log(odds);
}
float logOddsOccupied; /// < The log odds representation of probability used for updating cells as occupied
float logOddsFree; /// < The log odds representation of probability used for updating cells as free
};
#endif
| cs |
/**
* Provides a log odds of occupancy probability representation for cells in a occupancy grid map.
*/
class LogOddsCell
{
public:
이게 진짜 log odds 를 이용한 class이다.
/**
* Sets the cell value to val.
* @param val The log odds value.
*/
void set(float val)
{
logOddsVal = val;
}
/**
* Returns the value of the cell.
* @return The log odds value.
*/
float getValue() const
{
return logOddsVal;
}
주석 참고
/**
* Returns wether the cell is occupied.
* @return Cell is occupied
*/
bool isOccupied() const
{
return logOddsVal > 0.0f;
}
bool isFree() const
{
return logOddsVal < 0.0f;
}
주석 참고.
특이점은 기준 값이 0.0f 란 것이다.
/**
* Reset Cell to prior probability.
*/
void resetGridCell()
{
logOddsVal = 0.0f;
updateIndex = -1;
}
주석 참고
public:
float logOddsVal; ///< The log odds representation of occupancy probability.
int updateIndex;
주석 참고
/**
* Provides functions related to a log odds of occupancy probability respresentation for cells in a occupancy grid map.
*/
class GridMapLogOddsFunctions
{
주석 참고
float logOddsOccupied; /// < The log odds representation of probability used for updating cells as occupied
float logOddsFree; /// < The log odds representation of probability used for updating cells as free
일단 클래스의 멤버변수부터 확인.
/**
* Constructor, sets parameters like free and occupied log odds ratios.
*/
GridMapLogOddsFunctions()
{
this->setUpdateFreeFactor(0.4f);
this->setUpdateOccupiedFactor(0.6f);
/*
//float probOccupied = 0.6f;
float probOccupied = 0.9f;
float oddsOccupied = probOccupied / (1.0f - probOccupied);
logOddsOccupied = log(oddsOccupied);
float probFree = 0.4f;
float oddsFree = probFree / (1.0f - probFree);
logOddsFree = log(oddsFree);
*/
}
생성자에서 파라미터값을 초기화 해준다.
setUpdateFreeFactor() 와 setUpdateOccupiedFactor 의 역할은 아래에 나와있다.
void setUpdateFreeFactor(float factor)
{
logOddsFree = probToLogOdds(factor);
}
void setUpdateOccupiedFactor(float factor)
{
logOddsOccupied = probToLogOdds(factor);
}
logOddsFree / logOddsOccupied 의 값을 정해준다.
이 때, probToLogOdds 함수를 사용한다.
해당 내용을 살펴보자.
float probToLogOdds(float prob)
{
float odds = prob / (1.0f - prob);
return log(odds);
}
생성자의 다음 부분을 살펴보자.
this->setUpdateFreeFactor(0.4f);
this->setUpdateOccupiedFactor(0.6f);
결국 저 위치의 파라미터 값이 probToLogOdds에 파라미터로 들어오게 된다.
확률 0% ~ 100% 를 0 ~ 1 로 보았을 때,
초기 Free의 확률을 0.4f / 초기 Occupied의 확률을 0.6f로 정의하려는 것이다.
이 때, 확률값을 받아서 odds를 정의해 주는데,
아래와 같은 식을 이용한다.
float odds = prob / (1.0f - prob);
odds는 probabilistic robotics 의 4.1.4 부분을 보면 알 수 있다.
odds : 어떠한 일이 일어날 확률 prob 를 어떠한 일이 일어나지 않을 확률로 나눈 것.
즉, 해당 cell이 occupancy 할 확률을 해당 cell이 occupancy하지 않을 확률로 나눈 것이다.
이렇게 하면 free할 확률과 occupied할 확률이 원래 확률보다 차이가 크게 벌어지게 된다.
return log(odds);
해당 값에 log를 취해주면 다음 그래프와 같이 -에서 + 값 까지 얻어지게 되는 것이다.
free의 경우 처음 확률이 0.4 / odds는 0.4/0.6=0.66... / log(odds)는 log(0.66)=-0.18...
occupied의 경우 처음 확률이 0.6 / odds는 0.6/0.4=1.5 / log(odds)는 log(1.5)=0.176...
이 되는 것이다.
/**
* Update cell as occupied
* @param cell The cell.
*/
void updateSetOccupied(LogOddsCell& cell) const
{
if (cell.logOddsVal < 50.0f){
cell.logOddsVal += logOddsOccupied;
}
}
cell이 지닌 logOddsVal 를 확인하고 해당 값이 50.0f보다 작다면 윗단계에서 계산된 logOddsOccupied (0.176...)를 cell의 logOddsVal에 더하게 된다.
/**
* Update cell as free
* @param cell The cell.
*/
void updateSetFree(LogOddsCell& cell) const
{
cell.logOddsVal += logOddsFree;
}
void updateUnsetFree(LogOddsCell& cell) const
{
cell.logOddsVal -= logOddsFree;
}
반대의 경우 cell의 logOddsVal에 logOddsFree(-0.18...) 값을 더하게 된다.
Unset의 경우는 빼준다.
/**
* Get the probability value represented by the grid cell.
* @param cell The cell.
* @return The probability
*/
float getGridProbability(const LogOddsCell& cell) const
{
float odds = exp(cell.logOddsVal);
return odds / (odds + 1.0f);
/*
float val = cell.logOddsVal;
//prevent #IND when doing exp(large number).
if (val > 50.0f) {
return 1.0f;
} else {
float odds = exp(val);
return odds / (odds + 1.0f);
}
*/
//return 0.5f;
}
float odds = exp(cell.logOddsVal);
return odds / (odds + 1.0f);
probabilistic robotics를 참고하면,
최종적인 probability를 구하기 위한 식은 다음과 같다.
prob = 1 - {1 / (1+exp(log(odds)))}
이 때, 1은 (1+exp(log(odds))) / (1+exp(log(odds))) 이므로
(1+exp(log(odds))) / (1+exp(log(odds))) - {1 / (1+exp(log(odds)))} 는
exp(log(odds)) / (exp(log(odds)) + 1) 이 된다.
이 과정은 sigmoid function을 이용하여 최종 값을 안정적으로 0과 1 사이의 값으로 얻어내는 방법이다.
댓글 없음:
댓글 쓰기