Main MRPT website > C++ reference for MRPT 1.5.6
CLandmark.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "vision-precomp.h" // Precompiled headers
11 
12 #include <mrpt/utils/CStream.h>
13 #include <mrpt/maps/CLandmark.h>
14 #include <mrpt/obs/CObservation.h>
16 
17 using namespace mrpt::obs;
18 using namespace mrpt::maps;
19 using namespace mrpt::utils;
20 using namespace mrpt::poses;
21 
22 
24 
25 // Initialization:
26 CLandmark::TLandmarkID CLandmark::m_counterIDs= static_cast<CLandmark::TLandmarkID>(0);
27 
28 /*---------------------------------------------------------------
29  Default constructor
30  ---------------------------------------------------------------*/
32  features(),
33  pose_mean(),
34  normal(),
35  pose_cov_11(),pose_cov_22(),pose_cov_33(),pose_cov_12(),pose_cov_13(),pose_cov_23(),
36  ID( INVALID_LANDMARK_ID ),
37  timestampLastSeen( INVALID_TIMESTAMP ),
38  seenTimesCount(0)
39 {
40 }
41 
42 /*---------------------------------------------------------------
43  Destructor
44  ---------------------------------------------------------------*/
45 CLandmark::~CLandmark()
46 {
47 
48 }
49 
50 /*---------------------------------------------------------------
51  Destructor
52  ---------------------------------------------------------------*/
53 void CLandmark::getPose( CPointPDFGaussian &pose ) const
54 {
55  pose.mean.x( pose_mean.x);
56  pose.mean.y( pose_mean.y);
57  pose.mean.z( pose_mean.z);
58 
59  pose.cov(0,0) = pose_cov_11;
60  pose.cov(1,1) = pose_cov_22;
61  pose.cov(2,2) = pose_cov_33;
62 
63  pose.cov(0,1) = pose.cov(1,0) = pose_cov_12;
64  pose.cov(0,2) = pose.cov(2,0) = pose_cov_13;
65 
66  pose.cov(1,2) = pose.cov(2,1) = pose_cov_23;
67 }
68 
69 /*---------------------------------------------------------------
70  Destructor
71  ---------------------------------------------------------------*/
72 void CLandmark::setPose( const CPointPDFGaussian &pose )
73 {
74  pose_mean.x = pose.mean.x();
75  pose_mean.y = pose.mean.y();
76  pose_mean.z = pose.mean.z();
77 
78  pose_cov_11 = pose.cov(0,0);
79  pose_cov_22 = pose.cov(1,1);
80  pose_cov_33 = pose.cov(2,2);
81  pose_cov_12 = pose.cov(0,1);
82  pose_cov_13 = pose.cov(0,2);
83  pose_cov_23 = pose.cov(1,2);
84 }
85 
86 /*---------------------------------------------------------------
87  writeToStream
88  Implements the writing to a CStream capability of
89  CSerializable objects
90  ---------------------------------------------------------------*/
91 void CLandmark::writeToStream(mrpt::utils::CStream &out, int *version) const
92 {
93  if (version)
94  *version = 4;
95  else
96  {
97  out << features
98  << pose_mean
99  << normal
100  << pose_cov_11 << pose_cov_22 << pose_cov_33
101  << pose_cov_12 << pose_cov_13 << pose_cov_23
102  << ID
103  << timestampLastSeen
104  << seenTimesCount;
105  }
106 }
107 
108 /*---------------------------------------------------------------
109  readFromStream
110  Implements the reading from a CStream capability of
111  CSerializable objects
112  ---------------------------------------------------------------*/
113 void CLandmark::readFromStream(mrpt::utils::CStream &in, int version)
114 {
115  switch(version)
116  {
117  case 0:
118  case 1:
119  case 2:
120  case 3:
121  THROW_EXCEPTION("Importing from this old version is not implemented");
122  break;
123  case 4:
124  {
125  in >> features
126  >> pose_mean
127  >> normal
128  >> pose_cov_11 >> pose_cov_22 >> pose_cov_33
129  >> pose_cov_12 >> pose_cov_13 >> pose_cov_23
130  >> ID
131  >> timestampLastSeen
132  >> seenTimesCount;
133  } break;
134  default:
136  };
137 }
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:39
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
CPoint3D mean
The mean value.
#define THROW_EXCEPTION(msg)
GLuint in
Definition: glew.h:7146
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
int version
Definition: mrpt_jpeglib.h:898
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:17
The class for storing "landmarks" (visual or laser-scan-extracted features,...)
uint64_t TLandmarkID
Unique IDs for landmarks.
#define INVALID_LANDMARK_ID
Used for CObservationBearingRange::TMeasurement::beaconID and others.
A gaussian distribution for 3D points.



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018