MRPT  2.0.1
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 }
#define IMPLEMENTS_VIRTUAL_SERIALIZABLE(class_name, base_class, NS)
This must be inserted as implementation of some required members for virtual CSerializable classes: ...
Parameters for CMetricMap::compute3DMatchingRatio()
#define MRPT_START
Definition: exceptions.h:241
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:32
mrpt::vision::TStereoCalibParams params
const_iterator begin() const
Returns a constant iterator to the first observation: this is an example of usage: ...
This base provides a set of functions for maths stuff.
Additional results from the determination of matchings between point clouds, etc., apart from the pairings themselves.
A list of TMatchingPair.
Definition: TMatchingPair.h:70
This namespace contains representation of robot actions and observations.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:51
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
size_t size() const
Returns the count of pairs (pose,sensory data)
Definition: CSimpleMap.cpp:53
void get(size_t index, mrpt::poses::CPose3DPDF::Ptr &out_posePDF, mrpt::obs::CSensoryFrame::Ptr &out_SF) const
Access to the i&#39;th pair, first one is index &#39;0&#39;.
Definition: CSimpleMap.cpp:56
Event emitted by a metric up upon a succesful call to insertObservation()
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Event emitted by a metric up upon call of clear()
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
#define MRPT_END
Definition: exceptions.h:245
const_iterator end() const
Returns a constant iterator to the end of the list of observations: this is an example of usage: ...
Parameters for the determination of matchings between point clouds, etc.
Functions for estimating the optimal transformation between two frames of references given measuremen...
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020