GridMapBase.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
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
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
|
#ifndef __GridMapBase_h_
#define __GridMapBase_h_
#include <Eigen/Geometry>
#include <Eigen/LU>
#include "MapDimensionProperties.h"
namespace hectorslam {
/**
* GridMapBase provides basic grid map functionality (creates grid , provides transformation from/to world coordinates).
* It serves as the base class for different map representations that may extend it's functionality.
*/
template<typename ConcreteCellType>
class GridMapBase
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* Indicates if given x and y are within map bounds
* @return True if coordinates are within map bounds
*/
bool hasGridValue(int x, int y) const
{
return (x >= 0) && (y >= 0) && (x < this->getSizeX()) && (y < this->getSizeY());
}
const Eigen::Vector2i& getMapDimensions() const { return mapDimensionProperties.getMapDimensions(); };
int getSizeX() const { return mapDimensionProperties.getSizeX(); };
int getSizeY() const { return mapDimensionProperties.getSizeY(); };
bool pointOutOfMapBounds(const Eigen::Vector2f& pointMapCoords) const
{
return mapDimensionProperties.pointOutOfMapBounds(pointMapCoords);
}
virtual void reset()
{
this->clear();
}
/**
* Resets the grid cell values by using the resetGridCell() function.
*/
void clear()
{
int size = this->getSizeX() * this->getSizeY();
for (int i = 0; i < size; ++i) {
this->mapArray[i].resetGridCell();
}
//this->mapArray[0].set(1.0f);
//this->mapArray[size-1].set(1.0f);
}
const MapDimensionProperties& getMapDimProperties() const { return mapDimensionProperties; };
/**
* Constructor, creates grid representation and transformations.
*/
GridMapBase(float mapResolution, const Eigen::Vector2i& size, const Eigen::Vector2f& offset)
: mapArray(0)
, lastUpdateIndex(-1)
{
Eigen::Vector2i newMapDimensions (size);
this->setMapGridSize(newMapDimensions);
sizeX = size[0];
setMapTransformation(offset, mapResolution);
this->clear();
}
/**
* Destructor
*/
virtual ~GridMapBase()
{
deleteArray();
}
/**
* Allocates memory for the two dimensional pointer array for map representation.
*/
void allocateArray(const Eigen::Vector2i& newMapDims)
{
int sizeX = newMapDims.x();
int sizeY = newMapDims.y();
mapArray = new ConcreteCellType [sizeX*sizeY];
mapDimensionProperties.setMapCellDims(newMapDims);
}
void deleteArray()
{
if (mapArray != 0){
delete[] mapArray;
mapArray = 0;
mapDimensionProperties.setMapCellDims(Eigen::Vector2i(-1,-1));
}
}
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];
}
void setMapGridSize(const Eigen::Vector2i& newMapDims)
{
if (newMapDims != mapDimensionProperties.getMapDimensions() ){
deleteArray();
allocateArray(newMapDims);
this->reset();
}
}
/**
* Copy Constructor, only needed if pointer members are present.
*/
GridMapBase(const GridMapBase& other)
{
allocateArray(other.getMapDimensions());
*this = other;
}
/**
* Assignment operator, only needed if pointer members are present.
*/
GridMapBase& operator=(const GridMapBase& other)
{
if ( !(this->mapDimensionProperties == other.mapDimensionProperties)){
this->setMapGridSize(other.mapDimensionProperties.getMapDimensions());
}
this->mapDimensionProperties = other.mapDimensionProperties;
this->worldTmap = other.worldTmap;
this->mapTworld = other.mapTworld;
this->worldTmap3D = other.worldTmap3D;
this->scaleToMap = other.scaleToMap;
//@todo potential resize
int sizeX = this->getSizeX();
int sizeY = this->getSizeY();
size_t concreteCellSize = sizeof(ConcreteCellType);
memcpy(this->mapArray, other.mapArray, sizeX*sizeY*concreteCellSize);
return *this;
}
/**
* Returns the world coordinates for the given map coords.
*/
inline Eigen::Vector2f getWorldCoords(const Eigen::Vector2f& mapCoords) const
{
return worldTmap * mapCoords;
}
/**
* Returns the map coordinates for the given world coords.
*/
inline Eigen::Vector2f getMapCoords(const Eigen::Vector2f& worldCoords) const
{
return mapTworld * worldCoords;
}
/**
* Returns the world pose for the given map pose.
*/
inline Eigen::Vector3f getWorldCoordsPose(const Eigen::Vector3f& mapPose) const
{
Eigen::Vector2f worldCoords (worldTmap * mapPose.head<2>());
return Eigen::Vector3f(worldCoords[0], worldCoords[1], mapPose[2]);
}
/**
* Returns the map pose for the given world pose.
*/
inline Eigen::Vector3f getMapCoordsPose(const Eigen::Vector3f& worldPose) const
{
Eigen::Vector2f mapCoords (mapTworld * worldPose.head<2>());
return Eigen::Vector3f(mapCoords[0], mapCoords[1], worldPose[2]);
}
void setDimensionProperties(const Eigen::Vector2f& topLeftOffsetIn, const Eigen::Vector2i& mapDimensionsIn, float cellLengthIn)
{
setDimensionProperties(MapDimensionProperties(topLeftOffsetIn,mapDimensionsIn,cellLengthIn));
}
void setDimensionProperties(const MapDimensionProperties& newMapDimProps)
{
//Grid map cell number has changed
if (!newMapDimProps.hasEqualDimensionProperties(this->mapDimensionProperties)){
this->setMapGridSize(newMapDimProps.getMapDimensions());
}
//Grid map transformation/cell size has changed
if(!newMapDimProps.hasEqualTransformationProperties(this->mapDimensionProperties)){
this->setMapTransformation(newMapDimProps.getTopLeftOffset(), newMapDimProps.getCellLength());
}
}
/**
* Set the map transformations
* @param xWorld The origin of the map coordinate system on the x axis in world coordinates
* @param yWorld The origin of the map coordinate system on the y axis in world coordinates
* @param The cell length of the grid map
*/
void setMapTransformation(const Eigen::Vector2f& topLeftOffset, float cellLength)
{
mapDimensionProperties.setCellLength(cellLength);
mapDimensionProperties.setTopLeftOffset(topLeftOffset);
scaleToMap = 1.0f / cellLength;
mapTworld = Eigen::AlignedScaling2f(scaleToMap, scaleToMap) * Eigen::Translation2f(topLeftOffset[0], topLeftOffset[1]);
worldTmap3D = Eigen::AlignedScaling3f(scaleToMap, scaleToMap, 1.0f) * Eigen::Translation3f(topLeftOffset[0], topLeftOffset[1], 0);
//std::cout << worldTmap3D.matrix() << std::endl;
worldTmap3D = worldTmap3D.inverse();
worldTmap = mapTworld.inverse();
}
/**
* Returns the scale factor for one unit in world coords to one unit in map coords.
* @return The scale factor
*/
float getScaleToMap() const
{
return scaleToMap;
}
/**
* Returns the cell edge length of grid cells in millimeters.
* @return the cell edge length in millimeters.
*/
float getCellLength() const
{
return mapDimensionProperties.getCellLength();
}
/**
* Returns a reference to the homogenous 2D transform from map to world coordinates.
* @return The homogenous 2D transform.
*/
const Eigen::Affine2f& getWorldTmap() const
{
return worldTmap;
}
/**
* Returns a reference to the homogenous 3D transform from map to world coordinates.
* @return The homogenous 3D transform.
*/
const Eigen::Affine3f& getWorldTmap3D() const
{
return worldTmap3D;
}
/**
* Returns a reference to the homogenous 2D transform from world to map coordinates.
* @return The homogenous 2D transform.
*/
const Eigen::Affine2f& getMapTworld() const
{
return mapTworld;
}
void setUpdated() { lastUpdateIndex++; };
int getUpdateIndex() const { return lastUpdateIndex; };
/**
* Returns the rectangle ([xMin,yMin],[xMax,xMax]) containing non-default cell values
*/
bool getMapExtends(int& xMax, int& yMax, int& xMin, int& yMin) const
{
int lowerStart = -1;
int upperStart = 10000;
int xMaxTemp = lowerStart;
int yMaxTemp = lowerStart;
int xMinTemp = upperStart;
int yMinTemp = upperStart;
int sizeX = this->getSizeX();
int sizeY = this->getSizeY();
for (int x = 0; x < sizeX; ++x) {
for (int y = 0; y < sizeY; ++y) {
if (this->mapArray[x][y].getValue() != 0.0f) {
if (x > xMaxTemp) {
xMaxTemp = x;
}
if (x < xMinTemp) {
xMinTemp = x;
}
if (y > yMaxTemp) {
yMaxTemp = y;
}
if (y < yMinTemp) {
yMinTemp = y;
}
}
}
}
if ((xMaxTemp != lowerStart) &&
(yMaxTemp != lowerStart) &&
(xMinTemp != upperStart) &&
(yMinTemp != upperStart)) {
xMax = xMaxTemp;
yMax = yMaxTemp;
xMin = xMinTemp;
yMin = yMinTemp;
return true;
} else {
return false;
}
}
protected:
ConcreteCellType *mapArray; ///< Map representation used with plain pointer array.
float scaleToMap; ///< Scaling factor from world to map.
Eigen::Affine2f worldTmap; ///< Homogenous 2D transform from map to world coordinates.
Eigen::Affine3f worldTmap3D; ///< Homogenous 3D transform from map to world coordinates.
Eigen::Affine2f mapTworld; ///< Homogenous 2D transform from world to map coordinates.
MapDimensionProperties mapDimensionProperties;
int sizeX;
private:
int lastUpdateIndex;
};
}
#endif
| cs |
template<typename ConcreteCellType>
template<typename T>
T get_max(T x, T y)
{
if(x>y) return x;
else return y;
}
예를 들어 위와 같은 템플릿 함수가 존재한다고 할 때,
int x = 20, int y = 30;
get_max(x, y);
위와 같이 함수를 호출하면
T는 int로써 작용하게 된다.
그러므로 아래의 경우엔
template<typename ConcreteCellType>
ConcreteCellType *mapArray; ///< Map representation used with plain pointer array.
ConcreteCellType은 pointer array로써 작용하게 되는 것이다.
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
/**
* Indicates if given x and y are within map bounds
* @return True if coordinates are within map bounds
*/
bool hasGridValue(int x, int y) const
{
return (x >= 0) && (y >= 0) && (x < this->getSizeX()) && (y < this->getSizeY());
}
const Eigen::Vector2i& getMapDimensions() const { return mapDimensionProperties.getMapDimensions(); };
int getSizeX() const { return mapDimensionProperties.getSizeX(); };
int getSizeY() const { return mapDimensionProperties.getSizeY(); };
getDimensions() / getSizeX() / getSizeY() 함수를 통해 mapDimensionProperties 가 갖고있는
map의 Dimension properties 값들을 가져올 수 있다.
hasGridValue 함수의 인자들이 0보다는 크고, mapDimensionProperties의 x, y 값보다는 작다면
coordinates 들이 map bounds 안에 존재한다고 판단한다.
bool pointOutOfMapBounds(const Eigen::Vector2f& pointMapCoords) const
{
return mapDimensionProperties.pointOutOfMapBounds(pointMapCoords);
}
mapDimensionProperties 로부터 pointMapCoords 인자에 대한 pointOutOfMapBounds 값을 받아 return 한다.
true면 coordinates가 map bounds 에서 벗어난다는 의미이고,
false면 coordinates가 map bounds 안에 존재한다는 의미이다.
virtual void reset()
{
this->clear();
}
/**
* Resets the grid cell values by using the resetGridCell() function.
*/
void clear()
{
int size = this->getSizeX() * this->getSizeY();
for (int i = 0; i < size; ++i) {
this->mapArray[i].resetGridCell();
}
//this->mapArray[0].set(1.0f);
//this->mapArray[size-1].set(1.0f);
}
resetGridCell() 함수가 어디에서 비롯되었는지는 모르겠지만,
일단 위의 코드 자체는 각 GridCell들을 reset시키는 간단한 코드이다.
const MapDimensionProperties& getMapDimProperties() const { return mapDimensionProperties; };
MapDimensionProperties를 포인터 채로 가져온다.
/**
* Constructor, creates grid representation and transformations.
*/
GridMapBase(float mapResolution, const Eigen::Vector2i& size, const Eigen::Vector2f& offset)
: mapArray(0)
, lastUpdateIndex(-1)
{
Eigen::Vector2i newMapDimensions (size);
this->setMapGridSize(newMapDimensions);
sizeX = size[0];
setMapTransformation(offset, mapResolution);
this->clear();
}
GridMapBase Class의 생성자.
생성될 때, MapDimensionsProperties 클래스의 멤버변수 값들을 인자로 받아 건네줌.
mapArray와 lastUpdateIndex 값도 리스트 초기화를 통해 생성함.
마지막으로 this->clear(); 를 통해 grid cell 들을 reset 시켜줌.
/**
* Destructor
*/
virtual ~GridMapBase()
{
deleteArray();
}
void deleteArray()
{
if (mapArray != 0){
delete[] mapArray;
mapArray = 0;
mapDimensionProperties.setMapCellDims(Eigen::Vector2i(-1,-1));
}
}
소멸자 함수에서는 메모리를 비워준다.
/**
* Allocates memory for the two dimensional pointer array for map representation.
*/
void allocateArray(const Eigen::Vector2i& newMapDims)
{
int sizeX = newMapDims.x();
int sizeY = newMapDims.y();
mapArray = new ConcreteCellType [sizeX*sizeY];
mapDimensionProperties.setMapCellDims(newMapDims);
}
newMapDims에서 맵 사이즈를 수신하면,
ConcreteCellType의 Array를 [sizeX*sizeY]의 크기만큼 할당해줌.
mapDimensionProperties 에도 MapDimensions 값을 저장해줌.
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 위치를 이용해 Cell 값을 획득함.
void setMapGridSize(const Eigen::Vector2i& newMapDims)
{
if (newMapDims != mapDimensionProperties.getMapDimensions() ){
deleteArray();
allocateArray(newMapDims);
this->reset();
}
}
MapGridSize를 setting할 때, 기존의 MapDimensions와 같지 않은 경우에만
Array 를 비우고,
Array Memory 를 다시 할당하고,
Grid Cell 을 모두 비워줌.
/**
* Copy Constructor, only needed if pointer members are present.
*/
GridMapBase(const GridMapBase& other)
{
allocateArray(other.getMapDimensions());
*this = other;
}
복사 생성자.
/**
* Assignment operator, only needed if pointer members are present.
*/
GridMapBase& operator=(const GridMapBase& other)
{
if ( !(this->mapDimensionProperties == other.mapDimensionProperties)){
this->setMapGridSize(other.mapDimensionProperties.getMapDimensions());
}
this->mapDimensionProperties = other.mapDimensionProperties;
this->worldTmap = other.worldTmap;
this->mapTworld = other.mapTworld;
this->worldTmap3D = other.worldTmap3D;
this->scaleToMap = other.scaleToMap;
//@todo potential resize
int sizeX = this->getSizeX();
int sizeY = this->getSizeY();
size_t concreteCellSize = sizeof(ConcreteCellType);
memcpy(this->mapArray, other.mapArray, sizeX*sizeY*concreteCellSize);
return *this;
}
GridMapBase 간에 "=" 연산을 수행.
GridMapBase a;
GridMapBase b;
a=b;
와같은 연산이 가능해지도록 함.
/**
* Returns the world coordinates for the given map coords.
*/
inline Eigen::Vector2f getWorldCoords(const Eigen::Vector2f& mapCoords) const
{
return worldTmap * mapCoords;
}
map coordinates 에서 world coordinates 로 변환시켜 반환함.
/**
* Returns the map coordinates for the given world coords.
*/
inline Eigen::Vector2f getMapCoords(const Eigen::Vector2f& worldCoords) const
{
return mapTworld * worldCoords;
}
world coordinates 에서 map coordinates 로 변환시켜 반환함.
/**
* Returns the world pose for the given map pose.
*/
inline Eigen::Vector3f getWorldCoordsPose(const Eigen::Vector3f& mapPose) const
{
Eigen::Vector2f worldCoords (worldTmap * mapPose.head<2>());
return Eigen::Vector3f(worldCoords[0], worldCoords[1], mapPose[2]);
}
map pose 를 world pose 로 변환시켜 반환함.
/**
* Returns the map pose for the given world pose.
*/
inline Eigen::Vector3f getMapCoordsPose(const Eigen::Vector3f& worldPose) const
{
Eigen::Vector2f mapCoords (mapTworld * worldPose.head<2>());
return Eigen::Vector3f(mapCoords[0], mapCoords[1], worldPose[2]);
}
world pose 를 map pose 로 변환시켜 반환함.
void setDimensionProperties(const Eigen::Vector2f& topLeftOffsetIn, const Eigen::Vector2i& mapDimensionsIn, float cellLengthIn)
{
setDimensionProperties(MapDimensionProperties(topLeftOffsetIn,mapDimensionsIn,cellLengthIn));
}
각 매개변수 값들을 받아 DimensionProperties에 저장.
void setDimensionProperties(const MapDimensionProperties& newMapDimProps)
{
//Grid map cell number has changed
if (!newMapDimProps.hasEqualDimensionProperties(this->mapDimensionProperties)){
this->setMapGridSize(newMapDimProps.getMapDimensions());
}
//Grid map transformation/cell size has changed
if(!newMapDimProps.hasEqualTransformationProperties(this->mapDimensionProperties)){
this->setMapTransformation(newMapDimProps.getTopLeftOffset(), newMapDimProps.getCellLength());
}
}
//Grid map cell number has changed
if (!newMapDimProps.hasEqualDimensionProperties(this->mapDimensionProperties)){
this->setMapGridSize(newMapDimProps.getMapDimensions());
}
새로운 Map의 Dimension이 기존 Map의 Dimension과 다르면 setMapGridSize 함수를 통해 맞춰줌.
//Grid map transformation/cell size has changed
if(!newMapDimProps.hasEqualTransformationProperties(this->mapDimensionProperties)){
this->setMapTransformation(newMapDimProps.getTopLeftOffset(), newMapDimProps.getCellLength());
}
새로운 Map의 Transformation이 기존 Map의 Transformation과 다르면
setMapTransformation 함수를 통해 맞춰줌.
/**
* Set the map transformations
* @param xWorld The origin of the map coordinate system on the x axis in world coordinates
* @param yWorld The origin of the map coordinate system on the y axis in world coordinates
* @param The cell length of the grid map
*/
void setMapTransformation(const Eigen::Vector2f& topLeftOffset, float cellLength)
{
mapDimensionProperties.setCellLength(cellLength);
mapDimensionProperties.setTopLeftOffset(topLeftOffset);
scaleToMap = 1.0f / cellLength;
mapTworld = Eigen::AlignedScaling2f(scaleToMap, scaleToMap) * Eigen::Translation2f(topLeftOffset[0], topLeftOffset[1]);
worldTmap3D = Eigen::AlignedScaling3f(scaleToMap, scaleToMap, 1.0f) * Eigen::Translation3f(topLeftOffset[0], topLeftOffset[1], 0);
//std::cout << worldTmap3D.matrix() << std::endl;
worldTmap3D = worldTmap3D.inverse();
worldTmap = mapTworld.inverse();
}
지금 우리는 map에 대한 정보를 가지고 있다. ( cell length, top left offset )
우리가 만든 map은 우리가 실제로 살고 있는 world와 크기가 다르다.
scaleToMap = 1.0f / cellLength;
: scaleToMap을 통해 우리는 map과 world 사이의 비율을 구할 수 있다.
Eigen::AlignedScaling2f(scaleToMap, scaleToMap)
: Eigen Library의 AlignedScaling 함수에 매개변수로 scaleToMap을 줌으로써
map을 world로 변환할 때, 크기를 조정하기 위한 Matrix인 Scaling Matrix를 구할 수 있다.
Eigen::Translation2f(topLeftOffset[0], topLeftOffset[1])
: 크기가 조정되면 map과 world 간의 기준좌표(top left offset)가 달라지게 된다.
Eigen Library의 Translation 함수에 map의 top left offset을 매개변수로 줌으로써
world의 top left offset을 map의 top left offset 값으로 바꿀 수 있는 Translation Matrix를 구할 수 있게 된다.
mapTworld = Eigen::AlignedScaling2f(scaleToMap, scaleToMap) * Eigen::Translation2f(topLeftOffset[0], topLeftOffset[1]);
: 위의 두 Matrix를 곱하게 되면 map의 크기 및 위치를 world에 맞게 바꿔줄 수 있는 Transformation Matrix를 얻을 수 있게 되는 것이다.
worldTmap = mapTworld.inverse();
: 즉, world = mapTworld * map 이 되게 된다. 이 수식은 다음과 같이 표현될 수도 있다.
mapTworld.inverse() * world = mapTworld.inverse() * mapTworld * map
mapTworld.inverse() * world = map
worldTmap * world = map
그러므로 mapTworld.inverse()는 worldTmap과 같게 된다.
/**
* Returns the scale factor for one unit in world coords to one unit in map coords.
* @return The scale factor
*/
float getScaleToMap() const
{
return scaleToMap;
}
/**
* Returns the cell edge length of grid cells in millimeters.
* @return the cell edge length in millimeters.
*/
float getCellLength() const
{
return mapDimensionProperties.getCellLength();
}
클래스 객체의 멤버변수들 값을 가져오는 함수
/**
* Returns a reference to the homogenous 2D transform from map to world coordinates.
* @return The homogenous 2D transform.
*/
const Eigen::Affine2f& getWorldTmap() const
{
return worldTmap;
}
/**
* Returns a reference to the homogenous 3D transform from map to world coordinates.
* @return The homogenous 3D transform.
*/
const Eigen::Affine3f& getWorldTmap3D() const
{
return worldTmap3D;
}
/**
* Returns a reference to the homogenous 2D transform from world to map coordinates.
* @return The homogenous 2D transform.
*/
const Eigen::Affine2f& getMapTworld() const
{
return mapTworld;
}
Affine Geometry에서, Affine Transformation은 공선점을 공선점으로 보내는 두 Affine Space 사이의 함수이다. Transform Matrix 라고 생각하면 되겠다.
worldTmap 등의 변수는 이미 위에서 정해져 있기 때문에 반환만 해주는데, 그 반환형은 Affine이 되는 것이다.
void setUpdated() { lastUpdateIndex++; };
int getUpdateIndex() const { return lastUpdateIndex; };
단순한 UpdateIndex 조작.
/**
* Returns the rectangle ([xMin,yMin],[xMax,xMax]) containing non-default cell values
*/
bool getMapExtends(int& xMax, int& yMax, int& xMin, int& yMin) const
{
int lowerStart = -1;
int upperStart = 10000;
int xMaxTemp = lowerStart;
int yMaxTemp = lowerStart;
int xMinTemp = upperStart;
int yMinTemp = upperStart;
int sizeX = this->getSizeX();
int sizeY = this->getSizeY();
for (int x = 0; x < sizeX; ++x) {
for (int y = 0; y < sizeY; ++y) {
if (this->mapArray[x][y].getValue() != 0.0f) {
if (x > xMaxTemp) {
xMaxTemp = x;
}
if (x < xMinTemp) {
xMinTemp = x;
}
if (y > yMaxTemp) {
yMaxTemp = y;
}
if (y < yMinTemp) {
yMinTemp = y;
}
}
}
}
if ((xMaxTemp != lowerStart) &&
(yMaxTemp != lowerStart) &&
(xMinTemp != upperStart) &&
(yMinTemp != upperStart)) {
xMax = xMaxTemp;
yMax = yMaxTemp;
xMin = xMinTemp;
yMin = yMinTemp;
return true;
} else {
return false;
}
}
값들이 범주안에 존재하는지 판단하여 true or false 로 반환.
protected:
ConcreteCellType *mapArray; ///< Map representation used with plain pointer array.
float scaleToMap; ///< Scaling factor from world to map.
Eigen::Affine2f worldTmap; ///< Homogenous 2D transform from map to world coordinates.
Eigen::Affine3f worldTmap3D; ///< Homogenous 3D transform from map to world coordinates.
Eigen::Affine2f mapTworld; ///< Homogenous 2D transform from world to map coordinates.
MapDimensionProperties mapDimensionProperties;
int sizeX;
private:
int lastUpdateIndex;
멤버변수들.
댓글 없음:
댓글 쓰기