OccupancyGrid.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2011, SRI International (R)
3  *
4  * This program is free software: you can redistribute it and/or modify
5  * it under the terms of the GNU Lesser General Public License as published by
6  * the Free Software Foundation, either version 3 of the License, or
7  * (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details.
13  *
14  * You should have received a copy of the GNU Lesser General Public License
15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
17 
19 #include <OpenKarto/OpenMapper.h>
20 
21 namespace karto
22 {
23 
27 
28  OccupancyGrid::OccupancyGrid(kt_int32s width, kt_int32s height, const Vector2d& rOffset, kt_double resolution)
29  : Grid<kt_int8u>(width, height)
30  , m_pCellPassCnt(Grid<kt_int32u>::CreateGrid(0, 0, resolution))
31  , m_pCellHitsCnt(Grid<kt_int32u>::CreateGrid(0, 0, resolution))
32  , m_pCellUpdater(NULL)
33  {
34  m_pCellUpdater = new CellUpdater(this);
35 
36  if (karto::math::DoubleEqual(resolution, 0.0))
37  {
38  throw Exception("Resolution cannot be 0");
39  }
40 
41  m_pMinPassThrough = new Parameter<kt_int32u>(NULL, "MinPassThrough", "", "", 2);
42  m_pOccupancyThreshold = new Parameter<kt_double>(NULL, "OccupancyThreshold", "", "", 0.1);
43 
44  GetCoordinateConverter()->SetScale(1.0 / resolution);
46  }
47 
48  OccupancyGrid::~OccupancyGrid()
49  {
50  delete m_pCellUpdater;
51  }
52 
54  {
55  OccupancyGrid* pOccupancyGrid = new OccupancyGrid(GetWidth(), GetHeight(), GetCoordinateConverter()->GetOffset(), 1.0 / GetCoordinateConverter()->GetScale());
56  memcpy(pOccupancyGrid->GetDataPointer(), GetDataPointer(), GetDataSize());
57 
59  pOccupancyGrid->m_pCellPassCnt = m_pCellPassCnt->Clone();
60  pOccupancyGrid->m_pCellHitsCnt = m_pCellHitsCnt->Clone();
61 
62  return pOccupancyGrid;
63  }
64 
65  // KARTO_DEPRECATED
66 #ifdef WIN32
67  OccupancyGrid* OccupancyGrid::CreateFromScans(const std::vector< SmartPointer<LocalizedRangeScan> >& /*rScans*/, kt_double /*resolution*/)
68  {
69  throw Exception("OccupancyGrid::CreateFromScans - Not supported in Windows. Please Use CreateFromScans(const LocalizedLaserScanList& rScans, kt_double resolution).");
70  }
71 #else
73  {
75  const_forEach(std::vector< SmartPointer<LocalizedRangeScan> >, &rScans)
76  {
77  scans.Add(*iter);
78  }
79 
80  return CreateFromScans(scans, resolution);
81  }
82 #endif
83 
85  {
86  if (rScans.Size() == 0)
87  {
88  return NULL;
89  }
90 
91  kt_int32s width, height;
92  Vector2d offset;
93  OccupancyGrid::ComputeDimensions(rScans, resolution, width, height, offset);
94  OccupancyGrid* pOccupancyGrid = new OccupancyGrid(width, height, offset, resolution);
95  pOccupancyGrid->CreateFromScans(rScans);
96 
97  return pOccupancyGrid;
98  }
99 
101  {
102  return karto::OccupancyGrid::CreateFromScans(pMapper->GetAllProcessedScans(), resolution);
103  }
104 
105  void OccupancyGrid::ComputeDimensions(const LocalizedLaserScanList& rScans, kt_double resolution, kt_int32s& rWidth, kt_int32s& rHeight, Vector2d& rOffset)
106  {
107  BoundingBox2 boundingBox;
109  {
110  LocalizedLaserScan* pLocalizedLaserScan = *iter;
111 
112  if (pLocalizedLaserScan != NULL)
113  {
114  boundingBox.Add(pLocalizedLaserScan->GetBoundingBox());
115  }
116  }
117 
118  kt_double scale = 1.0 / resolution;
119  Size2<kt_double> size = boundingBox.GetSize();
120 
121  rWidth = static_cast<kt_int32s>(math::Round(size.GetWidth() * scale));
122  rHeight = static_cast<kt_int32s>(math::Round(size.GetHeight() * scale));
123  rOffset = boundingBox.GetMinimum();
124  }
125 
127  {
128  m_pCellPassCnt->Resize(GetWidth(), GetHeight());
129  m_pCellPassCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
130 
131  m_pCellHitsCnt->Resize(GetWidth(), GetHeight());
132  m_pCellHitsCnt->GetCoordinateConverter()->SetOffset(GetCoordinateConverter()->GetOffset());
133 
135  {
136  AddScan(*iter);
137  }
138 
139  UpdateGrid();
140  }
141 
143  {
144  kt_double rangeThreshold = pScan->GetLaserRangeFinder()->GetRangeThreshold();
145  kt_double maxRange = pScan->GetLaserRangeFinder()->GetMaximumRange();
146  kt_double minRange = pScan->GetLaserRangeFinder()->GetMinimumRange();
147 
148  Vector2d scanPosition = pScan->GetSensorPose().GetPosition();
149 
150  // get scan point readings
151  const Vector2dList& rPointReadings = pScan->GetPointReadings(false);
152 
153  kt_bool scanInGrid = false;
154 
155  // draw lines from scan position to all point readings
156  karto_const_forEach(Vector2dList, &rPointReadings)
157  {
158  Vector2d point = *iter;
159  kt_double range = scanPosition.Distance(point);
160  kt_bool isEndPointValid = range < (rangeThreshold - KT_TOLERANCE);
161 
162  if (range >= maxRange || range < minRange)
163  {
164  // ignore max range or min range readings
165  continue;
166  }
167  else if (range >= rangeThreshold)
168  {
169  // trace up to range reading
170  kt_double ratio = rangeThreshold / range;
171  kt_double dx = point.GetX() - scanPosition.GetX();
172  kt_double dy = point.GetY() - scanPosition.GetY();
173  point.SetX(scanPosition.GetX() + ratio * dx);
174  point.SetY(scanPosition.GetY() + ratio * dy);
175  }
176 
177  if (RayTrace(scanPosition, point, isEndPointValid, doUpdate))
178  {
179  scanInGrid = true;
180  }
181  }
182 
183  return scanInGrid;
184  }
185 
186  kt_double OccupancyGrid::RayCast(const Pose2& rPose2, kt_double maxRange) const
187  {
189 
190  kt_double x = rPose2.GetX();
191  kt_double y = rPose2.GetY();
192  kt_double theta = rPose2.GetHeading();
193 
194  kt_double sinTheta = sin(theta);
195  kt_double cosTheta = cos(theta);
196 
197  kt_double xStop = x + maxRange * cosTheta;
198  kt_double xSteps = 1 + fabs(xStop - x) * scale;
199 
200  kt_double yStop = y + maxRange * sinTheta;
201  kt_double ySteps = 1 + fabs(yStop - y) * scale;
202 
203  kt_double steps = math::Maximum(xSteps, ySteps);
204  kt_double delta = maxRange / steps;
205  kt_double distance = delta;
206 
207  for (kt_int32u i = 1; i < steps; i++)
208  {
209  kt_double x1 = x + distance * cosTheta;
210  kt_double y1 = y + distance * sinTheta;
211 
212  Vector2i gridIndex = WorldToGrid(Vector2d(x1, y1));
213  if (IsValidGridIndex(gridIndex) && IsFree(gridIndex))
214  {
215  distance = (i + 1) * delta;
216  }
217  else
218  {
219  break;
220  }
221  }
222 
223  return (distance < maxRange) ? distance : maxRange;
224  }
225 
226  kt_bool OccupancyGrid::RayTrace(const Vector2d& rWorldFrom, const Vector2d& rWorldTo, kt_bool isEndPointValid, kt_bool doUpdate)
227  {
228  assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL);
229 
230  Vector2i gridFrom = m_pCellPassCnt->WorldToGrid(rWorldFrom);
231  Vector2i gridTo = m_pCellPassCnt->WorldToGrid(rWorldTo);
232 
233  CellUpdater* pCellUpdater = doUpdate ? m_pCellUpdater : NULL;
234  m_pCellPassCnt->TraceLine(gridFrom.GetX(), gridFrom.GetY(), gridTo.GetX(), gridTo.GetY(), pCellUpdater);
235 
236  // for the end point
237  if (isEndPointValid)
238  {
239  if (m_pCellPassCnt->IsValidGridIndex(gridTo))
240  {
241  kt_int32s index = m_pCellPassCnt->GridIndex(gridTo, false);
242 
243  kt_int32u* pCellPassCntPtr = m_pCellPassCnt->GetDataPointer();
244  kt_int32u* pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer();
245 
246  // increment cell pass through and hit count
247  pCellPassCntPtr[index]++;
248  pCellHitCntPtr[index]++;
249 
250  if (doUpdate)
251  {
252  (*m_pCellUpdater)(index);
253  }
254  }
255  }
256 
257  return m_pCellPassCnt->IsValidGridIndex(gridTo);
258  }
259 
260  void OccupancyGrid::UpdateCell(kt_int8u* pCell, kt_int32u cellPassCnt, kt_int32u cellHitCnt)
261  {
262  if (cellPassCnt > m_pMinPassThrough->GetValue())
263  {
264  kt_double hitRatio = static_cast<kt_double>(cellHitCnt) / static_cast<kt_double>(cellPassCnt);
265 
266  if (hitRatio > m_pOccupancyThreshold->GetValue())
267  {
268  *pCell = GridStates_Occupied;
269  }
270  else
271  {
272  *pCell = GridStates_Free;
273  }
274  }
275  }
276 
278  {
279  assert(m_pCellPassCnt != NULL && m_pCellHitsCnt != NULL);
280 
281  // clear grid
282  Clear();
283 
284  // set occupancy status of cells
285  kt_int8u* pDataPtr = GetDataPointer();
286  kt_int32u* pCellPassCntPtr = m_pCellPassCnt->GetDataPointer();
287  kt_int32u* pCellHitCntPtr = m_pCellHitsCnt->GetDataPointer();
288 
289  kt_int32u nBytes = GetDataSize();
290  for (kt_int32u i = 0; i < nBytes; i++, pDataPtr++, pCellPassCntPtr++, pCellHitCntPtr++)
291  {
292  UpdateCell(pDataPtr, *pCellPassCntPtr, *pCellHitCntPtr);
293  }
294  }
295 
297  {
298  Grid<kt_int8u>::Resize(width, height);
299 
300  m_pCellPassCnt->Resize(width, height);
301  m_pCellHitsCnt->Resize(width, height);
302  }
303 
307 
309  {
310  kt_int8u* pDataPtr = m_pOccupancyGrid->GetDataPointer();
311  kt_int32u* pCellPassCntPtr = m_pOccupancyGrid->m_pCellPassCnt->GetDataPointer();
312  kt_int32u* pCellHitCntPtr = m_pOccupancyGrid->m_pCellHitsCnt->GetDataPointer();
313 
314  m_pOccupancyGrid->UpdateCell(&pDataPtr[index], pCellPassCntPtr[index], pCellHitCntPtr[index]);
315  }
316 
317 }
kt_int32s GetHeight() const
Definition: Grid.h:263
OccupancyGrid * Clone() const
virtual kt_size_t Size() const
Definition: List.h:214
void SetSize(const Size2< kt_int32s > &rSize)
kt_double Distance(const Vector2 &rOther) const
Definition: Geometry.h:443
bool kt_bool
Definition: Types.h:145
kt_int32s GetDataSize() const
Definition: Grid.h:308
CellUpdater * m_pCellUpdater
const Size2< kt_int32s > GetSize() const
Definition: Grid.h:272
kt_double GetHeading() const
Definition: Geometry.h:2274
kt_bool IsValidGridIndex(const Vector2i &rGrid) const
Definition: Grid.h:157
const T & Maximum(const T &value1, const T &value2)
Definition: Math.h:138
const BoundingBox2 & GetBoundingBox() const
Definition: SensorData.h:666
SmartPointer< Parameter< kt_double > > m_pOccupancyThreshold
const T & GetY() const
Definition: Geometry.h:369
OccupancyGrid(kt_int32s width, kt_int32s height, const Vector2d &rOffset, kt_double resolution)
kt_double GetX() const
Definition: Geometry.h:2220
void SetX(const T &x)
Definition: Geometry.h:360
virtual void Add(const T &rValue)
Definition: List.h:111
kt_int32s GetWidth() const
Definition: Grid.h:254
TFSIMD_FORCE_INLINE tfScalar distance(const Vector3 &v) const
kt_double Round(kt_double value)
Definition: Math.h:114
#define const_forEach(listtype, list)
Definition: Macros.h:100
static OccupancyGrid * CreateFromMapper(OpenMapper *pMapper, kt_double resolution)
TFSIMD_FORCE_INLINE const tfScalar & y() const
Pose2 GetSensorPose() const
Definition: SensorData.h:627
SmartPointer< Grid< kt_int32u > > m_pCellPassCnt
SmartPointer< Parameter< kt_int32u > > m_pMinPassThrough
void SetOffset(const Vector2d &rOffset)
const LocalizedLaserScanList GetAllProcessedScans() const
uint8_t kt_int8u
Definition: Types.h:91
const T & GetX() const
Definition: Geometry.h:351
void SetScale(kt_double scale)
uint32_t kt_int32u
Definition: Types.h:111
kt_bool IsFree(const Vector2i &rGridIndex) const
Size2< kt_double > GetSize() const
Definition: Geometry.h:1649
virtual void operator()(kt_int32u index)
virtual void Resize(kt_int32s width, kt_int32s height)
Definition: Grid.h:117
virtual void Resize(kt_int32s width, kt_int32s height)
const kt_double KT_TOLERANCE
Definition: Math.h:71
kt_int8u * GetDataPointer()
Definition: Grid.h:290
TFSIMD_FORCE_INLINE const tfScalar & x() const
const Vector2d & GetMinimum() const
Definition: Geometry.h:1613
int32_t kt_int32s
Definition: Types.h:106
SmartPointer< Grid< kt_int32u > > m_pCellHitsCnt
const Vector2d & GetPosition() const
Definition: Geometry.h:2256
kt_bool RayTrace(const Vector2d &rWorldFrom, const Vector2d &rWorldTo, kt_bool isEndPointValid, kt_bool doUpdate=false)
void UpdateCell(kt_int8u *pCell, kt_int32u cellPassCnt, kt_int32u cellHitCnt)
Vector2< kt_double > Vector2d
Definition: Geometry.h:610
static OccupancyGrid * CreateFromScans(const LocalizedLaserScanList &rScans, kt_double resolution)
CoordinateConverter * GetCoordinateConverter() const
Definition: Grid.h:339
double kt_double
Definition: Types.h:160
kt_double RayCast(const Pose2 &rPose2, kt_double maxRange) const
kt_bool DoubleEqual(kt_double a, kt_double b)
Definition: Math.h:162
Vector2i WorldToGrid(const Vector2d &rWorld, kt_bool flipY=false) const
Definition: Grid.h:212
kt_double GetY() const
Definition: Geometry.h:2238
const T GetWidth() const
Definition: Geometry.h:80
friend class CellUpdater
Definition: OccupancyGrid.h:61
static void ComputeDimensions(const LocalizedLaserScanList &rScans, kt_double resolution, kt_int32s &rWidth, kt_int32s &rHeight, Vector2d &rOffset)
Definition: Any.cpp:20
kt_double GetMinimumRange() const
Definition: Sensor.h:264
T * GetDataPointer(const Vector2i &rGrid)
Definition: Grid.h:233
const T GetHeight() const
Definition: Geometry.h:98
void SetY(const T &y)
Definition: Geometry.h:378
kt_double GetRangeThreshold() const
Definition: Sensor.h:304
void Add(const Vector2d &rPoint)
Definition: Geometry.h:1660
kt_bool AddScan(LocalizedLaserScan *pScan, kt_bool doUpdate=false)
#define karto_const_forEach(listtype, list)
Definition: Macros.h:136
LaserRangeFinder * GetLaserRangeFinder() const
Definition: SensorData.h:576
kt_double GetMaximumRange() const
Definition: Sensor.h:284
const Vector2dList & GetPointReadings(kt_bool wantFiltered=false) const
Definition: SensorData.cpp:152


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Thu Jun 6 2019 19:20:24