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>
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.get(), 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->get());
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.get(), robotPose);
116  MRPT_END
117 }
118 
119 bool CMetricMap::canComputeObservationLikelihood(
120  const CObservation::Ptr& obs) const
121 {
122  return canComputeObservationLikelihood(obs.get());
123 }
124 
125 void CMetricMap::determineMatching2D(
126  const mrpt::maps::CMetricMap* otherMap, const CPose2D& otherMapPose,
127  TMatchingPairList& correspondences, const TMatchingParams& params,
128  TMatchingExtraResults& extraResults) const
129 {
130  MRPT_UNUSED_PARAM(otherMap);
131  MRPT_UNUSED_PARAM(otherMapPose);
132  MRPT_UNUSED_PARAM(correspondences);
134  MRPT_UNUSED_PARAM(extraResults);
135  MRPT_START
136  THROW_EXCEPTION("Virtual method not implemented in derived class.");
137  MRPT_END
138 }
139 
140 void CMetricMap::determineMatching3D(
141  const mrpt::maps::CMetricMap* otherMap, const CPose3D& otherMapPose,
142  TMatchingPairList& correspondences, const TMatchingParams& params,
143  TMatchingExtraResults& extraResults) const
144 {
145  MRPT_UNUSED_PARAM(otherMap);
146  MRPT_UNUSED_PARAM(otherMapPose);
147  MRPT_UNUSED_PARAM(correspondences);
149  MRPT_UNUSED_PARAM(extraResults);
150  MRPT_START
151  THROW_EXCEPTION("Virtual method not implemented in derived class.");
152  MRPT_END
153 }
154 
155 float CMetricMap::compute3DMatchingRatio(
156  const mrpt::maps::CMetricMap* otherMap,
157  const mrpt::poses::CPose3D& otherMapPose,
158  const TMatchingRatioParams& params) const
159 {
160  MRPT_UNUSED_PARAM(otherMap);
161  MRPT_UNUSED_PARAM(otherMapPose);
163  MRPT_START
164  THROW_EXCEPTION("Virtual method not implemented in derived class.");
165  MRPT_END
166 }
167 
168 float CMetricMap::squareDistanceToClosestCorrespondence(
169  float x0, float y0) const
170 {
171  MRPT_UNUSED_PARAM(x0);
172  MRPT_UNUSED_PARAM(y0);
173  MRPT_START
174  THROW_EXCEPTION("Virtual method not implemented in derived class.");
175  MRPT_END
176 }
177 
178 bool CMetricMap::canComputeObservationLikelihood(
179  const mrpt::obs::CObservation* obs) const
180 {
181  if (genericMapParams.enableObservationLikelihood)
182  return internal_canComputeObservationLikelihood(obs);
183  else
184  return false;
185 }
186 
187 double CMetricMap::computeObservationLikelihood(
188  const mrpt::obs::CObservation* obs, const mrpt::poses::CPose3D& takenFrom)
189 {
190  if (genericMapParams.enableObservationLikelihood)
191  return internal_computeObservationLikelihood(obs, takenFrom);
192  else
193  return false;
194 }
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
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: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:84
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:42
#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...
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: 363e65e4a Wed Jun 19 01:52:47 2019 +0200 at miƩ jun 19 02:00:17 CEST 2019