MRPT  2.0.5
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-2020, 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 
12 #include <mrpt/core/lock_helper.h>
15 
16 using namespace std;
17 using namespace mrpt::obs;
18 
19 /** Return two vectors with the cos and the sin of the angles for each of the
20  * rays in a scan, computing them only the first time and returning a cached
21  * copy the rest.
22  */
24  CSinCosLookUpTableFor2DScans::getSinCosForScan(
25  const CObservation2DRangeScan& scan) const
26 {
27  T2DScanProperties scan_prop;
28  scan.getScanProperties(scan_prop);
29  return getSinCosForScan(scan_prop);
30 }
31 
32 /** Return two vectors with the cos and the sin of the angles for each of the
33  * rays in a scan, computing them only the first time and returning a cached
34  * copy the rest.
35  */
37  CSinCosLookUpTableFor2DScans::getSinCosForScan(
38  const T2DScanProperties& scan_prop) const
39 {
40  auto lck = mrpt::lockHelper(m_cache_mtx);
41 
42  auto it = m_cache.find(scan_prop);
43  if (it != m_cache.end())
44  { // Found in the cache:
45  return it->second;
46  }
47  else
48  {
49  // If there're too many LUTs, something is wrong, so just free the
50  // memory:
51  // (If you someday find someone with TWENTY *different* laser scanner
52  // models,
53  // please, accept my apologies... but send me a photo of them all!!
54  // ;-)
55  if (m_cache.size() > 20) m_cache.clear();
56 
57  // Compute and insert in the cache:
58  TSinCosValues& new_entry = m_cache[scan_prop];
59 
60  // Make sure the allocated memory at least have room for 4 extra
61  // double's at the end,
62  // for the case we use these buffers for SSE2 optimized code. If the
63  // final values are uninitialized it doesn't matter.
64  new_entry.ccos.resize(scan_prop.nRays + 4);
65  new_entry.csin.resize(scan_prop.nRays + 4);
66 
67  ASSERT_(scan_prop.nRays >= 2);
68  if (scan_prop.nRays > 0)
69  {
70  double Ang =
71  (scan_prop.rightToLeft ? -0.5 : +0.5) * scan_prop.aperture;
72  const double dA = (scan_prop.rightToLeft ? 1.0 : -1.0) *
73  (scan_prop.aperture / (scan_prop.nRays - 1));
74 
75  for (size_t i = 0; i < scan_prop.nRays; i++)
76  {
77  new_entry.ccos[i] = d2f(cos(Ang));
78  new_entry.csin[i] = d2f(sin(Ang));
79  Ang += dA;
80  }
81  }
82  // return them:
83  return new_entry;
84  }
85 }
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
float d2f(const double d)
shortcut for static_cast<float>(double)
LockHelper< T > lockHelper(T &t)
Syntactic sugar to easily create a locker to any kind of std::mutex.
Definition: lock_helper.h:50
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 2.0.5 Git: eda5ade6c Tue Aug 4 12:48:50 2020 +0200 at mar ago 4 13:00:11 CEST 2020