Object.cpp
Go to the documentation of this file.
1 //
2 // Object.cpp
3 //
4 
5 #include "Object.hpp"
6 #include "../tools/errorhandler.hpp"
7 #include "../tools/MathToolbox.hpp" // for NaN_double
8 
9 namespace datatypes
10 {
11 
12 
14  : m_objectId(0)
15  , m_flags(0)
16  , m_objectAge(0)
17  , m_hiddenStatusAge(0)
18  , m_timestamp()
19  , m_classification(Object::Unclassified)
20  , m_classificationAge(0)
21  , m_classificationQuality(0.0f)
22  , m_centerPoint(Point2D(0, 0))
23  , m_centerPointSigma(Point2D(0, 0))
24  , m_courseAngle(0.0f)
25  , m_courseAngleSigma(0.0f)
26  , m_relativeVelocity(Point2D(0, 0))
27  , m_relativeVelocitySigma(Point2D(0, 0))
28  , m_absoluteVelocity(Point2D(0, 0))
29  , m_absoluteVelocitySigma(Point2D(0, 0))
30  , m_objectBox(Point2D(0, 0))
31  , m_objectBoxSigma(Point2D(0, 0))
32  , m_boundingBoxCenter(Point2D(0, 0))
33  , m_boundingBox(Point2D(0, 0))
34  , m_closestPoint(Point2D(0, 0))
35  , m_contourPoints()
36  , m_vehicleWLANid(0)
37  , m_objectHeight(0)
38  , m_objectHeightSigma(0)
39  , m_objectMass(0)
40  , m_maxAbsoluteVelocity(NaN_double)
41  , m_normalizedMeanPointDist(0)
42  , m_totalTrackingDuration(0)
43  , m_totalTrackedPathLength(0)
44  , m_isValid(true)
45 {
46 }
47 
49 {
50 }
51 
52 bool Object::operator==(const Object& other) const
53 {
54  bool b =
55  m_objectId == other.m_objectId
56  && m_flags == other.m_flags
57  && m_objectAge == other.m_objectAge
59  && m_timestamp == other.m_timestamp
63  && m_centerPoint == other.m_centerPoint
65  && m_courseAngle == other.m_courseAngle
71  && m_objectBox == other.m_objectBox
74  && m_boundingBox == other.m_boundingBox
75  && m_closestPoint == other.m_closestPoint
77  && m_objectHeight == other.m_objectHeight
79  && m_objectMass == other.m_objectMass
80  ;
81  return b;
82 }
83 
84 
86 {
88 }
89 
90 // The set-functions with checks for value ranges
92 {
93  assert(v >= 0);
94  assert(v <= 1);
96 }
97 
99 {
100  assert((v.getX() >= 0) || isNaN(v.getX()));
101  assert((v.getY() >= 0) || isNaN(v.getY()));
102  m_centerPointSigma = v;
103 }
104 
105 void Object::setCourseAngle(double new_angle)
106 {
107  double normd_angle = normalizeRadians(new_angle);
108  if (!fuzzyCompare(normd_angle, new_angle))
109  {
110  printWarning("Object::setCourseAngle was called with angle " + ::toString(new_angle, 2) +
111  " which is outside of its [-pi,pi] definition range (but the calling function should have ensured to normalize into that interval) - normalizing it back into that range now (" +
112  ::toString(normd_angle, 2) + ").");
113  new_angle = normd_angle;
114  // Note: We intentionally do not throw an exception here
115  // because erroneous normalizations might occur very seldomly
116  // (especially the exact value of +PI), hence might go through
117  // development unnoticed, but an exception causes a crash in
118  // front of the customer, which is bad. Also, we *do* print
119  // the warning even though we also normalize it so that the
120  // developers have a chance to notice this problem and fix
121  // their calling code.
122  }
123  m_courseAngle = new_angle;
124 }
125 
127 {
128  assert(v >= 0);
129  m_courseAngleSigma = v;
130 }
131 
133 {
134  m_absoluteVelocity = v;
135  double absolute = v.dist();
137  || absolute > getMaxAbsoluteVelocity())
138  {
139  setMaxAbsoluteVelocity(absolute);
140  }
141 }
142 
144 {
145  assert((v.getX() >= 0) || isNaN(v.getX()));
146  assert((v.getY() >= 0) || isNaN(v.getY()));
148 }
149 
151 {
152  assert((v.getX() >= 0) || isNaN(v.getX()));
153  assert((v.getY() >= 0) || isNaN(v.getY()));
154  m_objectBox = v;
155 }
156 
158 {
159  assert((v.getX() >= 0) || isNaN(v.getX()));
160  assert((v.getY() >= 0) || isNaN(v.getY()));
161  m_objectBoxSigma = v;
162 }
163 
165 {
166  assert((v.getX() >= 0) || isNaN(v.getX()));
167  assert((v.getY() >= 0) || isNaN(v.getY()));
168  m_boundingBox = v;
169 }
170 
172 {
174 }
175 
177 {
178  assert(v >= 0);
180 }
181 
182 void Object::setObjectMass(double v)
183 {
184  assert(v >= 0);
185  m_objectMass = v;
186 }
187 
189 {
190  assert(v.size() <= 0xff);
191  m_contourPoints = v;
192 }
193 
195 {
196  assert(m_contourPoints.size() < 0xff);
197  m_contourPoints.push_back(cp);
198 }
199 
200 
201 std::string Object::toString() const
202 {
203  std::string text = "Object: ";
204  text += "id=" + ::toString(m_objectId);
205  text += ", flags=" + ::toString(m_flags);
206  text += ", objAge=" + ::toString(m_objectAge);
207  text += ", hiddenStatusAge=" + ::toString(m_hiddenStatusAge);
208  text += ", timestamp=" + m_timestamp.toString();
209  text += std::string(", classification=") + objectClassificationToString(m_classification);
210 
211  text += ", classificationAge=" + ::toString(m_classificationAge);
212 
213  text += ", classificationQuality=" + ::toString(m_classificationQuality, 2);
214 
215  text += ", centerPoint=" + m_centerPoint.toString();
216  text += ", centerPointSigma=" + m_centerPointSigma.toString();
217 
218  text += ", courseAngle=" + ::toString(m_courseAngle, 2);
219  text += ", courseAngleSigma=" + ::toString(m_courseAngleSigma, 2);
220 
221  text += ", relativeVelocity=" + m_relativeVelocity.toString();
222  text += ", relativeVelocitySigma=" + m_relativeVelocitySigma.toString();
223 
224  text += ", absoluteVelocity=" + m_absoluteVelocity.toString();
225  text += ", absoluteVelocitySigma=" + m_absoluteVelocitySigma.toString();
226 
227  text += ", objectBox=" + m_objectBox.toString();
228  text += ", objectBoxSigma=" + m_objectBoxSigma.toString();
229 
230  text += ", boundingBox=" + m_boundingBox.toString();
231  text += ", closestPoint=" + m_closestPoint.toString();
232 
233  text += ", contourPointsNum=" + ::toString(m_contourPoints.size());
234  text += ", contourPoints=" + Polygon2D(m_contourPoints).toString();
235 
236  text += ", vehicleWLANid=" + ::toString(m_vehicleWLANid, 2);
237  text += ", objectHeight=" + ::toString(m_objectHeight, 2);
238  text += ", objectHeightSigma=" + ::toString(m_objectHeightSigma, 2);
239  text += ", objectMass=" + ::toString(m_objectMass, 2);
240 
241  text += ", maxAbsoluteVelocity=" + ::toString(m_maxAbsoluteVelocity, 2);
242  text += ", normalizedMeanPointDist=" + ::toString(m_normalizedMeanPointDist, 2);
243  text += ", totalTrackingDuration=" + ::toString(m_totalTrackingDuration, 2);
244  text += ", totalTrackedPathLength=" + ::toString(m_totalTrackedPathLength, 2);
245 
246  return text;
247 }
248 
249 #define TYPE_TO_STR(tstr) tstr : return #tstr
250 
252 {
253  switch (v)
254  {
255  case TYPE_TO_STR ( Unclassified );
256  case TYPE_TO_STR ( UnknownSmall );
257  case TYPE_TO_STR ( UnknownBig );
258  case TYPE_TO_STR ( Pedestrian );
259  case TYPE_TO_STR ( Bike );
260  case TYPE_TO_STR ( Car );
261  case TYPE_TO_STR ( Truck );
262  case TYPE_TO_STR ( Structure_Pylon );
263  case TYPE_TO_STR ( Structure_Beacon );
266  case TYPE_TO_STR ( NumClasses );
267  case TYPE_TO_STR ( Unknown );
268  default:
269  throw std::invalid_argument("Unknown object classification " + ::toString(int(v)));
270  return "<unknown>";
271  }
272 }
273 
275 {
276  if (s == "Unclassified" || s == "Unclass")
277  return Unclassified;
278  else if (s == "UnknownSmall" || s == "us")
279  return UnknownSmall;
280  else if (s == "UnknownBig" || s == "UB")
281  return UnknownBig;
282  else if (s == "Pedestrian" || s == "Ped")
283  return Pedestrian;
284  else if (s == "Bike")
285  return Bike;
286  else if (s == "Car")
287  return Car;
288  else if (s == "Truck")
289  return Truck;
290  else if (s == "Structure_Pylon" || s == "Pylon")
291  return Structure_Pylon;
292  else if (s == "Structure_Beacon" || s == "Bcn")
293  return Structure_Beacon;
294  else if (s == "Structure_GuardRail" || s == "GrdRl")
295  return Structure_GuardRail;
296  else if (s == "Structure_ConcreteBarrier" || s == "CBar")
298  else if (s == "NumClasses" || s == "#Classes")
299  return NumClasses;
300  else if (s == "Unknown")
301  return Unknown;
302  else
303  {
304  throw std::invalid_argument("Unknown object classification string \"" + s + "\"");
305  return Unknown;
306  }
307 }
308 
310 {
311  const std::string s = objectClassificationToString(v);
312  return s + " (" + ::toString((int)v) + ")";
313 }
314 
316 {
317  switch (v)
318  {
319  case Unclassified:
320  return "Unclass";
321  case UnknownSmall:
322  return "us";
323  case UnknownBig:
324  return "UB";
325  case Pedestrian:
326  return "Ped";
327  case Bike:
328  return "Bike";
329  case Car:
330  return "Car";
331  case Truck:
332  return "Truck";
333  case Structure_Pylon:
334  return "Pylon";
335  case Structure_Beacon:
336  return "Bcn";
337  case Structure_GuardRail:
338  return "GrdRl";
340  return "CBar";
341  case NumClasses:
342  return "#Classes";
343  case Unknown:
344  return "Unknown";
345  default:
346  throw std::invalid_argument("Unknown object classification " + ::toString(int(v)));
347  return "<unknown>";
348  }
349 }
350 
351 // ////////////////////////////////////////////////////////////
352 
353 void Object::getObjectBoxVarCovar(double &var_x, double &var_y, double &covar_xy) const
354 {
355  // Square the stored standard deviation to get the variances
356  double var_x_obj = sqr(getObjectBoxSigma().getX());
357  double var_y_obj = sqr(getObjectBoxSigma().getY());
358 
359  // Rotate the variances back by the angle given by the courseAngle
360  double dCos = std::cos(getCourseAngle());
361  double dSin = std::sin(getCourseAngle());
362  double cos_sq = sqr(dCos);
363  double sin_sq = sqr(dSin);
364  double cos_sin = dCos * dSin;
365 
366  // And rotate the covariance matrix C_o: To rotate C_o by the
367  // rotation matrix R, we need to calculate C_v = R * C_o * R^T
368  var_x = cos_sq * var_x_obj + sin_sq * var_y_obj;
369  covar_xy = -cos_sin * var_x_obj + cos_sin * var_y_obj;
370  var_y = sin_sq * var_x_obj + cos_sq * var_y_obj;
371 }
372 
373 /*
374 void Object::compensateEgoMovement(const Point2D& deltaPos, double deltaAngle)
375 {
376  m_centerPoint -= deltaPos;
377  m_centerPoint = m_centerPoint.rotated(-deltaAngle);
378 
379  m_centerPointSigma = m_centerPointSigma.rotated(-deltaAngle);
380 
381  m_courseAngle -= deltaAngle;
382  m_courseAngleSigma -= deltaAngle;
383 
384  m_relativeVelocity = m_relativeVelocity.rotated(-deltaAngle);
385  m_relativeVelocitySigma = m_relativeVelocitySigma.rotated(-deltaAngle);
386  m_absoluteVelocity = m_absoluteVelocity.rotated(-deltaAngle);
387  m_absoluteVelocitySigma = m_absoluteVelocitySigma.rotated(-deltaAngle);
388 
389  m_boundingBoxCenter -= deltaPos;
390  m_boundingBoxCenter = m_boundingBoxCenter.rotated(-deltaAngle);
391 
392  // m_boundingBox TODO: anpassen
393  m_closestPoint -= deltaPos;
394  m_closestPoint = m_closestPoint.rotated(-deltaAngle);
395 
396  // transformation of the contour points - otherwise the contour points are invalid!
397 
398  Point2DVector::iterator pt;
399  for (pt = m_contourPoints.begin(); pt != m_contourPoints.end(); pt++)
400  {
401  *pt -= deltaPos;
402  *pt = pt->rotated(-deltaAngle);
403  }
404 }
405 */
406 
408 {
409  m_objectAge++;
410 }
411 
413 {
414 // if (v < 0)
415 // throw InvalidArgumentException("setMaxAbsoluteVelocity called with negative argument " + ::toString(v, 2) + ", but must be non-negative.");
417 }
419 {
420 // if (v < 0)
421 // throw InvalidArgumentException("setNormalizedMeanPointDist called with negative argument " + ::toString(v, 2) + ", but must be non-negative.");
423 }
425 {
426 // if (v < 0)
427 // throw InvalidArgumentException ("setTotalTrackingDuration called with negative argument " + ::toString(v, 2) + ", but must be non-negative.");
429 }
431 {
432 // if (v < 0)
433 // throw InvalidArgumentException("setTotalTrackedPathLength called with negative argument " + ::toString(v,2) + ", but must be non-negative.");
435 }
436 
438 {
439  // mean velocity
440  return (m_totalTrackingDuration > 0)
442  : 0.0f;
443 }
444 
446 //
447 // ******************* ObjectList ***************************
448 //
450 
452  : base_class()
453 {
455  m_timestamp.set(0.0);
456 }
457 
458 bool ObjectList::operator==(const ObjectList& other) const
459 {
460  return (m_timestamp == other.m_timestamp)
461  && (static_cast<const base_class&>(*this)
462  == static_cast<const base_class&>(other));
463 }
464 
465 
466 void ObjectList::setTimestamp(const Time& timestamp)
467 {
468  m_timestamp = timestamp;
469 }
470 
471 /*
472 void ObjectList::compensateEgoMovement(const Point2D& deltaPos, double deltaAngle)
473 {
474  iterator obj;
475  for (obj = begin(); obj != end(); obj++)
476  {
477  obj->compensateEgoMovement(deltaPos, deltaAngle);
478  }
479 }
480 */
481 
482 //
483 // Alter aller Objekte um 1 erhoehen.
484 //
486 {
487  iterator obj;
488  for (obj = begin(); obj != end(); obj++)
489  {
490  obj->incrementObjectAge();
491  }
492 }
493 
494 } // namespace datatypes
UINT16 m_objectId
Definition: Object.hpp:447
void incrementObjectAge()
Just increment objectAge by one.
Definition: Object.cpp:407
const double NaN_double
Not-a-Number in double precision.
Definition: MathToolbox.cpp:13
double getCourseAngle() const
Definition: Object.hpp:207
void setObjectBox(const Point2D &v)
Definition: Object.cpp:150
std::string toString(UINT16 digits=2) const
Text output for debugging.
Definition: Point2D.cpp:75
double m_courseAngleSigma
Definition: Object.hpp:461
Point2D m_relativeVelocitySigma
Definition: Object.hpp:463
Point2D m_absoluteVelocitySigma
Definition: Object.hpp:465
double m_objectHeight
The height of this object in [m] (most probably received through WLAN data).
Definition: Object.hpp:479
std::string toString() const
Text output for debugging.
Definition: Polygon2D.cpp:584
value_type getX() const
Definition: Point2D.hpp:70
double m_totalTrackedPathLength
Definition: Object.hpp:499
A rotated 2-dimensional box in the plane.
Definition: Box2D.hpp:34
void setTotalTrackedPathLength(double v)
Definition: Object.cpp:430
Definition: Time.hpp:44
const Point2D & getCenterPoint() const
Definition: Object.hpp:187
std::string toString() const
Definition: Object.cpp:201
static const char * objectClassificationToShortString(ObjectClassification v)
Returns the given classification value as a short string.
Definition: Object.cpp:315
void setAbsoluteVelocity(const Point2D &v)
Definition: Object.cpp:132
A polygon of 2D-points.
Definition: Polygon2D.hpp:43
Point2D m_boundingBoxCenter
Center of the bounding box.
Definition: Object.hpp:469
void setCourseAngleSigma(double v)
Definition: Object.cpp:126
Point2D m_closestPoint
The point of this object that is closest to the origin of the vehicle coordinate system.
Definition: Object.hpp:473
Point2D m_boundingBox
A rectangle in parallel to the vehicle coordinate system (a paraxial rectangle) that contains (bounds...
Definition: Object.hpp:470
const Point2D & getObjectBox() const
Definition: Object.hpp:273
void setBoundingBoxCenter(const Point2D &v)
Definition: Object.cpp:171
value_type dist() const
Definition: Point2D.hpp:353
static Object::ObjectClassification stringToObjectClassification(const std::string &s)
Definition: Object.cpp:274
value_type getY() const
Definition: Point2D.hpp:73
void getObjectBoxVarCovar(double &var_x, double &var_y, double &covar_xy) const
Definition: Object.cpp:353
UINT16 m_flags
reserved
Definition: Object.hpp:448
void set(double time)
Definition: Time.cpp:69
Point2D m_centerPoint
Center point of object rectangle, given in Vehicle coordinate system.
Definition: Object.hpp:458
bool operator==(const Object &other) const
Equality predicate.
Definition: Object.cpp:52
Box2D getBox() const
Definition: Object.cpp:85
void addContourPoint(const Point2D cp)
Definition: Object.cpp:194
UINT32 m_classificationAge
Counts how long the object has been classified in the current classification.
Definition: Object.hpp:455
void setClassificationQuality(double v)
Definition: Object.cpp:91
bool operator==(const ObjectList &other) const
Equality predicate.
Definition: Object.cpp:458
void incrementObjectAge()
Just increment objectAge of all objects by one.
Definition: Object.cpp:485
Polygon2D m_contourPoints
A poly-line that describes the outline of the current object measurement.
Definition: Object.hpp:476
double getMeanAbsoluteVelocity() const
Definition: Object.cpp:437
double getMaxAbsoluteVelocity() const
Definition: Object.hpp:385
#define TYPE_TO_STR(tstr)
Definition: Object.cpp:249
void setNormalizedMeanPointDist(double v)
Definition: Object.cpp:418
double m_maxAbsoluteVelocity
Classification feature: The maximum observed absolute velocity [m/s].
Definition: Object.hpp:486
Point2D m_absoluteVelocity
Velocity of this object [meter/seconds] as absolute velocity; the orientation is relative to the vehi...
Definition: Object.hpp:464
static std::string objectClassificationToStringWithNum(ObjectClassification v)
Returns the given classification value as a string with the integer number included.
Definition: Object.cpp:309
void setTotalTrackingDuration(double v)
Definition: Object.cpp:424
void setContourPoints(const Polygon2D &v)
Definition: Object.cpp:188
double m_objectMass
The mass of this object in kilogram
Definition: Object.hpp:482
ObjectClassification m_classification
The object class that is most likely for this object.
Definition: Object.hpp:454
void setAbsoluteVelocitySigma(const Point2D &v)
Definition: Object.cpp:143
bool fuzzyCompare(double a, double b)
Tests if two double values are nearly equal.
Definition: MathToolbox.hpp:28
const Point2D & getObjectBoxSigma() const
Definition: Object.hpp:297
double normalizeRadians(double radians)
Definition: MathToolbox.cpp:31
static const char * objectClassificationToString(ObjectClassification v)
Returns the given classification value as a string.
Definition: Object.cpp:251
UINT32 m_objectAge
number of scans in which this object has been tracked, or instead time?
Definition: Object.hpp:450
double sqr(double val)
Definition: MathToolbox.hpp:36
void setTimestamp(const Time &timestamp)
Definition: Object.cpp:466
void setCenterPointSigma(const Point2D &v)
Definition: Object.cpp:98
UINT16 m_hiddenStatusAge
Counts how long the object has not been observed but only predicted.
Definition: Object.hpp:451
double m_totalTrackingDuration
Definition: Object.hpp:495
double m_normalizedMeanPointDist
Definition: Object.hpp:491
void setObjectHeightSigma(double v)
Definition: Object.cpp:176
void setMaxAbsoluteVelocity(double v)
Definition: Object.cpp:412
bool isNaN(floatT x)
Checks if a floating point value is Not-a-Number (NaN)
Definition: MathToolbox.hpp:63
void setObjectMass(double v)
Definition: Object.cpp:182
Point2D m_relativeVelocity
Velocity of this object [meter/seconds], relative to the vehicle coordinate system.
Definition: Object.hpp:462
void setBoundingBox(const Point2D &v)
Definition: Object.cpp:164
Point2D m_objectBox
The object&#39;s length and width as a rectangle, relative to the object&#39;s coordinate system...
Definition: Object.hpp:467
double m_objectHeightSigma
The standard deviation of the height of this object in [m] (most probably received through WLAN data)...
Definition: Object.hpp:480
double m_classificationQuality
The quality of the current classification.
Definition: Object.hpp:456
void setCourseAngle(double newCourseAngle)
Definition: Object.cpp:105
void printWarning(std::string message)
std::string toString() const
Definition: Time.cpp:40
double m_courseAngle
named by ISO 8855; also called Orientation or Heading [rad]
Definition: Object.hpp:460
UINT64 m_vehicleWLANid
An identifier to be used by WLAN fusion algorithms.
Definition: Object.hpp:478
Point2D m_centerPointSigma
Definition: Object.hpp:459
Time m_timestamp
Time of when the center point of this object was observed.
Definition: Object.hpp:452
std::vector< Object > base_class
Definition: Object.hpp:523
void setObjectBoxSigma(const Point2D &v)
Definition: Object.cpp:157
Point2D m_objectBoxSigma
Definition: Object.hpp:468


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