ScanPoint.cpp
Go to the documentation of this file.
1 //
2 // ScanPoint.cpp
3 //
4 
5 #include "ScanPoint.hpp"
6 #include <algorithm>
7 #include <cmath>
8 #include <limits>
9 #include <memory.h> // for memset()
10 #include "../tools/errorhandler.hpp"
11 #include "ScannerInfo.hpp"
12 #include "Point3D.hpp"
13 #include "Point2D.hpp"
14 
15 
17 // ScanPoint
19 namespace datatypes
20 {
21 
23 {
24 }
25 
27 {
28  memset (this, 0, sizeof(*this));
29 
30  m_x = NaN_double;
31  m_y = NaN_double;
32  m_z = NaN_double;
36 }
37 
45 void ScanPoint::setCartesian (double x, double y, double z)
46 {
47  m_x = x;
48  m_y = y;
49  m_z = z;
50 
51  updatePolar(); // compute equivalent polar coordinates
52 }
53 
60 {
61  setCartesian(double(pt.getX()), double(pt.getY()), double(pt.getZ()));
62 }
63 
64 
69 {
70  return pt1.getDist(pt2);
71 }
72 
73 double ScanPoint::getDist(const ScanPoint& other) const
74 {
75  // We use "double" for the temporaries because we hope this might
76  // give better precision when squaring. Who knows.
77  const double x = other.m_x - m_x;
78  const double y = other.m_y - m_y;
79  const double z = other.m_z - m_z;
80  return double(::hypot(x, y, z));
81 }
82 
83 double ScanPoint::getDist2D(const ScanPoint& other) const
84 {
85  // We use "double" for the temporaries because we hope this might
86  // give better precision when squaring. Who
87  // knows. ::hypot_precise() will for sure have better
88  // precision, but it might be slower. We stick with this
89  // workaround so far.
90  const double x = other.m_x - m_x;
91  const double y = other.m_y - m_y;
92  return double(::hypot(x, y));
93 }
94 
106 void ScanPoint::setPolar (double dist, double hAngle, double vAngle)
107 {
108  m_dist = dist;
109  m_hAngle = normalizeRadians(hAngle);
110  m_vAngle = normalizeRadians(vAngle);
111 
112  updateCartesian();
113 }
114 
115 
117 {
118  Point3D pt;
119  pt.setXYZ(m_x, m_y, m_z);
120  return pt;
121 }
122 
124 {
125  Point2D pt;
126  pt.setXY(m_x, m_y);
127  return pt;
128 }
129 
130 //
131 // Useful e.g. to add the offset of the mounting position of a laserscanner.
132 //
133 // \param xOffset x-offset in meters
134 // \param yOffset y-offset in meters
135 // \param zOffset z-offset in meters
136 //
137 void ScanPoint::addCartesianOffset (double xOffset, double yOffset, double zOffset)
138 {
139  m_x += xOffset;
140  m_y += yOffset;
141  m_z += zOffset;
142 
143  updatePolar(); // compute equivalent polar coordinates
144 }
145 
158 void ScanPoint::addPolarOffset (double distOffset, double hAngleOffset, double vAngleOffset)
159 {
160  m_dist += distOffset;
161  m_hAngle = normalizeRadians(m_hAngle + hAngleOffset);
162  m_vAngle = normalizeRadians(m_vAngle + vAngleOffset);
163 
164  updateCartesian(); // compute equivalent Cartesian coordinates
165 }
166 
181 {
182  // diag^2 = x^2 + y^2
183  const double diag2 = m_x * m_x + m_y * m_y;
184 
185  m_dist = sqrt (diag2 + m_z * m_z);
186 
187  // Normalize v/hAngle here as well to maintain the invariant that
188  // we're always strictly inside the interval of
189  // normalizeRadians().
190  m_hAngle = normalizeRadians( atan2 (m_y, m_x));
191  m_vAngle = normalizeRadians(-atan2 (m_z, sqrt(diag2)));
192 
193  // Note: This calculation is expensive - two atan2() for each scan
194  // point. We already checked whether we can get rid of some of
195  // these steps by instead using a "bool m_polarIsValid" cache flag
196  // and calculate the m_hAngle and m_vAngle only when getHAngle()
197  // etc is called. However, it turned out almost all appbase graphs
198  // use the polar coordinates in any coordinate system (e.g. dirt
199  // detection in scanner coordinates, distance thresholds in
200  // vehicle coordinates). For this reason, the potential savings
201  // are more than offset by the overhead of the additional check of
202  // the cache flag, and we removed any such cache implementation
203  // again because valgrind told us the overhead increased the
204  // overall complexity instead of reducing it. There might be an
205  // exception if only the m_vAngle is calculated on-demand, but on
206  // the other hand the saving is probably not that much anymore, so
207  // we just leave the implementation as-is.
208 }
209 
218 {
219  // diag = m_dist projected onto x-y-plane
220  const double diag = m_dist * cos(m_vAngle);
221 
222  // Pitch and Yaw transformation:
223  // x = dist * cos(pitch) * cos(yaw)
224  // y = dist * cos(pitch) * sin(yaw)
225  // z = dist * -sin(pitch)
226 
227  m_x = diag * cos(m_hAngle);
228  m_y = diag * sin(m_hAngle);
229  m_z = m_dist * -sin(m_vAngle);
230 }
231 
237 void ScanPoint::setEchoWidth (double echoWidth)
238 {
239  m_echoWidth = echoWidth;
240 }
241 
242 
243 std::string ScanPoint::toString() const
244 {
245  std::ostringstream ostr;
246  ostr << *this;
247  return ostr.str();
248 }
249 
250 // Text output for debugging
251 std::ostream& operator<<(std::ostream& os, const ScanPoint& point)
252 {
253  os << "[(" << point.getX() << ", " << point.getY() << ", " << point.getZ() << ")"
254  << " channel " << int(point.getLayer())
255  << " subch " << int(point.getEchoNum())
256  << " devID " << int(point.getSourceId())
257  << "]"
258  ;
259  return os;
260 }
261 
262 bool operator==(const ScanPoint &p1, const ScanPoint &p2)
263 {
264  return (p1.getSourceId() == p2.getSourceId())
265  && (p1.getLayer() == p2.getLayer())
266  && (p1.getEchoNum() == p2.getEchoNum())
267  && (p1.getX() == p2.getX() || (::isNaN(p1.getX()) && ::isNaN(p2.getX())))
268  && (p1.getY() == p2.getY() || (::isNaN(p1.getY()) && ::isNaN(p2.getY())))
269  && (p1.getZ() == p2.getZ() || (::isNaN(p1.getZ()) && ::isNaN(p2.getZ())))
270  ;
271 }
272 
273 bool operator!=(const ScanPoint &p1, const ScanPoint &p2)
274 {
275  return !(p1 == p2);
276 }
277 
278 
279 } // namespace datatypes
280 
double getDist() const
Definition: ScanPoint.hpp:93
void setXYZ(double x, double y, double z)
Sets the coordinates of this point to the given values.
Definition: Point3D.hpp:84
const double NaN_double
Not-a-Number in double precision.
Definition: MathToolbox.cpp:13
This class defines a point in the three-dimensional plane.
Definition: Point3D.hpp:25
friend bool operator==(const ScanPoint &, const ScanPoint &)
Equality predicate.
Definition: ScanPoint.cpp:262
double getY() const
Returns the y-coordinate of this point.
Definition: Point3D.hpp:64
double hypot(double x, double y, double z)
Definition: MathToolbox.cpp:19
std::string toString() const
Definition: ScanPoint.cpp:243
double getY() const
Definition: ScanPoint.hpp:90
void setPolar(double dist, double hAngle, double vAngle)
Definition: ScanPoint.cpp:106
void setEchoWidth(double echoWidth)
Set the echo pulse width, typically in [m].
Definition: ScanPoint.cpp:237
void addPolarOffset(double distOffset, double hAngleOffset, double vAngleOffset)
Definition: ScanPoint.cpp:158
static double getDistanceBetweenScanpoints(const ScanPoint &pt1, const ScanPoint &pt2)
Definition: ScanPoint.cpp:68
void setPoint3D(const Point3D &pt)
Definition: ScanPoint.cpp:59
void setCartesian(double x, double y, double z)
Definition: ScanPoint.cpp:45
Point2D toPoint2D() const
Returns the x and y coordinates as a Point2D structure.
Definition: ScanPoint.cpp:123
double getZ() const
Returns the z-coordinate of this point.
Definition: Point3D.hpp:65
void setXY(value_type x, value_type y)
Definition: Point2D.hpp:147
void addCartesianOffset(double xOffset, double yOffset, double zOffset)
Definition: ScanPoint.cpp:137
Point3D toPoint3D() const
Returns the x,y,z coordinates as a Point3D structure.
Definition: ScanPoint.cpp:116
UINT8 getSourceId() const
Definition: ScanPoint.hpp:99
double getX() const
Returns the x-coordinate of this point.
Definition: Point3D.hpp:63
double normalizeRadians(double radians)
Definition: MathToolbox.cpp:31
double getZ() const
Definition: ScanPoint.hpp:91
UINT8 getLayer() const
Definition: ScanPoint.hpp:100
void updateCartesian()
Compute Cartesian coordinates from the current polar coordinates.
Definition: ScanPoint.cpp:217
void updatePolar()
Compute polar coordinates from the current Cartesian coordinates.
Definition: ScanPoint.cpp:180
double getX() const
Definition: ScanPoint.hpp:89
double getDist2D(const ScanPoint &other) const
Returns the two-dimensional distance between this and the given other scanpoint, in [m]...
Definition: ScanPoint.cpp:83
bool operator!=(const Box2D &b1, const Box2D &b2)
Definition: Box2D.hpp:277
bool isNaN(floatT x)
Checks if a floating point value is Not-a-Number (NaN)
Definition: MathToolbox.hpp:63
UINT8 getEchoNum() const
Definition: ScanPoint.hpp:101
std::ostream & operator<<(std::ostream &os, const EvalCaseResult &result)


libsick_ldmrs
Author(s): SICK AG , Martin Günther , Jochen Sprickerhof
autogenerated on Sat Jun 8 2019 17:57:33