Main MRPT website > C++ reference for MRPT 1.9.9
CSinCosLookUpTableFor2DScans.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "obs-precomp.h" // Precompiled headers
11 
14 
15 using namespace std;
16 using namespace mrpt::obs;
17 
18 /** Return two vectors with the cos and the sin of the angles for each of the
19  * rays in a scan, computing them only the first time and returning a cached
20  * copy the rest.
21  */
23  CSinCosLookUpTableFor2DScans::getSinCosForScan(
24  const CObservation2DRangeScan& scan) const
25 {
26  T2DScanProperties scan_prop;
27  scan.getScanProperties(scan_prop);
28  return getSinCosForScan(scan_prop);
29 }
30 
31 /** Return two vectors with the cos and the sin of the angles for each of the
32  * rays in a scan, computing them only the first time and returning a cached
33  * copy the rest.
34  */
36  CSinCosLookUpTableFor2DScans::getSinCosForScan(
37  const T2DScanProperties& scan_prop) const
38 {
40  m_cache.find(scan_prop);
41  if (it != m_cache.end())
42  { // Found in the cache:
43  return it->second;
44  }
45  else
46  {
47  // If there're too many LUTs, something is wrong, so just free the
48  // memory:
49  // (If you someday find someone with TWENTY *different* laser scanner
50  // models,
51  // please, accept my apologies... but send me a photo of them all!!
52  // ;-)
53  if (m_cache.size() > 20) m_cache.clear();
54 
55  // Compute and insert in the cache:
56  TSinCosValues& new_entry = m_cache[scan_prop];
57 
58  // Make sure the allocated memory at least have room for 4 extra
59  // double's at the end,
60  // for the case we use these buffers for SSE2 optimized code. If the
61  // final values are uninitialized it doesn't matter.
62  new_entry.ccos.resize(scan_prop.nRays + 4);
63  new_entry.csin.resize(scan_prop.nRays + 4);
64 
65  ASSERT_(scan_prop.nRays >= 2);
66  if (scan_prop.nRays > 0)
67  {
68  double Ang =
69  (scan_prop.rightToLeft ? -0.5 : +0.5) * scan_prop.aperture;
70  const double dA = (scan_prop.rightToLeft ? 1.0 : -1.0) *
71  (scan_prop.aperture / (scan_prop.nRays - 1));
72 
73  for (size_t i = 0; i < scan_prop.nRays; i++)
74  {
75  new_entry.ccos[i] = cos(Ang);
76  new_entry.csin[i] = sin(Ang);
77  Ang += dA;
78  }
79  }
80  // return them:
81  return new_entry;
82  }
83 }
void getScanProperties(T2DScanProperties &p) const
Fill out a T2DScanProperties structure with the parameters of this scan.
STL namespace.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
Auxiliary struct that holds all the relevant geometry information about a 2D scan.
This namespace contains representation of robot actions and observations.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
A pair of vectors with the cos and sin values.
bool rightToLeft
Angles storage order: true=counterclockwise; false=clockwise.
const Scalar * const_iterator
Definition: eigen_plugins.h:27



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019