MRPT  2.0.4
CMetricMap.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 "obs-precomp.h" // Precompiled headers
11 
12 #include <mrpt/maps/CMetricMap.h>
13 #include <mrpt/maps/CSimpleMap.h>
14 #include <mrpt/obs/CSensoryFrame.h>
15 #include <mrpt/poses/CPoint3D.h>
16 #include <mrpt/poses/CPose3D.h>
17 #include <mrpt/poses/CPosePDF.h>
19 
20 using namespace mrpt::obs;
21 using namespace mrpt::maps;
22 using namespace mrpt::poses;
23 using namespace mrpt::math;
24 using namespace mrpt::tfest;
25 
27 
28 CMetricMap::CMetricMap() = default;
29 /** Erase all the contents of the map */
31 {
32  internal_clear();
33  publishEvent(mrptEventMetricMapClear(this));
34 }
35 
36 void CMetricMap::loadFromProbabilisticPosesAndObservations(
37  const mrpt::maps::CSimpleMap& sfSeq)
38 {
39  CPose3DPDF::Ptr posePDF;
41  const size_t n = sfSeq.size();
42 
43  // Erase previous contents:
44  this->clear();
45 
46  // Insert new content:
47  for (size_t i = 0; i < n; i++)
48  {
49  sfSeq.get(i, posePDF, sf);
50  ASSERTMSG_(posePDF, "Input map has an empty `CPose3DPDF` ptr");
51  ASSERTMSG_(sf, "Input map has an empty `CSensoryFrame` ptr");
52 
53  CPose3D robotPose;
54  posePDF->getMean(robotPose);
55 
56  sf->insertObservationsInto(
57  this, // Insert into THIS map.
58  &robotPose // At this pose.
59  );
60  }
61 }
62 
63 /*---------------------------------------------------------------
64  computeObservationsLikelihood
65  ---------------------------------------------------------------*/
66 double CMetricMap::computeObservationsLikelihood(
67  const CSensoryFrame& sf, const CPose2D& takenFrom)
68 {
69  double lik = 0;
70  for (const auto& it : sf)
71  lik += computeObservationLikelihood(*it, takenFrom);
72 
73  return lik;
74 }
75 
76 double CMetricMap::computeObservationLikelihood(
77  const CObservation& obs, const CPose2D& takenFrom)
78 {
79  return computeObservationLikelihood(obs, CPose3D(takenFrom));
80 }
81 
82 /*---------------------------------------------------------------
83  canComputeObservationLikelihood
84  ---------------------------------------------------------------*/
85 bool CMetricMap::canComputeObservationsLikelihood(const CSensoryFrame& sf) const
86 {
87  bool can = false;
88  for (auto it = sf.begin(); !can && it != sf.end(); ++it)
89  can = can || canComputeObservationLikelihood(**it);
90  return can;
91 }
92 
93 bool CMetricMap::insertObservation(
94  const CObservation& obs, const CPose3D* robotPose)
95 {
96  if (!genericMapParams.enableObservationInsertion) return false;
97 
98  bool done = internal_insertObservation(obs, robotPose);
99  if (done)
100  {
101  OnPostSuccesfulInsertObs(obs);
102  publishEvent(mrptEventMetricMapInsert(this, &obs, robotPose));
103  }
104  return done;
105 }
106 
107 bool CMetricMap::insertObservationPtr(
108  const CObservation::Ptr& obs, const CPose3D* robotPose)
109 {
110  MRPT_START
111  if (!obs)
112  {
113  THROW_EXCEPTION("Trying to pass a null pointer.");
114  }
115  return insertObservation(*obs, robotPose);
116  MRPT_END
117 }
118 
119 void CMetricMap::determineMatching2D(
120  [[maybe_unused]] const mrpt::maps::CMetricMap* otherMap,
121  [[maybe_unused]] const CPose2D& otherMapPose,
122  [[maybe_unused]] TMatchingPairList& correspondences,
123  [[maybe_unused]] const TMatchingParams& params,
124  [[maybe_unused]] TMatchingExtraResults& extraResults) const
125 {
126  MRPT_START
127  THROW_EXCEPTION("Virtual method not implemented in derived class.");
128  MRPT_END
129 }
130 
131 void CMetricMap::determineMatching3D(
132  [[maybe_unused]] const mrpt::maps::CMetricMap* otherMap,
133  [[maybe_unused]] const CPose3D& otherMapPose,
134  [[maybe_unused]] TMatchingPairList& correspondences,
135  [[maybe_unused]] const TMatchingParams& params,
136  [[maybe_unused]] TMatchingExtraResults& extraResults) const
137 {
138  MRPT_START
139  THROW_EXCEPTION("Virtual method not implemented in derived class.");
140  MRPT_END
141 }
142 
143 float CMetricMap::compute3DMatchingRatio(
144  [[maybe_unused]] const mrpt::maps::CMetricMap* otherMap,
145  [[maybe_unused]] const mrpt::poses::CPose3D& otherMapPose,
146  [[maybe_unused]] const TMatchingRatioParams& params) const
147 {
148  MRPT_START
149  THROW_EXCEPTION("Virtual method not implemented in derived class.");
150  MRPT_END
151 }
152 
153 float CMetricMap::squareDistanceToClosestCorrespondence(
154  [[maybe_unused]] float x0, [[maybe_unused]] float y0) const
155 {
156  MRPT_START
157  THROW_EXCEPTION("Virtual method not implemented in derived class.");
158  MRPT_END
159 }
160 
161 bool CMetricMap::canComputeObservationLikelihood(
162  const mrpt::obs::CObservation& obs) const
163 {
164  if (genericMapParams.enableObservationLikelihood)
165  return internal_canComputeObservationLikelihood(obs);
166  else
167  return false;
168 }
169 
170 double CMetricMap::computeObservationLikelihood(
171  const mrpt::obs::CObservation& obs, const mrpt::poses::CPose3D& takenFrom)
172 {
173  if (genericMapParams.enableObservationLikelihood)
174  return internal_computeObservationLikelihood(obs, takenFrom);
175  else
176  return false;
177 }
mrpt::obs::CObservation::Ptr
std::shared_ptr< CObservation > Ptr
Definition: CObservation.h:45
mrpt::containers::clear
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183
mrpt::maps::mrptEventMetricMapClear
Event emitted by a metric up upon call of clear()
Definition: CMetricMapEvents.h:28
mrpt::obs::CSensoryFrame::end
const_iterator end() const
Returns a constant iterator to the end of the list of observations: this is an example of usage:
Definition: CSensoryFrame.h:248
mrpt::obs::CSensoryFrame::Ptr
std::shared_ptr< mrpt::obs ::CSensoryFrame > Ptr
Definition: CSensoryFrame.h:53
mrpt::maps::TMatchingRatioParams
Parameters for CMetricMap::compute3DMatchingRatio()
Definition: metric_map_types.h:64
obs-precomp.h
mrpt::tfest
Functions for estimating the optimal transformation between two frames of references given measuremen...
Definition: indiv-compat-decls.h:14
IMPLEMENTS_VIRTUAL_SERIALIZABLE
#define IMPLEMENTS_VIRTUAL_SERIALIZABLE(class_name, base_class, NS)
This must be inserted as implementation of some required members for virtual CSerializable classes:
Definition: CSerializable.h:177
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
CMetricMap.h
mrpt::obs::CSensoryFrame
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:51
mrpt::maps::CMetricMap
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
mrpt::obs::CSensoryFrame::begin
const_iterator begin() const
Returns a constant iterator to the first observation: this is an example of usage:
Definition: CSensoryFrame.h:235
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
CPosePDF.h
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::maps::CSimpleMap::size
size_t size() const
Returns the count of pairs (pose,sensory data)
Definition: CSimpleMap.cpp:53
mrpt::maps::CSimpleMap::get
void get(size_t index, mrpt::poses::CPose3DPDF::Ptr &out_posePDF, mrpt::obs::CSensoryFrame::Ptr &out_SF) const
Access to the i'th pair, first one is index '0'.
Definition: CSimpleMap.cpp:56
params
mrpt::vision::TStereoCalibParams params
Definition: chessboard_stereo_camera_calib_unittest.cpp:24
mrpt::maps::TMatchingParams
Parameters for the determination of matchings between point clouds, etc.
Definition: metric_map_types.h:20
ASSERTMSG_
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
CPoint3D.h
CPose3D.h
CSensoryFrame.h
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::maps::CSimpleMap
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:32
mrpt::obs::CObservation
Declares a class that represents any robot's observation.
Definition: CObservation.h:43
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::poses::CPose3DPDF::Ptr
std::shared_ptr< CPose3DPDF > Ptr
Definition: CPose3DPDF.h:42
mrpt::tfest::TMatchingPairList
A list of TMatchingPair.
Definition: TMatchingPair.h:70
mrpt::maps
Definition: CBeacon.h:21
mrpt::maps::TMatchingExtraResults
Additional results from the determination of matchings between point clouds, etc.,...
Definition: metric_map_types.h:51
CArchive.h
CSimpleMap.h
mrpt::maps::mrptEventMetricMapInsert
Event emitted by a metric up upon a succesful call to insertObservation()
Definition: CMetricMapEvents.h:47



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sat Jun 27 14:00:59 UTC 2020