GridIndexLookup.h
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 
18 #pragma once
19 
20 #ifndef __OpenKarto_GridIndexLookup_h__
21 #define __OpenKarto_GridIndexLookup_h__
22 
23 #include <OpenKarto/Grid.h>
24 #include <OpenKarto/SensorData.h>
25 
26 namespace karto
27 {
28 
30 
31 
35 
41  {
42  public:
46  LookupArray();
47 
51  virtual ~LookupArray();
52 
53  public:
57  void Clear();
58 
63  kt_int32u GetSize() const;
64 
69  void SetSize(kt_int32u size);
70 
76  inline kt_int32s& operator[](kt_int32u index)
77  {
78  assert(index < m_Size);
79 
80  return m_pArray[index];
81  }
82 
88  inline kt_int32s operator[](kt_int32u index) const
89  {
90  assert(index < m_Size);
91 
92  return m_pArray[index];
93  }
94 
100  {
101  return m_pArray;
102  }
103 
108  inline kt_int32s* GetArrayPointer() const
109  {
110  return m_pArray;
111  }
112 
113  private:
117  }; // LookupArray
118 
122 
135  template<typename T>
137  {
138  public:
144  : m_pGrid(pGrid)
145  , m_Capacity(0)
146  , m_Size(0)
147  , m_ppLookupArray(NULL)
148  {
149  }
150 
155  {
156  DestroyArrays();
157  }
158 
159  public:
166  {
167  assert(math::IsUpTo(index, m_Size));
168 
169  return m_ppLookupArray[index];
170  }
171 
176  const List<kt_double>& GetAngles() const
177  {
178  return m_Angles;
179  }
180 
188  void ComputeOffsets(LocalizedLaserScan* pScan, kt_double angleCenter, kt_double angleOffset, kt_double angleResolution)
189  {
190  assert(angleOffset != 0.0);
191  assert(angleResolution != 0.0);
192 
193  kt_int32u nAngles = static_cast<kt_int32u>(math::Round(angleOffset * 2.0 / angleResolution) + 1);
194  SetSize(nAngles);
195 
197  // convert points into local coordinates of scan pose
198 
199  const Vector2dList& rPointReadings = pScan->GetPointReadings();
200 
201  // compute transform to scan pose
202  Transform transform(pScan->GetSensorPose());
203 
204  Pose2List localPoints;
205  karto_const_forEach(Vector2dList, &rPointReadings)
206  {
207  // do inverse transform to get points in local coordinates
208  Pose2 vec = transform.InverseTransformPose(Pose2(*iter, 0.0));
209  localPoints.Add(vec);
210  }
211 
213  // create lookup array for different angles
214  kt_double angle = 0.0;
215  kt_double startAngle = angleCenter - angleOffset;
216  for (kt_int32u angleIndex = 0; angleIndex < nAngles; angleIndex++)
217  {
218  angle = startAngle + angleIndex * angleResolution;
219  ComputeOffsets(angleIndex, angle, localPoints);
220  }
221  //assert(math::DoubleEqual(angle, angleCenter + angleOffset));
222  }
223 
224  private:
231  void ComputeOffsets(kt_int32u angleIndex, kt_double angle, const Pose2List& rLocalPoints)
232  {
233  m_ppLookupArray[angleIndex]->SetSize(static_cast<kt_int32u>(rLocalPoints.Size()));
234  m_Angles[angleIndex] = angle;
235 
236  // set up point array by computing relative offsets to points readings
237  // when rotated by given angle
238 
239  const Vector2d& rGridOffset = m_pGrid->GetCoordinateConverter()->GetOffset();
240 
241  kt_double cosine = cos(angle);
242  kt_double sine = sin(angle);
243 
244  kt_int32u readingIndex = 0;
245 
246  kt_int32s* pAngleIndexPointer = m_ppLookupArray[angleIndex]->GetArrayPointer();
247 
248  karto_const_forEach(Pose2List, &rLocalPoints)
249  {
250  const Vector2d& rPosition = iter->GetPosition();
251 
252  // counterclockwise rotation and that rotation is about the origin (0, 0).
253  Vector2d offset;
254  offset.SetX(cosine * rPosition.GetX() - sine * rPosition.GetY());
255  offset.SetY( sine * rPosition.GetX() + cosine * rPosition.GetY());
256 
257  // have to compensate for the grid offset when getting the grid index
258  Vector2i gridPoint = m_pGrid->WorldToGrid(offset + rGridOffset);
259 
260  // use base GridIndex to ignore ROI
261  kt_int32s lookupIndex = m_pGrid->Grid<T>::GridIndex(gridPoint, false);
262 
263  pAngleIndexPointer[readingIndex] = lookupIndex;
264 
265  readingIndex++;
266  }
267  assert(readingIndex == rLocalPoints.Size());
268  }
269 
274  void SetSize(kt_int32u size)
275  {
276  assert(size != 0);
277 
278  if (size > m_Capacity)
279  {
280  if (m_ppLookupArray != NULL)
281  {
282  DestroyArrays();
283  }
284 
285  m_Capacity = size;
286  m_ppLookupArray = new LookupArray*[m_Capacity];
287  for (kt_int32u i = 0; i < m_Capacity; i++)
288  {
289  m_ppLookupArray[i] = new LookupArray();
290  }
291  }
292 
293  m_Size = size;
294 
295  m_Angles.Resize(size);
296  }
297 
302  {
303  for (kt_int32u i = 0; i < m_Capacity; i++)
304  {
305  delete m_ppLookupArray[i];
306  }
307 
308  delete[] m_ppLookupArray;
309  m_ppLookupArray = NULL;
310  }
311 
312  private:
314 
317 
319 
320  // for sanity check
322  }; // class GridIndexLookup
323 
325 
326 }
327 
328 #endif // __OpenKarto_GridIndexLookup_h__
virtual kt_size_t Size() const
Definition: List.h:214
const List< kt_double > & GetAngles() const
kt_int32s * GetArrayPointer()
const T & GetY() const
Definition: Geometry.h:369
void SetX(const T &x)
Definition: Geometry.h:360
kt_bool IsUpTo(const T &value, const T &maximum)
Definition: Math.h:175
void ComputeOffsets(kt_int32u angleIndex, kt_double angle, const Pose2List &rLocalPoints)
kt_double Round(kt_double value)
Definition: Math.h:114
Pose2 GetSensorPose() const
Definition: SensorData.h:627
void ComputeOffsets(LocalizedLaserScan *pScan, kt_double angleCenter, kt_double angleOffset, kt_double angleResolution)
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
List< kt_double > m_Angles
const T & GetX() const
Definition: Geometry.h:351
uint32_t kt_int32u
Definition: Types.h:111
const LookupArray * GetLookupArray(kt_int32u index) const
kt_int32s & operator[](kt_int32u index)
int32_t kt_int32s
Definition: Types.h:106
void SetSize(kt_int32u size)
GridIndexLookup(Grid< T > *pGrid)
kt_int32u GetSize() const
LookupArray ** m_ppLookupArray
kt_int32s * GetArrayPointer() const
void SetSize(kt_int32u size)
double kt_double
Definition: Types.h:160
Definition: Any.cpp:20
void SetY(const T &y)
Definition: Geometry.h:378
kt_int32s operator[](kt_int32u index) const
#define karto_const_forEach(listtype, list)
Definition: Macros.h:136
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