MRPT  2.0.4
CLandmark.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "vision-precomp.h" // Precompiled headers
11 
12 #include <mrpt/maps/CLandmark.h>
13 #include <mrpt/obs/CObservation.h>
16 
17 using namespace mrpt::obs;
18 using namespace mrpt::maps;
19 using namespace mrpt::poses;
20 
22 
23 // Initialization:
24 CLandmark::TLandmarkID CLandmark::m_counterIDs =
25  static_cast<CLandmark::TLandmarkID>(0);
26 
27 CLandmark::~CLandmark() = default;
28 
29 /*---------------------------------------------------------------
30  Destructor
31  ---------------------------------------------------------------*/
32 void CLandmark::getPose(CPointPDFGaussian& pose) const
33 {
34  pose.mean.x(pose_mean.x);
35  pose.mean.y(pose_mean.y);
36  pose.mean.z(pose_mean.z);
37 
38  pose.cov(0, 0) = pose_cov_11;
39  pose.cov(1, 1) = pose_cov_22;
40  pose.cov(2, 2) = pose_cov_33;
41 
42  pose.cov(0, 1) = pose.cov(1, 0) = pose_cov_12;
43  pose.cov(0, 2) = pose.cov(2, 0) = pose_cov_13;
44 
45  pose.cov(1, 2) = pose.cov(2, 1) = pose_cov_23;
46 }
47 
48 /*---------------------------------------------------------------
49  Destructor
50  ---------------------------------------------------------------*/
51 void CLandmark::setPose(const CPointPDFGaussian& pose)
52 {
53  pose_mean.x = pose.mean.x();
54  pose_mean.y = pose.mean.y();
55  pose_mean.z = pose.mean.z();
56 
57  pose_cov_11 = d2f(pose.cov(0, 0));
58  pose_cov_22 = d2f(pose.cov(1, 1));
59  pose_cov_33 = d2f(pose.cov(2, 2));
60  pose_cov_12 = d2f(pose.cov(0, 1));
61  pose_cov_13 = d2f(pose.cov(0, 2));
62  pose_cov_23 = d2f(pose.cov(1, 2));
63 }
64 
65 uint8_t CLandmark::serializeGetVersion() const { return 4; }
66 void CLandmark::serializeTo(mrpt::serialization::CArchive& out) const
67 {
68  out << features << pose_mean << normal << pose_cov_11 << pose_cov_22
69  << pose_cov_33 << pose_cov_12 << pose_cov_13 << pose_cov_23 << ID
70  << timestampLastSeen << seenTimesCount;
71 }
72 
73 void CLandmark::serializeFrom(
74  mrpt::serialization::CArchive& in, uint8_t version)
75 {
76  switch (version)
77  {
78  case 0:
79  case 1:
80  case 2:
81  case 3:
83  "Importing from this old version is not implemented");
84  break;
85  case 4:
86  {
87  in >> features >> pose_mean >> normal >> pose_cov_11 >>
88  pose_cov_22 >> pose_cov_33 >> pose_cov_12 >> pose_cov_13 >>
89  pose_cov_23 >> ID >> timestampLastSeen >> seenTimesCount;
90  }
91  break;
92  default:
94  };
95 }
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
CPoint3D mean
The mean value.
int64_t TLandmarkID
The type for the IDs of landmarks.
Definition: CLandmark.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
float d2f(const double d)
shortcut for static_cast<float>(double)
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
This namespace contains representation of robot actions and observations.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
The class for storing "landmarks" (visual or laser-scan-extracted features,...)
Definition: CLandmark.h:35
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::vision::TStereoCalibResults out
A gaussian distribution for 3D points.



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020