Position3D.cpp
Go to the documentation of this file.
1 //
2 // Position3D.cpp
3 //
4 // 3D-Punkt mit Orientierung.
5 //
6 // SICK AG, 11.11.2011, willhvo
7 //
8 
9 #include "Position3D.hpp"
10 #include "Point2D.hpp"
11 #include "Point3D.hpp"
12 #include <cmath> // for sin, cos, ...
13 #include <cassert>
14 #include <iostream>
15 #include <sstream>
16 
17 
18 namespace datatypes
19 {
20 
22  : m_yawAngle(0)
23  , m_pitchAngle(0)
24  , m_rollAngle(0)
25  , m_point(0.0, 0.0, 0.0)
26 {
27 }
28 
31  : m_yawAngle(yaw)
32  , m_pitchAngle(pitch)
33  , m_rollAngle(roll)
34  , m_point(x, y, z)
35 {
36 }
37 
39  const Point3D& pt)
40  : m_yawAngle(yaw)
41  , m_pitchAngle(pitch)
42  , m_rollAngle(roll)
43  , m_point(pt)
44 {
45 }
46 
47 //
48 // **********************************************
49 //
50 
51 Vector::Vector(UINT16 numOfElements)
52 {
53  if (numOfElements > 0)
54  {
55  m_elements = new double[numOfElements];
56  }
57  else
58  {
59  m_elements = NULL;
60  }
61  m_numOfElements = numOfElements;
62 }
63 
65 {
66  if (m_elements != NULL)
67  {
68  delete m_elements;
69  m_elements = NULL;
70  }
71 }
72 
73 // --------------------------------------------------
74 
75 Matrix::Matrix(UINT16 numOfRows, UINT16 numOfColumns)
76 {
77  if ((numOfRows > 0) && (numOfColumns > 0))
78  {
79  m_elements = new double*[numOfRows];
80  for (UINT16 r=0; r < numOfRows; r++)
81  {
82  m_elements[r] = new double[numOfColumns];
83  }
84  m_numOfRows = numOfRows;
85  m_numOfColumns = numOfColumns;
86  }
87  else
88  {
89  m_elements = NULL;
90  m_numOfRows = 0;
91  m_numOfColumns = 0;
92  }
93 }
94 
96 {
97  if (m_elements != NULL)
98  {
99  for (UINT16 r=0; r < m_numOfRows; r++)
100  {
101  delete m_elements[r];
102  }
103 
104  delete m_elements;
105  m_elements = NULL;
106  }
107 }
108 
109 //
110 // ***********************************************
111 //
112 
113 bool Position3D::transformToVehicle(Point3D* pt) // double *dXPos, double *dYPos, double *dZPos, const CRect& r)
114 {
115  //Punkt in Laserscannkoordinaten
116  Vector P_LS(4);
117 
118  //Scanpunkt in Scannerkoordinaten
119  P_LS[0] = pt->getX(); // *dXPos;
120  P_LS[1] = pt->getY(); // *dYPos;
121  P_LS[2] = pt->getZ(); // *dZPos;
122  P_LS[3] = 1.0;
123 
124  // Sensorposition in Carcoordinates
125  double dXOffset_LS, dYOffset_LS, dZOffset_LS;
126 
127  // Nick-, Gier- und Wankwinkel des Laserscanners
128  double dPitch_LS, dYaw_LS, dRoll_LS;
129 
130  //Laserscanner to Vehicle transformation Matrix
131  Matrix H_LS_Car(4,4);
132 
133  dRoll_LS = m_rollAngle;
134  dYaw_LS = m_yawAngle;
135  dPitch_LS = m_pitchAngle;
136 
137  dXOffset_LS = m_point.getX();
138  dYOffset_LS = m_point.getY();
139  dZOffset_LS = m_point.getZ();
140 
141  //sin und cos der Nick-, Gier- und Wankwinkel des Laserscanners
142  double dSPitch_LS = sin(dPitch_LS);
143  double dCPitch_LS = cos(dPitch_LS);
144  double dSYaw_LS = sin(dYaw_LS);
145  double dCYaw_LS = cos(dYaw_LS);
146  double dSRoll_LS = sin(dRoll_LS);
147  double dCRoll_LS = cos(dRoll_LS);
148 
149  //Trafo-Matrix von Laserscannkoordinaten in Fahrzeugkoordinaten
150  H_LS_Car(0,0) = dCYaw_LS*dCPitch_LS - dSYaw_LS*dSPitch_LS*dSRoll_LS;
151  H_LS_Car(0,1) = -dSYaw_LS*dCRoll_LS;
152  H_LS_Car(0,2) = dCYaw_LS*dSPitch_LS + dSYaw_LS*dSRoll_LS*dCPitch_LS;
153  H_LS_Car(0,3) = dXOffset_LS;
154 
155  H_LS_Car(1,0) = dSYaw_LS*dCPitch_LS + dCYaw_LS*dSRoll_LS*dSPitch_LS;
156  H_LS_Car(1,1) = dCYaw_LS*dCRoll_LS;
157  H_LS_Car(1,2) = dSYaw_LS*dSPitch_LS - dCYaw_LS*dSRoll_LS*dCPitch_LS;
158  H_LS_Car(1,3) = dYOffset_LS;
159 
160  H_LS_Car(2,0) = -dCRoll_LS*dSPitch_LS;
161  H_LS_Car(2,1) = dSRoll_LS;
162  H_LS_Car(2,2) = dCRoll_LS*dCPitch_LS;
163  H_LS_Car(2,3) = dZOffset_LS;
164 
165  H_LS_Car(3,0) = 0;
166  H_LS_Car(3,1) = 0;
167  H_LS_Car(3,2) = 0;
168  H_LS_Car(3,3) = 1;
169 
170 
171  Vector pt_vehicle(4);
172  pt_vehicle = H_LS_Car * P_LS;
173  pt->setXYZ(pt_vehicle[0], pt_vehicle[1], pt_vehicle[2]);
174 
175  return true;
176 }
177 
178 
179 
180 bool Position3D::operator==(const Position3D& other) const
181 {
182  return
183  m_yawAngle == other.m_yawAngle
184  && m_pitchAngle == other.m_pitchAngle
185  && m_rollAngle == other.m_rollAngle
186  && m_point == other.m_point
187  ;
188 }
189 
192 {
193  m_yawAngle = yaw;
194  m_pitchAngle = pitch;
195  m_rollAngle = roll;
196  m_point.setXYZ(x, y, z);
197 
198  return *this;
199 }
200 
201 
203 {
204  return m_point;
205 }
206 
208 {
209  Point2D result(m_point.getX(), m_point.getY());
210  return result;
211 }
212 
213 
214 std::string Position3D::toString() const
215 {
216  std::ostringstream ostr;
217  ostr << "(x " << getX()
218  << ", y " << getY()
219  << ", z " << getZ() << "m"
220  << "; yaw " << getYawAngle() * rad2deg
221  << ", pitch " << getPitchAngle() * rad2deg
222  << ", roll " << getRollAngle() * rad2deg
223  << "deg)";
224  return ostr.str();
225 }
226 
227 
229 {
233 }
234 
235 } // namespace datatypes
void setXYZ(double x, double y, double z)
Sets the coordinates of this point to the given values.
Definition: Point3D.hpp:84
value_type m_yawAngle
Yaw angle [rad].
Definition: Position3D.hpp:249
Position3D & set(value_type yaw, value_type pitch, value_type roll, value_type x, value_type y, value_type z)
Set all values.
Definition: Position3D.cpp:190
This class defines a point in the three-dimensional plane.
Definition: Point3D.hpp:25
double getY() const
Returns the y-coordinate of this point.
Definition: Point3D.hpp:64
value_type getX() const
x-coordinate [m] of the sensor in the vehicle coordinate system
Definition: Position3D.hpp:200
#define rad2deg
uint16_t UINT16
A Position with orientation.
Definition: Position3D.hpp:149
Position3D()
Empty constructor.
Definition: Position3D.cpp:21
Matrix(UINT16 numOfRows, UINT16 numOfColumns)
Definition: Position3D.cpp:75
value_type m_rollAngle
Roll angle [rad].
Definition: Position3D.hpp:253
value_type m_pitchAngle
Pitch angle [rad].
Definition: Position3D.hpp:251
value_type getPitchAngle() const
Pitch angle [rad] of the sensor in the vehicle coordinate system.
Definition: Position3D.hpp:195
value_type getYawAngle() const
Yaw angle [rad] of the sensor in the vehicle coordinate system.
Definition: Position3D.hpp:193
Point2D toPoint2D() const
Returns the x/y components of this class, converted into a Point2D object.
Definition: Position3D.cpp:207
Vector(UINT16 numOfElements)
Definition: Position3D.cpp:51
double getZ() const
Returns the z-coordinate of this point.
Definition: Point3D.hpp:65
value_type getRollAngle() const
Roll angle [rad] of the sensor in the vehicle coordinate system.
Definition: Position3D.hpp:197
bool transformToVehicle(Point3D *pt)
Definition: Position3D.cpp:113
void normalizeAngles()
Normalize all three internal angles to the range [-PI..PI].
Definition: Position3D.cpp:228
double getX() const
Returns the x-coordinate of this point.
Definition: Point3D.hpp:63
double normalizeRadians(double radians)
Definition: MathToolbox.cpp:31
value_type getY() const
y-coordinate [m] of the sensor in the vehicle coordinate system
Definition: Position3D.hpp:202
Point3D toPoint3D() const
Returns the x/y/z components of this class, converted into a Point3D object.
Definition: Position3D.cpp:202
bool operator==(const Position3D &other) const
Equality predicate.
Definition: Position3D.cpp:180
value_type getZ() const
z-coordinate [m] of the sensor in the vehicle coordinate system
Definition: Position3D.hpp:204
std::string toString() const
Definition: Position3D.cpp:214


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