MRPT  1.9.9
CSinCosLookUpTableFor2DScans.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://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 {
39  std::lock_guard lck(m_cache_mtx);
40 
41  auto it = m_cache.find(scan_prop);
42  if (it != m_cache.end())
43  { // Found in the cache:
44  return it->second;
45  }
46  else
47  {
48  // If there're too many LUTs, something is wrong, so just free the
49  // memory:
50  // (If you someday find someone with TWENTY *different* laser scanner
51  // models,
52  // please, accept my apologies... but send me a photo of them all!!
53  // ;-)
54  if (m_cache.size() > 20) m_cache.clear();
55 
56  // Compute and insert in the cache:
57  TSinCosValues& new_entry = m_cache[scan_prop];
58 
59  // Make sure the allocated memory at least have room for 4 extra
60  // double's at the end,
61  // for the case we use these buffers for SSE2 optimized code. If the
62  // final values are uninitialized it doesn't matter.
63  new_entry.ccos.resize(scan_prop.nRays + 4);
64  new_entry.csin.resize(scan_prop.nRays + 4);
65 
66  ASSERT_(scan_prop.nRays >= 2);
67  if (scan_prop.nRays > 0)
68  {
69  double Ang =
70  (scan_prop.rightToLeft ? -0.5 : +0.5) * scan_prop.aperture;
71  const double dA = (scan_prop.rightToLeft ? 1.0 : -1.0) *
72  (scan_prop.aperture / (scan_prop.nRays - 1));
73 
74  for (size_t i = 0; i < scan_prop.nRays; i++)
75  {
76  new_entry.ccos[i] = cos(Ang);
77  new_entry.csin[i] = sin(Ang);
78  Ang += dA;
79  }
80  }
81  // return them:
82  return new_entry;
83  }
84 }
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:120
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.
void resize(std::size_t N, bool zeroNewElements=false)
bool rightToLeft
Angles storage order: true=counterclockwise; false=clockwise.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 479715d5b Tue Nov 12 07:26:21 2019 +0100 at mar nov 12 07:30:12 CET 2019