Position3D.cpp
Go to the documentation of this file.
00001 //
00002 // Position3D.cpp
00003 //
00004 // 3D-Punkt mit Orientierung.
00005 //
00006 // SICK AG, 11.11.2011, willhvo
00007 //
00008 
00009 #include "Position3D.hpp"
00010 #include "Point2D.hpp"
00011 #include "Point3D.hpp"
00012 #include <cmath>        // for sin, cos, ...
00013 #include <cassert>
00014 #include <iostream>
00015 #include <sstream>
00016 
00017 
00018 namespace datatypes
00019 {
00020         
00021 Position3D::Position3D()
00022         : m_yawAngle(0)
00023         , m_pitchAngle(0)
00024         , m_rollAngle(0)
00025         , m_point(0.0, 0.0, 0.0)
00026 {
00027 }
00028 
00029 Position3D::Position3D(value_type yaw, value_type pitch, value_type roll,
00030                                                                    value_type x, value_type y, value_type z)
00031         : m_yawAngle(yaw)
00032         , m_pitchAngle(pitch)
00033         , m_rollAngle(roll)
00034         , m_point(x, y, z)
00035 {
00036 }
00037 
00038 Position3D::Position3D(value_type yaw, value_type pitch, value_type roll,
00039                                                                    const Point3D& pt)
00040         : m_yawAngle(yaw)
00041         , m_pitchAngle(pitch)
00042         , m_rollAngle(roll)
00043         , m_point(pt)
00044 {
00045 }
00046 
00047 //
00048 // **********************************************
00049 //
00050 
00051 Vector::Vector(UINT16 numOfElements)
00052 {
00053         if (numOfElements > 0)
00054         {
00055                 m_elements = new double[numOfElements];
00056         }
00057         else
00058         {
00059                 m_elements = NULL;
00060         }
00061         m_numOfElements = numOfElements;
00062 }
00063 
00064 Vector::~Vector()
00065 {
00066         if (m_elements != NULL)
00067         {
00068                 delete m_elements;
00069                 m_elements = NULL;
00070         }
00071 }
00072 
00073 // --------------------------------------------------
00074 
00075 Matrix::Matrix(UINT16 numOfRows, UINT16 numOfColumns)
00076 {
00077         if ((numOfRows > 0) && (numOfColumns > 0))
00078         {
00079                 m_elements = new double*[numOfRows];
00080                 for (UINT16 r=0; r < numOfRows; r++)
00081                 {
00082                         m_elements[r] = new double[numOfColumns];
00083                 }
00084                 m_numOfRows = numOfRows;
00085                 m_numOfColumns = numOfColumns;
00086         }
00087         else
00088         {
00089                 m_elements = NULL;
00090                 m_numOfRows = 0;
00091                 m_numOfColumns = 0;
00092         }
00093 }
00094 
00095 Matrix::~Matrix()
00096 {
00097         if (m_elements != NULL)
00098         {
00099                 for (UINT16 r=0; r < m_numOfRows; r++)
00100                 {
00101                         delete m_elements[r];
00102                 }
00103                 
00104                 delete m_elements;
00105                 m_elements = NULL;
00106         }
00107 }
00108 
00109 //
00110 // ***********************************************
00111 //
00112 
00113 bool Position3D::transformToVehicle(Point3D* pt)        // double *dXPos, double *dYPos, double *dZPos, const CRect& r)
00114 {
00115         //Punkt in Laserscannkoordinaten
00116         Vector P_LS(4);
00117 
00118         //Scanpunkt in Scannerkoordinaten
00119         P_LS[0] = pt->getX();   // *dXPos;
00120         P_LS[1] = pt->getY();   // *dYPos;
00121         P_LS[2] = pt->getZ();   // *dZPos;
00122         P_LS[3] = 1.0;
00123 
00124         // Sensorposition in Carcoordinates
00125         double dXOffset_LS, dYOffset_LS, dZOffset_LS;
00126 
00127         // Nick-, Gier- und Wankwinkel des Laserscanners
00128         double dPitch_LS, dYaw_LS, dRoll_LS;
00129 
00130         //Laserscanner to Vehicle transformation Matrix
00131         Matrix H_LS_Car(4,4);
00132 
00133         dRoll_LS                = m_rollAngle;
00134         dYaw_LS                 = m_yawAngle;
00135         dPitch_LS               = m_pitchAngle;
00136         
00137         dXOffset_LS             = m_point.getX();
00138         dYOffset_LS             = m_point.getY();
00139         dZOffset_LS             = m_point.getZ();
00140                 
00141         //sin und cos der Nick-, Gier- und Wankwinkel des Laserscanners
00142         double dSPitch_LS       = sin(dPitch_LS);
00143         double dCPitch_LS       = cos(dPitch_LS);
00144         double dSYaw_LS         = sin(dYaw_LS);
00145         double dCYaw_LS         = cos(dYaw_LS);
00146         double dSRoll_LS        = sin(dRoll_LS);
00147         double dCRoll_LS        = cos(dRoll_LS);
00148 
00149         //Trafo-Matrix von Laserscannkoordinaten in Fahrzeugkoordinaten
00150         H_LS_Car(0,0) = dCYaw_LS*dCPitch_LS - dSYaw_LS*dSPitch_LS*dSRoll_LS;
00151         H_LS_Car(0,1) = -dSYaw_LS*dCRoll_LS;
00152         H_LS_Car(0,2) = dCYaw_LS*dSPitch_LS + dSYaw_LS*dSRoll_LS*dCPitch_LS;
00153         H_LS_Car(0,3) = dXOffset_LS;
00154         
00155         H_LS_Car(1,0) = dSYaw_LS*dCPitch_LS + dCYaw_LS*dSRoll_LS*dSPitch_LS;
00156         H_LS_Car(1,1) = dCYaw_LS*dCRoll_LS;
00157         H_LS_Car(1,2) = dSYaw_LS*dSPitch_LS - dCYaw_LS*dSRoll_LS*dCPitch_LS;
00158         H_LS_Car(1,3) = dYOffset_LS;
00159         
00160         H_LS_Car(2,0) = -dCRoll_LS*dSPitch_LS;
00161         H_LS_Car(2,1) = dSRoll_LS;
00162         H_LS_Car(2,2) = dCRoll_LS*dCPitch_LS;
00163         H_LS_Car(2,3) = dZOffset_LS;
00164         
00165         H_LS_Car(3,0) = 0;
00166         H_LS_Car(3,1) = 0;
00167         H_LS_Car(3,2) = 0;
00168         H_LS_Car(3,3) = 1;
00169 
00170 
00171         Vector pt_vehicle(4);
00172         pt_vehicle = H_LS_Car * P_LS;
00173         pt->setXYZ(pt_vehicle[0], pt_vehicle[1], pt_vehicle[2]);
00174 
00175         return true;
00176 }
00177 
00178 
00179 
00180 bool Position3D::operator==(const Position3D& other) const
00181 {
00182         return
00183                 m_yawAngle == other.m_yawAngle
00184                 && m_pitchAngle == other.m_pitchAngle
00185                 && m_rollAngle == other.m_rollAngle
00186                 && m_point == other.m_point
00187                 ;
00188 }
00189 
00190 Position3D& Position3D::set(value_type yaw, value_type pitch, value_type roll,
00191                                                                                 value_type x, value_type y, value_type z)
00192 {
00193         m_yawAngle = yaw;
00194         m_pitchAngle = pitch;
00195         m_rollAngle = roll;
00196         m_point.setXYZ(x, y, z);
00197 
00198         return *this;
00199 }
00200 
00201 
00202 Point3D Position3D::toPoint3D() const
00203 {
00204         return m_point;
00205 }
00206 
00207 Point2D Position3D::toPoint2D() const
00208 {
00209         Point2D result(m_point.getX(), m_point.getY());
00210         return result;
00211 }
00212 
00213 
00214 std::string Position3D::toString() const
00215 {
00216         std::ostringstream ostr;
00217         ostr << "(x " << getX()
00218                  << ", y " << getY()
00219                  << ", z " << getZ() << "m"
00220                  << "; yaw " << getYawAngle() * rad2deg
00221                  << ", pitch " << getPitchAngle() * rad2deg
00222                  << ", roll " << getRollAngle() * rad2deg
00223                  << "deg)";
00224         return ostr.str();
00225 }
00226 
00227 
00228 void Position3D::normalizeAngles()
00229 {
00230         m_yawAngle = normalizeRadians(m_yawAngle);
00231         m_pitchAngle = normalizeRadians(m_pitchAngle);
00232         m_rollAngle = normalizeRadians(m_rollAngle);
00233 }
00234 
00235 }       // namespace datatypes


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