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, 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  const mrpt::maps::CMetricMap* otherMap, const CPose2D& otherMapPose,
121  TMatchingPairList& correspondences, const TMatchingParams& params,
122  TMatchingExtraResults& extraResults) const
123 {
124  MRPT_UNUSED_PARAM(otherMap);
125  MRPT_UNUSED_PARAM(otherMapPose);
126  MRPT_UNUSED_PARAM(correspondences);
128  MRPT_UNUSED_PARAM(extraResults);
129  MRPT_START
130  THROW_EXCEPTION("Virtual method not implemented in derived class.");
131  MRPT_END
132 }
133 
134 void CMetricMap::determineMatching3D(
135  const mrpt::maps::CMetricMap* otherMap, const CPose3D& otherMapPose,
136  TMatchingPairList& correspondences, const TMatchingParams& params,
137  TMatchingExtraResults& extraResults) const
138 {
139  MRPT_UNUSED_PARAM(otherMap);
140  MRPT_UNUSED_PARAM(otherMapPose);
141  MRPT_UNUSED_PARAM(correspondences);
143  MRPT_UNUSED_PARAM(extraResults);
144  MRPT_START
145  THROW_EXCEPTION("Virtual method not implemented in derived class.");
146  MRPT_END
147 }
148 
149 float CMetricMap::compute3DMatchingRatio(
150  const mrpt::maps::CMetricMap* otherMap,
151  const mrpt::poses::CPose3D& otherMapPose,
152  const TMatchingRatioParams& params) const
153 {
154  MRPT_UNUSED_PARAM(otherMap);
155  MRPT_UNUSED_PARAM(otherMapPose);
157  MRPT_START
158  THROW_EXCEPTION("Virtual method not implemented in derived class.");
159  MRPT_END
160 }
161 
162 float CMetricMap::squareDistanceToClosestCorrespondence(
163  float x0, float y0) const
164 {
165  MRPT_UNUSED_PARAM(x0);
166  MRPT_UNUSED_PARAM(y0);
167  MRPT_START
168  THROW_EXCEPTION("Virtual method not implemented in derived class.");
169  MRPT_END
170 }
171 
172 bool CMetricMap::canComputeObservationLikelihood(
173  const mrpt::obs::CObservation& obs) const
174 {
175  if (genericMapParams.enableObservationLikelihood)
176  return internal_canComputeObservationLikelihood(obs);
177  else
178  return false;
179 }
180 
181 double CMetricMap::computeObservationLikelihood(
182  const mrpt::obs::CObservation& obs, const mrpt::poses::CPose3D& takenFrom)
183 {
184  if (genericMapParams.enableObservationLikelihood)
185  return internal_computeObservationLikelihood(obs, takenFrom);
186  else
187  return false;
188 }
#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
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: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...
GLenum const GLfloat * params
Definition: glext.h:3538
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183
#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: 45d659fbb Tue Dec 10 18:21:14 2019 +0100 at mar dic 10 18:30:09 CET 2019