MRPT  1.9.9
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-2019, 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>
15 #include <mrpt/obs/CSensoryFrame.h>
16 #include <mrpt/poses/CPoint3D.h>
17 #include <mrpt/poses/CPose3D.h>
18 #include <mrpt/poses/CPosePDF.h>
20 
21 using namespace mrpt::obs;
22 using namespace mrpt::maps;
23 using namespace mrpt::poses;
24 using namespace mrpt::math;
25 using namespace mrpt::tfest;
26 
28 
29 CMetricMap::CMetricMap() = default;
30 /** Erase all the contents of the map */
32 {
33  internal_clear();
34  publishEvent(mrptEventMetricMapClear(this));
35 }
36 
37 void CMetricMap::loadFromProbabilisticPosesAndObservations(
38  const mrpt::maps::CSimpleMap& sfSeq)
39 {
40  CPose3DPDF::Ptr posePDF;
42  const size_t n = sfSeq.size();
43 
44  // Erase previous contents:
45  this->clear();
46 
47  // Insert new content:
48  for (size_t i = 0; i < n; i++)
49  {
50  sfSeq.get(i, posePDF, sf);
51  ASSERTMSG_(posePDF, "Input map has an empty `CPose3DPDF` ptr");
52  ASSERTMSG_(sf, "Input map has an empty `CSensoryFrame` ptr");
53 
54  CPose3D robotPose;
55  posePDF->getMean(robotPose);
56 
57  sf->insertObservationsInto(
58  this, // Insert into THIS map.
59  &robotPose // At this pose.
60  );
61  }
62 }
63 
64 /*---------------------------------------------------------------
65  computeObservationsLikelihood
66  ---------------------------------------------------------------*/
67 double CMetricMap::computeObservationsLikelihood(
68  const CSensoryFrame& sf, const CPose2D& takenFrom)
69 {
70  double lik = 0;
71  for (const auto& it : sf)
72  lik += computeObservationLikelihood(it.get(), takenFrom);
73 
74  return lik;
75 }
76 
77 double CMetricMap::computeObservationLikelihood(
78  const CObservation* obs, const CPose2D& takenFrom)
79 {
80  return computeObservationLikelihood(obs, CPose3D(takenFrom));
81 }
82 
83 /*---------------------------------------------------------------
84  canComputeObservationLikelihood
85  ---------------------------------------------------------------*/
86 bool CMetricMap::canComputeObservationsLikelihood(const CSensoryFrame& sf) const
87 {
88  bool can = false;
89  for (auto it = sf.begin(); !can && it != sf.end(); ++it)
90  can = can || canComputeObservationLikelihood(it->get());
91  return can;
92 }
93 
94 bool CMetricMap::insertObservation(
95  const CObservation* obs, const CPose3D* robotPose)
96 {
97  if (!genericMapParams.enableObservationInsertion) return false;
98 
99  bool done = internal_insertObservation(obs, robotPose);
100  if (done)
101  {
102  OnPostSuccesfulInsertObs(obs);
103  publishEvent(mrptEventMetricMapInsert(this, obs, robotPose));
104  }
105  return done;
106 }
107 
108 bool CMetricMap::insertObservationPtr(
109  const CObservation::Ptr& obs, const CPose3D* robotPose)
110 {
111  MRPT_START
112  if (!obs)
113  {
114  THROW_EXCEPTION("Trying to pass a null pointer.");
115  }
116  return insertObservation(obs.get(), robotPose);
117  MRPT_END
118 }
119 
120 bool CMetricMap::canComputeObservationLikelihood(
121  const CObservation::Ptr& obs) const
122 {
123  return canComputeObservationLikelihood(obs.get());
124 }
125 
126 void CMetricMap::determineMatching2D(
127  const mrpt::maps::CMetricMap* otherMap, const CPose2D& otherMapPose,
128  TMatchingPairList& correspondences, const TMatchingParams& params,
129  TMatchingExtraResults& extraResults) const
130 {
131  MRPT_UNUSED_PARAM(otherMap);
132  MRPT_UNUSED_PARAM(otherMapPose);
133  MRPT_UNUSED_PARAM(correspondences);
135  MRPT_UNUSED_PARAM(extraResults);
136  MRPT_START
137  THROW_EXCEPTION("Virtual method not implemented in derived class.");
138  MRPT_END
139 }
140 
141 void CMetricMap::determineMatching3D(
142  const mrpt::maps::CMetricMap* otherMap, const CPose3D& otherMapPose,
143  TMatchingPairList& correspondences, const TMatchingParams& params,
144  TMatchingExtraResults& extraResults) const
145 {
146  MRPT_UNUSED_PARAM(otherMap);
147  MRPT_UNUSED_PARAM(otherMapPose);
148  MRPT_UNUSED_PARAM(correspondences);
150  MRPT_UNUSED_PARAM(extraResults);
151  MRPT_START
152  THROW_EXCEPTION("Virtual method not implemented in derived class.");
153  MRPT_END
154 }
155 
156 float CMetricMap::compute3DMatchingRatio(
157  const mrpt::maps::CMetricMap* otherMap,
158  const mrpt::poses::CPose3D& otherMapPose,
159  const TMatchingRatioParams& params) const
160 {
161  MRPT_UNUSED_PARAM(otherMap);
162  MRPT_UNUSED_PARAM(otherMapPose);
164  MRPT_START
165  THROW_EXCEPTION("Virtual method not implemented in derived class.");
166  MRPT_END
167 }
168 
169 float CMetricMap::squareDistanceToClosestCorrespondence(
170  float x0, float y0) const
171 {
172  MRPT_UNUSED_PARAM(x0);
173  MRPT_UNUSED_PARAM(y0);
174  MRPT_START
175  THROW_EXCEPTION("Virtual method not implemented in derived class.");
176  MRPT_END
177 }
178 
179 bool CMetricMap::canComputeObservationLikelihood(
180  const mrpt::obs::CObservation* obs) const
181 {
182  if (genericMapParams.enableObservationLikelihood)
183  return internal_canComputeObservationLikelihood(obs);
184  else
185  return false;
186 }
187 
188 double CMetricMap::computeObservationLikelihood(
189  const mrpt::obs::CObservation* obs, const mrpt::poses::CPose3D& takenFrom)
190 {
191  if (genericMapParams.enableObservationLikelihood)
192  return internal_computeObservationLikelihood(obs, takenFrom);
193  else
194  return false;
195 }
Parameters for CMetricMap::compute3DMatchingRatio()
#define MRPT_START
Definition: exceptions.h:282
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:108
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:32
GLenum GLsizei n
Definition: glext.h:5136
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:149
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:53
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:38
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:82
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:42
#define MRPT_END
Definition: exceptions.h:286
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...
GLenum const GLfloat * params
Definition: glext.h:3538
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:182
#define IMPLEMENTS_VIRTUAL_SERIALIZABLE( class_name, base_class_name, NameSpace)
This must be inserted as implementation of some required members for virtual CSerializable classes: ...
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 5887d2b31 Wed Apr 24 13:03:27 2019 +0200 at miƩ abr 24 13:10:13 CEST 2019