Position3D.hpp
Go to the documentation of this file.
1 //
2 // Position3D.hpp
3 //
4 
5 #ifndef POSITION3D_HPP
6 #define POSITION3D_HPP
7 
8 #include "../BasicDatatypes.hpp"
9 #include "Point3D.hpp"
10 #include "../tools/errorhandler.hpp"
11 
12 namespace datatypes
13 {
14 
15 class Point2D;
16 //class Point3D;
17 
18 class Matrix;
19 
20 
21 //
22 // ***********************************************
23 //
24 class Vector
25 {
26 public:
27  Vector(UINT16 numOfElements);
28  ~Vector();
29 
30  inline const double operator[](const UINT16 elementNr) const;
31  inline double& operator[](const UINT16 elementNr);
32  inline Vector& operator=(const Vector& in);
33 // inline const Vector operator*(Matrix& matrix);
34 
35 private:
37  double* m_elements;
38 };
39 
40 // Keine Pruefung des Index wg. Laufzeit
41 inline Vector& Vector::operator=(const Vector& in)
42 {
44  {
45  dieWithError("Vector::operator=: Vector size does not match!");
46  return (*this);
47  }
48 
49  for (UINT16 i = 0; i<m_numOfElements; i++)
50  {
51  m_elements[i] = in[i];
52  }
53  return (*this);
54 }
55 
56 // Keine Pruefung des Index wg. Laufzeit
57 inline const double Vector::operator[](const UINT16 elementNr) const
58 {
59  return (m_elements[elementNr]);
60 }
61 
62 // Fuer v[p] = x
63 inline double& Vector::operator[](const UINT16 elementNr)
64 {
65  return (m_elements[elementNr]);
66 }
67 
68 class Matrix
69 {
70 public:
71  Matrix(UINT16 numOfRows, UINT16 numOfColumns);
72  ~Matrix();
73 
74  inline const double operator()(const UINT16 rowNr, const UINT16 columnNr) const;
75  inline double& operator()(const UINT16 rowNr, const UINT16 columnNr);
76  inline const Vector operator*(Vector& vector);
77 
78 private:
81  double** m_elements;
82 };
83 
84 // Fuer x = m(r,c);
85 // Keine Pruefung des Index wg. Laufzeit
86 inline const double Matrix::operator()(const UINT16 rowNr, const UINT16 columnNr) const
87 {
88  return (m_elements[rowNr][columnNr]);
89 }
90 
91 // Fuer m(r,c) = x;
92 // Keine Pruefung des Index wg. Laufzeit
93 inline double& Matrix::operator()(const UINT16 rowNr, const UINT16 columnNr)
94 {
95  return (m_elements[rowNr][columnNr]);
96 }
97 
98 // Fuer v2 = m * v1;
99 // Vektor-Laenge und Spaltenanzahl der Matrix muessen gleich sein!
100 inline const Vector Matrix::operator*(Vector& vector)
101 {
102  UINT16 row, col;
103  double sum;
104  Vector result(m_numOfRows);
105 
106  for (row = 0; row < m_numOfRows; row++)
107  {
108  sum = 0.0;
109  for (col=0; col < m_numOfColumns; col++)
110  {
111  sum += m_elements[row][col] * vector[col];
112  }
113  result[row] = sum;
114  }
115 
116  return result;
117 }
118 
119 
120 // Fuer v2 = v1 * m;
121 /*
122 inline const Vector Vector::operator*(Matrix& matrix)
123 {
124  UINT16 row, col;
125  double sum;
126  Vector result(m_numOfElements);
127 
128  for (row = 0; row < m_numOfElements; row++)
129  {
130  sum = 0.0;
131  for (col=0; col < m_numOfElements; col++)
132  {
133  sum += matrix(row, col) * m_elements[col];
134  }
135  result[row] = sum;
136  }
137 
138  return result;
139 }
140 */
141 
142 //
143 // *************************************************
144 //
145 
147 
149 class Position3D : public BasicData
150 {
151  typedef double value_type;
152 
153 public:
155 
158  Position3D();
159 
161 
166  Position3D(value_type yaw, value_type pitch, value_type roll,
167  value_type x, value_type y, value_type z);
168 
170 
179  Position3D(value_type yaw, value_type pitch, value_type roll,
180  const Point3D& point);
181 
182  // Estimate the memory usage of this object
183  inline virtual const UINT32 getUsedMemory() const {return sizeof(*this);};
184 
185  // Die eigene Position3D wird als Laserscanner-Mountingposition interpretiert
186  bool transformToVehicle(Point3D* pt); // double *dXPos, double *dYPos, double *dZPos, const CRect& r)
187 
188 
190  bool operator==(const Position3D& other) const;
191 
193  value_type getYawAngle() const { return m_yawAngle; }
195  value_type getPitchAngle() const { return m_pitchAngle; }
197  value_type getRollAngle() const { return m_rollAngle; }
198 
200  value_type getX() const { return m_point.getX(); }
202  value_type getY() const { return m_point.getY(); }
204  value_type getZ() const { return m_point.getZ(); }
205 
207  Position3D toPosition3D() const;
208 
210  Point3D toPoint3D() const;
211 
213  Point2D toPoint2D() const;
214 
216  void setX(value_type x) { m_point.setX(x); }
218  void setY(value_type y) { m_point.setY(y); }
220  void setZ(value_type z) { m_point.setZ(z); }
222  void setYawAngle(value_type angle) { m_yawAngle = angle; }
224  void setPitchAngle(value_type angle) { m_pitchAngle = angle; }
226  void setRollAngle(value_type angle) { m_rollAngle = angle; }
227 
229 
237  Position3D& set(value_type yaw, value_type pitch, value_type roll,
238  value_type x, value_type y, value_type z);
239 
241  void normalizeAngles();
242 
243  // For debugging output: Conversion to string.
244  std::string toString() const;
245 
246 private:
247  // Orientierung im 3D-Raum
249  value_type m_yawAngle;
251  value_type m_pitchAngle;
253  value_type m_rollAngle;
254 
255  Point3D m_point; // Koordinaten im 3D-Raum
256 };
257 
258 } // namespace datatypes
259 
260 
261 #endif // POSITION3D_HPP
std::string toString(const PositionWGS84::PositionWGS84SourceType &type)
value_type m_yawAngle
Yaw angle [rad].
Definition: Position3D.hpp:249
This class defines a point in the three-dimensional plane.
Definition: Point3D.hpp:25
Vector & operator=(const Vector &in)
Definition: Position3D.hpp:41
void setPitchAngle(value_type angle)
Pitch angle [rad] of the sensor in the vehicle coordinate system.
Definition: Position3D.hpp:224
value_type getX() const
x-coordinate [m] of the sensor in the vehicle coordinate system
Definition: Position3D.hpp:200
uint16_t UINT16
A Position with orientation.
Definition: Position3D.hpp:149
void setRollAngle(value_type angle)
Roll angle [rad] of the sensor in the vehicle coordinate system.
Definition: Position3D.hpp:226
value_type m_rollAngle
Roll angle [rad].
Definition: Position3D.hpp:253
uint32_t UINT32
value_type m_pitchAngle
Pitch angle [rad].
Definition: Position3D.hpp:251
void setY(value_type y)
y-coordinate [m] of the sensor in the vehicle coordinate system
Definition: Position3D.hpp:218
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
const double operator()(const UINT16 rowNr, const UINT16 columnNr) const
Definition: Position3D.hpp:86
void dieWithError(std::string errorMessage)
double * m_elements
Definition: Position3D.hpp:37
Vector(UINT16 numOfElements)
Definition: Position3D.cpp:51
const double operator[](const UINT16 elementNr) const
Definition: Position3D.hpp:57
value_type getRollAngle() const
Roll angle [rad] of the sensor in the vehicle coordinate system.
Definition: Position3D.hpp:197
bool operator==(const Box2D &b1, const Box2D &b2)
Definition: Box2D.hpp:269
void setX(value_type x)
x-coordinate [m] of the sensor in the vehicle coordinate system
Definition: Position3D.hpp:216
double ** m_elements
Definition: Position3D.hpp:81
void setYawAngle(value_type angle)
Yaw angle [rad] of the sensor in the vehicle coordinate system.
Definition: Position3D.hpp:222
void setZ(value_type z)
z-coordinate [m] of the sensor in the vehicle coordinate system
Definition: Position3D.hpp:220
const Point2D operator*(const Point2D &p, Point2D::value_type factor)
Definition: Point2D.hpp:326
const Vector operator*(Vector &vector)
Definition: Position3D.hpp:100
value_type getY() const
y-coordinate [m] of the sensor in the vehicle coordinate system
Definition: Position3D.hpp:202
UINT16 m_numOfElements
Definition: Position3D.hpp:36
virtual const UINT32 getUsedMemory() const
Definition: Position3D.hpp:183
value_type getZ() const
z-coordinate [m] of the sensor in the vehicle coordinate system
Definition: Position3D.hpp:204


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