ScanPoint.cpp
Go to the documentation of this file.
00001 //
00002 // ScanPoint.cpp
00003 //
00004 
00005 #include "ScanPoint.hpp"
00006 #include <algorithm>
00007 #include <cmath>
00008 #include <limits>
00009 #include <memory.h>     // for memset()
00010 #include "../tools/errorhandler.hpp"
00011 #include "ScannerInfo.hpp"
00012 #include "Point3D.hpp"
00013 #include "Point2D.hpp"
00014 
00015 
00017 // ScanPoint
00019 namespace datatypes
00020 {
00021 
00022 ScanPoint::ScanPoint()
00023 {
00024 }
00025 
00026 void ScanPoint::clear()
00027 {
00028         memset (this, 0, sizeof(*this));
00029 
00030         m_x = NaN_double;
00031         m_y = NaN_double;
00032         m_z = NaN_double;
00033         m_dist = NaN_double;
00034         m_hAngle = NaN_double;
00035         m_vAngle = NaN_double;
00036 }
00037 
00045 void ScanPoint::setCartesian (double x, double y, double z)
00046 {
00047         m_x = x;
00048         m_y = y;
00049         m_z = z;
00050 
00051         updatePolar();          // compute equivalent polar coordinates
00052 }
00053 
00059 void ScanPoint::setPoint3D(const Point3D& pt)
00060 {
00061         setCartesian(double(pt.getX()), double(pt.getY()), double(pt.getZ()));
00062 }
00063 
00064 
00068 double ScanPoint::getDistanceBetweenScanpoints(const ScanPoint& pt1, const ScanPoint& pt2)
00069 {
00070         return pt1.getDist(pt2);
00071 }
00072 
00073 double ScanPoint::getDist(const ScanPoint& other) const
00074 {
00075         // We use "double" for the temporaries because we hope this might
00076         // give better precision when squaring. Who knows.
00077         const double x = other.m_x - m_x;
00078         const double y = other.m_y - m_y;
00079         const double z = other.m_z - m_z;
00080         return double(::hypot(x, y, z));
00081 }
00082 
00083 double ScanPoint::getDist2D(const ScanPoint& other) const
00084 {
00085         // We use "double" for the temporaries because we hope this might
00086         // give better precision when squaring. Who
00087         // knows. ::hypot_precise() will for sure have better
00088         // precision, but it might be slower. We stick with this
00089         // workaround so far.
00090         const double x = other.m_x - m_x;
00091         const double y = other.m_y - m_y;
00092         return double(::hypot(x, y));
00093 }
00094 
00106 void ScanPoint::setPolar (double dist, double hAngle, double vAngle)
00107 {
00108         m_dist   = dist;
00109         m_hAngle = normalizeRadians(hAngle);
00110         m_vAngle = normalizeRadians(vAngle);
00111 
00112         updateCartesian();
00113 }
00114 
00115 
00116 Point3D ScanPoint::toPoint3D() const
00117 {
00118         Point3D pt;
00119         pt.setXYZ(m_x, m_y, m_z);
00120         return pt;
00121 }
00122 
00123 Point2D ScanPoint::toPoint2D() const
00124 {
00125         Point2D pt;
00126         pt.setXY(m_x, m_y);
00127         return pt;
00128 }
00129 
00130 //
00131 // Useful e.g. to add the offset of the mounting position of a laserscanner.
00132 //
00133 // \param xOffset x-offset in meters
00134 // \param yOffset y-offset in meters
00135 // \param zOffset z-offset in meters
00136 //
00137 void ScanPoint::addCartesianOffset (double xOffset, double yOffset, double zOffset)
00138 {
00139         m_x += xOffset;
00140         m_y += yOffset;
00141         m_z += zOffset;
00142 
00143         updatePolar();          // compute equivalent polar coordinates
00144 }
00145 
00158 void ScanPoint::addPolarOffset (double distOffset, double hAngleOffset, double vAngleOffset)
00159 {
00160         m_dist   += distOffset;
00161         m_hAngle = normalizeRadians(m_hAngle + hAngleOffset);
00162         m_vAngle = normalizeRadians(m_vAngle + vAngleOffset);
00163 
00164         updateCartesian();              // compute equivalent Cartesian coordinates
00165 }
00166 
00180 void ScanPoint::updatePolar()
00181 {
00182         // diag^2 = x^2 + y^2
00183         const double diag2 = m_x * m_x + m_y * m_y;
00184 
00185         m_dist   = sqrt (diag2 + m_z * m_z);
00186 
00187         // Normalize v/hAngle here as well to maintain the invariant that
00188         // we're always strictly inside the interval of
00189         // normalizeRadians().
00190         m_hAngle = normalizeRadians( atan2 (m_y, m_x));
00191         m_vAngle = normalizeRadians(-atan2 (m_z, sqrt(diag2)));
00192 
00193         // Note: This calculation is expensive - two atan2() for each scan
00194         // point. We already checked whether we can get rid of some of
00195         // these steps by instead using a "bool m_polarIsValid" cache flag
00196         // and calculate the m_hAngle and m_vAngle only when getHAngle()
00197         // etc is called. However, it turned out almost all appbase graphs
00198         // use the polar coordinates in any coordinate system (e.g. dirt
00199         // detection in scanner coordinates, distance thresholds in
00200         // vehicle coordinates). For this reason, the potential savings
00201         // are more than offset by the overhead of the additional check of
00202         // the cache flag, and we removed any such cache implementation
00203         // again because valgrind told us the overhead increased the
00204         // overall complexity instead of reducing it. There might be an
00205         // exception if only the m_vAngle is calculated on-demand, but on
00206         // the other hand the saving is probably not that much anymore, so
00207         // we just leave the implementation as-is.
00208 }
00209 
00217 void ScanPoint::updateCartesian()
00218 {
00219         // diag = m_dist projected onto x-y-plane
00220         const double diag = m_dist * cos(m_vAngle);
00221 
00222         // Pitch and Yaw transformation:
00223         // x = dist *  cos(pitch) * cos(yaw)
00224         // y = dist *  cos(pitch) * sin(yaw)
00225         // z = dist * -sin(pitch)
00226 
00227         m_x =   diag * cos(m_hAngle);
00228         m_y =   diag * sin(m_hAngle);
00229         m_z = m_dist * -sin(m_vAngle);
00230 }
00231 
00237 void ScanPoint::setEchoWidth (double echoWidth)
00238 {
00239         m_echoWidth = echoWidth;
00240 }
00241 
00242 
00243 std::string ScanPoint::toString() const
00244 {
00245         std::ostringstream ostr;
00246         ostr << *this;
00247         return ostr.str();
00248 }
00249 
00250 // Text output for debugging
00251 std::ostream& operator<<(std::ostream& os, const ScanPoint& point)
00252 {
00253         os << "[(" << point.getX() << ", " << point.getY() << ", " << point.getZ() << ")"
00254            << " channel " << int(point.getLayer())
00255            << " subch " << int(point.getEchoNum())
00256            << " devID " << int(point.getSourceId())
00257            << "]"
00258            ;
00259         return os;
00260 }
00261 
00262 bool operator==(const ScanPoint &p1, const ScanPoint &p2)
00263 {
00264         return (p1.getSourceId() == p2.getSourceId())
00265                    && (p1.getLayer() == p2.getLayer())
00266                    && (p1.getEchoNum() == p2.getEchoNum())
00267                    && (p1.getX() == p2.getX() || (::isNaN(p1.getX()) && ::isNaN(p2.getX())))
00268                    && (p1.getY() == p2.getY() || (::isNaN(p1.getY()) && ::isNaN(p2.getY())))
00269                    && (p1.getZ() == p2.getZ() || (::isNaN(p1.getZ()) && ::isNaN(p2.getZ())))
00270                    ;
00271 }
00272 
00273 bool operator!=(const ScanPoint &p1, const ScanPoint &p2)
00274 {
00275         return !(p1 == p2);
00276 }
00277 
00278 
00279 }       // namespace datatypes
00280 


libsick_ldmrs
Author(s): SICK AG , Martin Günther , Jochen Sprickerhof
autogenerated on Wed Jun 14 2017 04:04:50