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
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
|
//=================================================================================================
// 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 __OccGridMapBase_h_
#define __OccGridMapBase_h_
#include "GridMapBase.h"
#include "../scan/DataPointContainer.h"
#include "../util/UtilFunctions.h"
#include <Eigen/Geometry>
namespace hectorslam {
template<typename ConcreteCellType, typename ConcreteGridFunctions>
class OccGridMapBase
: public GridMapBase<ConcreteCellType>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
OccGridMapBase(float mapResolution, const Eigen::Vector2i& size, const Eigen::Vector2f& offset)
: GridMapBase<ConcreteCellType>(mapResolution, size, offset)
, currUpdateIndex(0)
, currMarkOccIndex(-1)
, currMarkFreeIndex(-1)
{}
virtual ~OccGridMapBase() {}
void updateSetOccupied(int index)
{
concreteGridFunctions.updateSetOccupied(this->getCell(index));
}
void updateSetFree(int index)
{
concreteGridFunctions.updateSetFree(this->getCell(index));
}
void updateUnsetFree(int index)
{
concreteGridFunctions.updateUnsetFree(this->getCell(index));
}
float getGridProbabilityMap(int index) const
{
return concreteGridFunctions.getGridProbability(this->getCell(index));
}
bool isOccupied(int xMap, int yMap) const
{
return (this->getCell(xMap,yMap).isOccupied());
}
bool isFree(int xMap, int yMap) const
{
return (this->getCell(xMap,yMap).isFree());
}
bool isOccupied(int index) const
{
return (this->getCell(index).isOccupied());
}
bool isFree(int index) const
{
return (this->getCell(index).isFree());
}
float getObstacleThreshold() const
{
ConcreteCellType temp;
temp.resetGridCell();
return concreteGridFunctions.getGridProbability(temp);
}
void setUpdateFreeFactor(float factor)
{
concreteGridFunctions.setUpdateFreeFactor(factor);
}
void setUpdateOccupiedFactor(float factor)
{
concreteGridFunctions.setUpdateOccupiedFactor(factor);
}
/**
* Updates the map using the given scan data and robot pose
* @param dataContainer Contains the laser scan data
* @param robotPoseWorld The 2D robot pose in world coordinates
*/
void updateByScan(const DataContainer& dataContainer, const Eigen::Vector3f& robotPoseWorld)
{
currMarkFreeIndex = currUpdateIndex + 1;
currMarkOccIndex = currUpdateIndex + 2;
//Get pose in map coordinates from pose in world coordinates
Eigen::Vector3f mapPose(this->getMapCoordsPose(robotPoseWorld));
//Get a 2D homogenous transform that can be left-multiplied to a robot coordinates vector to get world coordinates of that vector
Eigen::Affine2f poseTransform((Eigen::Translation2f(
mapPose[0], mapPose[1]) * Eigen::Rotation2Df(mapPose[2])));
//Get start point of all laser beams in map coordinates (same for alle beams, stored in robot coords in dataContainer)
Eigen::Vector2f scanBeginMapf(poseTransform * dataContainer.getOrigo());
//Get integer vector of laser beams start point
Eigen::Vector2i scanBeginMapi(scanBeginMapf[0] + 0.5f, scanBeginMapf[1] + 0.5f);
//Get number of valid beams in current scan
int numValidElems = dataContainer.getSize();
//std::cout << "\n maxD: " << maxDist << " num: " << numValidElems << "\n";
//Iterate over all valid laser beams
for (int i = 0; i < numValidElems; ++i) {
//Get map coordinates of current beam endpoint
Eigen::Vector2f scanEndMapf(poseTransform * (dataContainer.getVecEntry(i)));
//std::cout << "\ns\n" << scanEndMapf << "\n";
//add 0.5 to beam endpoint vector for following integer cast (to round, not truncate)
scanEndMapf.array() += (0.5f);
//Get integer map coordinates of current beam endpoint
Eigen::Vector2i scanEndMapi(scanEndMapf.cast<int>());
//Update map using a bresenham variant for drawing a line from beam start to beam endpoint in map coordinates
if (scanBeginMapi != scanEndMapi){
updateLineBresenhami(scanBeginMapi, scanEndMapi);
}
}
//Tell the map that it has been updated
this->setUpdated();
//Increase update index (used for updating grid cells only once per incoming scan)
currUpdateIndex += 3;
}
inline void updateLineBresenhami( const Eigen::Vector2i& beginMap, const Eigen::Vector2i& endMap, unsigned int max_length = UINT_MAX){
int x0 = beginMap[0];
int y0 = beginMap[1];
//check if beam start point is inside map, cancel update if this is not the case
if ((x0 < 0) || (x0 >= this->getSizeX()) || (y0 < 0) || (y0 >= this->getSizeY())) {
return;
}
int x1 = endMap[0];
int y1 = endMap[1];
//std::cout << " x: "<< x1 << " y: " << y1 << " length: " << length << " ";
//check if beam end point is inside map, cancel update if this is not the case
if ((x1 < 0) || (x1 >= this->getSizeX()) || (y1 < 0) || (y1 >= this->getSizeY())) {
return;
}
int dx = x1 - x0;
int dy = y1 - y0;
unsigned int abs_dx = abs(dx);
unsigned int abs_dy = abs(dy);
int offset_dx = util::sign(dx);
int offset_dy = util::sign(dy) * this->sizeX;
unsigned int startOffset = beginMap.y() * this->sizeX + beginMap.x();
//if x is dominant
if(abs_dx >= abs_dy){
int error_y = abs_dx / 2;
bresenham2D(abs_dx, abs_dy, error_y, offset_dx, offset_dy, startOffset);
}else{
//otherwise y is dominant
int error_x = abs_dy / 2;
bresenham2D(abs_dy, abs_dx, error_x, offset_dy, offset_dx, startOffset);
}
unsigned int endOffset = endMap.y() * this->sizeX + endMap.x();
this->bresenhamCellOcc(endOffset);
}
inline void bresenhamCellFree(unsigned int offset)
{
ConcreteCellType& cell (this->getCell(offset));
if (cell.updateIndex < currMarkFreeIndex) {
concreteGridFunctions.updateSetFree(cell);
cell.updateIndex = currMarkFreeIndex;
}
}
inline void bresenhamCellOcc(unsigned int offset)
{
ConcreteCellType& cell (this->getCell(offset));
if (cell.updateIndex < currMarkOccIndex) {
//if this cell has been updated as free in the current iteration, revert this
if (cell.updateIndex == currMarkFreeIndex) {
concreteGridFunctions.updateUnsetFree(cell);
}
concreteGridFunctions.updateSetOccupied(cell);
//std::cout << " setOcc " << "\n";
cell.updateIndex = currMarkOccIndex;
}
}
inline void bresenham2D( unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b, unsigned int offset){
this->bresenhamCellFree(offset);
unsigned int end = abs_da-1;
for(unsigned int i = 0; i < end; ++i){
offset += offset_a;
error_b += abs_db;
if((unsigned int)error_b >= abs_da){
offset += offset_b;
error_b -= abs_da;
}
this->bresenhamCellFree(offset);
}
}
protected:
ConcreteGridFunctions concreteGridFunctions;
int currUpdateIndex;
int currMarkOccIndex;
int currMarkFreeIndex;
};
}
#endif
| cs |
template<typename ConcreteCellType, typename ConcreteGridFunctions>
여기서는 "클래스 템플릿"으로 작용한다.
"클래스 템플릿"이란 다음의 예를 통해 이해할 수 있다.
template <typename T>
class Box {
T data;
public :
Box() { }
void set(T value) {
data = value;
}
T get() {
return data;
}
};
int main()
{
Box<int> box;
box.set(100);
cout << box.get() << endl;
Box<double> box1;
box1.set(3.141592);
cout << box1.get() << endl;
return 0;
}
결과값
100
3.14159
즉, 다음과 같은 순서를 따르게 되는 것이다.
1. 클래스 템플릿을 만듦.
2. class 객체를 만들 때, 해당 템플릿의 typename이 어떤 자료형을 지닐지 언급해줌.
3. class 객체가 만들어지면서 해당 typename들은 2번에서 언급된 자료형으로 바뀌어 생성됨.
그리고 이는 typename이 여러개여도 같다.
class OccGridMapBase
: public GridMapBase<ConcreteCellType>
OccGridMapBase 클래스는 GridMapBase 클래스를 상속받아 생성되게 된다.
즉, 다음과 같은 것이다.
GridMapBase : Parent Class (부모 클래스)
OccGridMapBase : Child Class (자식 클래스)
이 때, GridMapBase 또한 클래스 템플릿이기 때문에 생성될 때, 자료형을 언급해줘야 하는 것이다.
그리고 여기에서는 OccGridMapBase 클래스 템플릿의 typename 중 하나인 ConcreteCellType을 가져와 GridMapBase의 자료형으로 건네주었다.
즉, 아직 ConcreteCellType 이란 자료형이 무엇인지 확정되지 않은 것이다.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
If you define a structure having members of fixed-size vectorizable Eigen types, you must overload its "operator new" so that it generates 16-bytes-aligned pointers. Fortunately, Eigen provides you with a macro EIGEN_MAKE_ALIGNED_OPERATOR_NEW that does that for you.
https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
OccGridMapBase(float mapResolution,
const Eigen::Vector2i& size,
const Eigen::Vector2f& offset)
const Eigen::Vector2i& size,
const Eigen::Vector2f& offset)
: GridMapBase<ConcreteCellType>(mapResolution, size, offset)
, currUpdateIndex(0)
, currMarkOccIndex(-1)
, currMarkFreeIndex(-1)
{}
생성자이다.
매개변수로 mapResolution 값과 size 값, offset 값을 받아 생성된다.
그리고 리스트 초기화를 통해 아래의 값들을 초기화 시켜준다.
: GridMapBase<ConcreteCellType>(mapResolution, size, offset)
, currUpdateIndex(0)
, currMarkOccIndex(-1)
, currMarkFreeIndex(-1)
virtual ~OccGridMapBase() {}
소멸자이다.
void updateSetOccupied(int index)
{
concreteGridFunctions.updateSetOccupied(this->getCell(index));
}
클래스가 생성될 때, 다음과 같은 멤버변수가 추가된다.
ConcreteGridFunctions concreteGridFunctions;
위 멤버변수의 의미는 ConcreteGridFunctions 자료형을 갖는 OccGridMapBase Class 객체이다.
그러므로 당연히 OccGridMapBase 내에 존재하는 함수들의 사용이 가능하다.
그러므로 당연히 다음의 함수도 사용 가능하게 된다.
concreteGridFunctions.updateSetOccupied(this->getCell(index));
이 때, getCell 함수는 Parent Class인 GridMapBase에 존재하는 함수이다.
ConcreteCellType& getCell(int x, int y)
{
return mapArray[y * sizeX + x];
}
const ConcreteCellType& getCell(int x, int y) const
{
return mapArray[y * sizeX + x];
}
ConcreteCellType& getCell(int index)
{
return mapArray[index];
}
const ConcreteCellType& getCell(int index) const
{
return mapArray[index];
}
해당 인덱스의 cell 값을 가져오게 된다.
그리고 updateSetOccupied 함수를 이용해 해당 Cell의 Occupied 여부를 Update 하게 된다.
void updateSetFree(int index)
{
concreteGridFunctions.updateSetFree(this->getCell(index));
}
void updateUnsetFree(int index)
{
concreteGridFunctions.updateUnsetFree(this->getCell(index));
}
해당 cell 이 free한지 아닌지 setting 하는 함수이다.
float getGridProbabilityMap(int index) const
{
return concreteGridFunctions.getGridProbability(this->getCell(index));
}
뭐지 이게 getGridProbability 함수는 어디서 튀어나왔지?
bool isOccupied(int xMap, int yMap) const
{
return (this->getCell(xMap,yMap).isOccupied());
}
bool isFree(int xMap, int yMap) const
{
return (this->getCell(xMap,yMap).isFree());
}
bool isOccupied(int index) const
{
return (this->getCell(index).isOccupied());
}
bool isFree(int index) const
{
return (this->getCell(index).isFree());
}
이건 뭐지? index를 주고 getCell이 작동하니 해당 cell의 값이 얻어질거고, isFree()를 써서 뭘 하겠단 거지?///
float getObstacleThreshold() const
{
ConcreteCellType temp;
temp.resetGridCell();
return concreteGridFunctions.getGridProbability(temp);
}
이건 뭐지 왜 만든거지 temp를 갖다가 return할거면?
void setUpdateFreeFactor(float factor)
{
concreteGridFunctions.setUpdateFreeFactor(factor);
}
void setUpdateOccupiedFactor(float factor)
{
concreteGridFunctions.setUpdateOccupiedFactor(factor);
}
얘네도 뭐지?
/**
* Updates the map using the given scan data and robot pose
* @param dataContainer Contains the laser scan data
* @param robotPoseWorld The 2D robot pose in world coordinates
*/
void updateByScan(const DataContainer& dataContainer,
const Eigen::Vector3f& robotPoseWorld)
const Eigen::Vector3f& robotPoseWorld)
{
Map을 Update하는 함수.
laser scan data와 2D robot pose를 받아서 map을 update 함.
currMarkFreeIndex = currUpdateIndex + 1;
currMarkOccIndex = currUpdateIndex + 2;
FreeIndex와 OccIndex.
어째서인지 FreeIndex는 1씩, OccIndex는 2씩 update함.
//Get pose in map coordinates from pose in world coordinates
Eigen::Vector3f mapPose(this->getMapCoordsPose(robotPoseWorld));
Parent Class에서 robotPoseWorld 값을 받아옴.
Eigen Library를 이용하면 매개변수를 줌으로써 생성자가 동작해
mapPose에 robotPoseWorld 값을 담을 수 있음.
//Get a 2D homogenous transform that can be left-multiplied to
//a robot coordinates vector to get world coordinates of that vector
//a robot coordinates vector to get world coordinates of that vector
Eigen::Affine2f poseTransform((Eigen::Translation2f(
mapPose[0], mapPose[1]) *
Eigen::Rotation2Df(mapPose[2])));
Eigen::Rotation2Df(mapPose[2])));
여기에서는 Eigen Library를 이용하여 Affine2f 형태의 Affine Matrix를 만들게 되는데,
이 때, Affine Matrix의 형태는 robot의 world coordinate에서 로봇의 위치인 mapPose[0], mapPose[1]을 이용해 Translation Matrix로 만들고, 로봇의 방향인 mapPose[2]를 이용해 Rotation Matrix로 만들어 두 Matrix를 곱함으로써 Affine Matrix를 만들게 된다.
즉, 현재 robot coordinates에 poseTransform이라는 Affine Matrix를 좌측에서 곱해주면 world coordinates로 변경시킬 수 있는 것이다.
//Get start point of all laser beams in map coordinates
//(same for alle beams, stored in robot coords in dataContainer)
//(same for alle beams, stored in robot coords in dataContainer)
Eigen::Vector2f scanBeginMapf(poseTransform * dataContainer.getOrigo());
dataContainer.getOrigo를 통해 map coordinates에서의 laser beams의 start point를 가져올 수 있다.
그리고 이 start point에 poseTransform을 좌측에서 곱해줌으로써,
Vector2f 형태의 scanBeginMapf라는 변수명을 갖는
world coordinate에서의 laserbeams의 start point를 얻게되는 것이다.
//Get integer vector of laser beams start point
Eigen::Vector2i scanBeginMapi(scanBeginMapf[0] + 0.5f, scanBeginMapf[1] + 0.5f);
integer 형태로 얻기 위함이다.
//Get number of valid beams in current scan
int numValidElems = dataContainer.getSize();
laser beams의 data들 중 유효한 data의 개수를 가져온다.
//Iterate over all valid laser beams
for (int i = 0; i < numValidElems; ++i) {
이제 유효한 laser beams의 갯수만큼 for문을 동작시킨다.
//Get map coordinates of current beam endpoint
Eigen::Vector2f scanEndMapf(poseTransform * (dataContainer.getVecEntry(i)));
각 laser beams 또한 map coordinates에서 world coordinates로 변환시킨다.
//add 0.5 to beam endpoint vector for following integer cast
//(to round, not truncate)
//(to round, not truncate)
scanEndMapf.array() += (0.5f);
//Get integer map coordinates of current beam endpoint
Eigen::Vector2i scanEndMapi(scanEndMapf.cast<int>());
integer cast를 해주기 위해 scanEndMapf 좌표 각각에 0.5f를 더해준다.
반올림 할 때, 0.5를 더하고 integer cast를 하면 올림으로 계산이 되므로
0.5f를 더하고 interger cast를 한 것이다.
그리고 scanEndMapi로 넣어준다.
//Update map using a bresenham variant for drawing a line
//from beam start to beam endpoint in map coordinates
//from beam start to beam endpoint in map coordinates
if (scanBeginMapi != scanEndMapi){
updateLineBresenhami(scanBeginMapi, scanEndMapi);
}
Bresenhami는 Line algorithm 중 하나인데, 점 좌표 두개를 받으면 그 두 Line을 이어준다.
https://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
//Tell the map that it has been updated
this->setUpdated();
Parent Class의 setUpdated() 함수를 이용해 map이 Update 되었음을 알려준다.
아래와 같은 함수이다.
void setUpdated() { lastUpdateIndex++; };
//Increase update index
//(used for updating grid cells only once per incoming scan)
//(used for updating grid cells only once per incoming scan)
currUpdateIndex += 3;
Index를 Update 해준다. 왜 3을 더해주는 걸까?
inline void updateLineBresenhami( const Eigen::Vector2i& beginMap,
const Eigen::Vector2i& endMap,
unsigned int max_length = UINT_MAX){
const Eigen::Vector2i& endMap,
unsigned int max_length = UINT_MAX){
inline 함수가 등장한다.
https://dojang.io/mod/page/view.php?id=748
위 링크를 참조하면, inline 함수는 작동면에서 일반 함수와 다를바 없지만,
컴파일러가 컴파일 할 때, inline 함수가 사용되는 부분에 함수를 그대로 가져오기 때문에
프로세스가 더 짧아진다. 그렇기 때문에 자주 호출되는 함수에 사용하면 유리하다.
(대신 함수가 사용되는 부분에 다시 사용되므로 실행파일의 크기가 커지는 단점이 있다.)
인자는 다음과 같다.
beginMap : laser scan start point
endMap : laser scan end point
max_length : 이 함수는 확실히 모르겠지만, 인자가 UINT_MAX인 것으로 보아 UINT의 범위 내에서 Line을 그리도록 하는 것 같다.
int x0 = beginMap[0];
int y0 = beginMap[1];
x0와 y0에 laser scan start point의 좌표를 넣어준다.
//check if beam start point is inside map, cancel update if this is not the case
if ((x0 < 0) || (x0 >= this->getSizeX()) || (y0 < 0) || (y0 >= this->getSizeY())) {
return;
}
만일 beam의 start point가 map 안에 존재하지 않는다면 작업을 취소한다.
int x1 = endMap[0];
int y1 = endMap[1];
x1와 y1에 laser scan end point의 좌표를 넣어준다.
//check if beam end point is inside map, cancel update if this is not the case
if ((x1 < 0) || (x1 >= this->getSizeX()) || (y1 < 0) || (y1 >= this->getSizeY())) {
return;
}
만일 beam의 end point가 map 안에 존재하지 않는다면 작업을 취소한다.
int dx = x1 - x0;
int dy = y1 - y0;
unsigned int abs_dx = abs(dx);
unsigned int abs_dy = abs(dy);
여기부터가 중요한데, 이 부분부터는 "A Rasterizing Algorithm for Drawing Curves"의 13page에 존재하는 code를 참고한 것으로서, bressenham algorithm을 좀 더 컴퓨팅에 효율적으로 만든 내용이다. 여기서는 자세한 설명을 하지 않을테니 꼭 해당 문서를 참고 바란다.
dx : laser scan start point와 laser scan end point 간의 x축 거리
dy : laser scan start point와 laser scan end point 간의 y축 거리
abs_dx, abs_dy : 절댓값
int offset_dx = util::sign(dx);
int offset_dy = util::sign(dy) * this->sizeX;
util::sign(dx)
: hector_slam의 library 중에 util/UtilFunctions.h 안을 참고하면 다음과 같은 함수가 있다.
1
2
3
4
|
static inline int sign(int x)
{
return x > 0 ? 1 : -1;
}
| cs |
입력된 값이 0보다 크다면 1을, 아니라면 -1을 반환한다.
즉 이 부분은 xy 좌표맵 상에서 x1에서 x0를 뺀 것이 양수라면 오른쪽으로 1씩 이동하면 된다는 의미이고, 음수라면 왼쪽으로 1씩 이동시키기 위해, offset_dx의 값을 1 또는 -1로 준 것이다.
util::sign(dy)*this->sizeX;
: 왜 sizeX를 곱해준 걸까?
y축으로 또한 위로 1 혹은 아래로 1로 이동하게 될 것이다.
그리고 offset은 또한 index를 의미하기 때문에 sizeX가 xy 좌표맵에서 한 줄을 의미하는 만큼 한 줄 위로 인덱스를 이동시키거나 한 줄 아래로 인덱스를 이동시키게 되는 것이다.
unsigned int startOffset = beginMap.y() * this->sizeX + beginMap.x();
beginMap.y()에는 laser start scan point의 y좌표가 존재할 것이다.
거기에 this->sizeX를 곱했단건 y의 좌표를 의미하는 것이다.
거기에 beginMap.x()를 더함으로써 laser start scan point의 pixel index를 얻었다 할 수 있을것이다.
//if x is dominant
if(abs_dx >= abs_dy){
int error_y = abs_dx / 2;
bresenham2D(abs_dx, abs_dy, error_y, offset_dx, offset_dy, startOffset);
}else{
//otherwise y is dominant
int error_x = abs_dy / 2;
bresenham2D(abs_dy, abs_dx, error_x, offset_dy, offset_dx, startOffset);
}
dominant는 우세하다, 더 크다 란 의미이다.
이는 bresengam algorithm의 과정인데,
처음에 dx와 dy 중 어느 것이 큰지 판별한 후,
큰 내용부터 algorithm을 적용하는 것이다.
다시 한번 읽어볼 필요가 있겠다.
unsigned int endOffset = endMap.y() * this->sizeX + endMap.x();
this->bresenhamCellOcc(endOffset);
endOffset도 얻어준 뒤, 이를 bresenhamCellOcc 함수에 매개변수로 입력한다.
왜냐하면 endOffset은 무조건 Occupied 하기 때문이다.
inline void bresenhamCellFree(unsigned int offset)
{
ConcreteCellType& cell (this->getCell(offset));
if (cell.updateIndex < currMarkFreeIndex) {
concreteGridFunctions.updateSetFree(cell);
cell.updateIndex = currMarkFreeIndex;
}
}
offset이 index의 의미를 갖는게 여기서 확실해졌다.
현재 작업중인 cell의 index가 currMarkFreeIndex보다 작으면 작업을 진행한다.
currMarkFreeIndex는 참고로 laser scan이 실행될 때 마다 1 씩 증가하는 index이다.
updateIndex라는 멤버변수는 ConcreteCellType 객체 안에 있는 멤버변수인 것 같은데, 대체 ConcreteCellType 녀석은 어디 존재하는 걸까?
inline void bresenhamCellOcc(unsigned int offset)
{
ConcreteCellType& cell (this->getCell(offset));
if (cell.updateIndex < currMarkOccIndex) {
//if this cell has been updated as free in the current iteration, revert this
if (cell.updateIndex == currMarkFreeIndex) {
concreteGridFunctions.updateUnsetFree(cell);
}
concreteGridFunctions.updateSetOccupied(cell);
//std::cout << " setOcc " << "\n";
cell.updateIndex = currMarkOccIndex;
}
}
CellOcc 함수의 경우 조금 다르다.
기존의 cell이 Free였는데 현재는 Occ로 검출되었다면,
UnsetFree를 해준 뒤, SetOccupied를 해준다.
inline void bresenham2D( unsigned int abs_da,
unsigned int abs_db,
int error_b,
int offset_a,
int offset_b,
unsigned int offset){
unsigned int abs_db,
int error_b,
int offset_a,
int offset_b,
unsigned int offset){
this->bresenhamCellFree(offset);
unsigned int end = abs_da-1;
for(unsigned int i = 0; i < end; ++i){
offset += offset_a;
error_b += abs_db;
if((unsigned int)error_b >= abs_da){
offset += offset_b;
error_b -= abs_da;
}
this->bresenhamCellFree(offset);
}
}
위 과정이 Line을 그리는 과정이다.
파라미터를 살펴보면, 보통 호출할 때 다음과 같이 한다.
//if x is dominant
if(abs_dx >= abs_dy){
int error_y = abs_dx / 2;
bresenham2D(abs_dx, abs_dy, error_y, offset_dx, offset_dy, startOffset);
}else{
//otherwise y is dominant
int error_x = abs_dy / 2;
bresenham2D(abs_dy, abs_dx, error_x, offset_dy, offset_dx, startOffset);
}
즉 파라미터는 위와 같은 것이다.
this->bresenhamCellFree(offset);
위 함수로 먼저 해당 cell을 free하게 만든다.
이 후에는 알고리즘에 따라 진행된다.
unsigned int end = abs_da-1;
위 내용은 for문이 0부터 시작하므로 1을 빼준 것이다.
offset += offset_a;
offset_a는 offset_dx 혹은 offset_dy로서, x, y 축 중 더 긴 축의 index를 의미한다.
offset은 startOffset으로 해당 cell의 index를 의미한다.
offset += offset_a;
offset += offset_b;
즉, 위와같은 내용은 가장 긴 축의 index를 먼저 더하고 다음으로 조건에 부응한다면 짧은 축의 index를 더해준다고 보면 된다.
더해준 이유는 start pixel부터 end pixel까지 x축 혹은 y축으로 (둘 중 더 긴 축) 한 칸씩 이동하면서 Rasterizing을 할 것이기 때문이다. Rasterizing은 쉽게말해 벡터를 비트맵화 한다고 생각하면 된다.
# error 계산 진행.
error에 관련된 code 부분은 이해하기 힘들 수 있다. 정리하자면 이렇다.(링크 참조 https://stackoverflow.com/questions/32251437/please-explain-this-bresenham-line-drawing-code-for-me/32252934#32252934)
1. Parametric Equation of a line
(X + t.Dx, Y + t.Dy )이 때 t는 0~1 의 값을 갖는 parameter이다.
. 는 곱하기로 여기면 되겠다.
(X, Y) 는 start point, (X + Dx, Y + Dy)는 end point 이다.
2. Turm it to digital line
먼저 가장 긴 축을 중심으로 파라미터를 증가시킬 것이므로 가장 긴 축을 구해준다.D = Max(|Dx|, |Dy|)
그리고 파라미터 t를 I로 치환한다.
t : range[0,1] -> I : range[0,D]
그리고 식을 변경해준다.
(X + I.Dx/D, Y + I.Dy/D)
3. Simplifies the Equation
D=Max(|Dx|,|Dy|)=Dx 라고 가정해보면 방정식은 다음과 같이 표현될 수 있다.(X + I, Y + I.Dy/Dx)
즉, x축에서 I만큼 증가할 때, y축에서는 I.Dy/Dx가 증가하는 것이다.
이 때, Dy/Dx 는 기울기를 의미하며, 당연한 얘기지만 기울기가 1보다 크면 45도이상이 되게 된다.
4. Real number problem
I.Dy/Dx는 나눗셈이 존재하므로 정수가 아닌 실수일 확률이 있다.이를 해결하기 위해 반올림 함수 round와 버림 함수 floor를 이용해 다음과 같이 표현할 수 있다.
round(I.Dy/Dx) = floor(I.Dy/Dx+1/2) = floor((I.Dy+Dx/2)/Dx)
이 때, I.Dy + Dx/2 가 Dx보다 크다면 기울기가 1 이상이므로 y축으로 이동해도 된다는 판단이 생기고, 1보다 작으면 x축으로만 이동해야 된다는 결론이 생긴다.
하지만 (I.Dy+Dx/2)/Dx 에도 여전히 2번의 나눗셈이 존재하는 문제가 생긴다.
이 문제를 해결하기 위해 우리는 (I.Dy+Dx/2)/Dx 식을 몫 Q와 나머지 R로 나누게 된다.
문자식으로 하나하나 나타내보면 다음과 같다.
N = I.Dy + Dx/2
Q = N / Dx (몫)
R = N % Dx (나머지)
I' = I + 1
N' = N + Dy ( 왜냐하면 N = I.Dy+Dx/2 -> N' = (I+1)Dy+Dx/2 = I.Dy + Dx/2 + Dy )
Q' = (N + Dy) / Dx
R' = (N + Dy) % Dx
위 식을 보면 알겠지만, I가 1씩 더해질 때 마다 Dy도 하나씩 더해지게 된다.
그러므로 다음과 같은 식이 존재하게 된다.
error_b += abs_db;
이 후, 다음 코드를 통하여, I.Dy + Dx/2 와 Dx 의 값을 비교하게 된다.
if((unsigned int)error_b >= abs_da){
I.Dy + Dx/2 가 더 크단 의미는, 기울기가 1 이상이며, 그러므로 y축으로 이동하는 것이 좋다는 판단이 되는 것이다.
아무래도 error_b는 N = I.Dy + Dx/2 로써 받아들이는게 좋겠다.
그러므로 아래 코드에 따라 y축으로 이동한다.
offset += offset_b;
이제 아래 코드에서 보듯이 Dx 값을 빼준다.
error_b -= abs_da;
이 부분은 다음과 같다.
만약 Dy가 5 Dx가 10이라고 해보자.
그리고 0,0 부터 10,5 까지 직선이 그어진다.
그러면 초기 error_b = 10/2 = 5
이후 Dy를 더하면 error_b = 5+5 = 10
이 후에 Dx 를 빼면 error_b = 10 - 10 = 0
그러면 error_b 가 Dx 값보다 작아지므로 y축으로는 이동하지 않고 x축으로만 이동하게 되는 것이다.
이후에 error_b에 Dy가 더해지면 error_b = 0 + 5 = 5
error_b 가 Dx 값보다 작아지므로 또 한번 x축으로 이동하고
이후에 error_b 에 Dy가 한번 더 더해지면 error_b = 5 + 5 = 10
error_b 값이 Dx 값과 같으므로 이번에는 y축으로 이동하게 된다.
그러니까 즉,
Dy + (1/2)Dx / Dx 는 Dx로 나누고 남은 나머지를 이용해 판단하므로, 값이 커지면 Dx만큼 Dy + (1/2)Dx 값에서 빼줌으로써 y축으로 올라갈 텀을 잡는 것이다.
=============================
=====================================
일단 리뷰는 이렇게 마무리 되었는데,
나머지 bresenham algorithm을 한번 더 읽어보고 좀 더 자세히 리뷰할 필요가 있겠다.
양질의 글 덕분에 Hector_slam 관련해서 많이 배우고 갑니다!
답글삭제