Main MRPT website > C++ reference for MRPT 1.9.9
CMetricMap.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "obs-precomp.h" // Precompiled headers
11 
13 #include <mrpt/maps/CMetricMap.h>
14 #include <mrpt/poses/CPosePDF.h>
15 #include <mrpt/poses/CPoint3D.h>
16 #include <mrpt/poses/CPose3D.h>
17 #include <mrpt/obs/CSensoryFrame.h>
18 #include <mrpt/maps/CSimpleMap.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 
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 (CSensoryFrame::const_iterator it = sf.begin(); it != sf.end(); ++it)
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 (CSensoryFrame::const_iterator it = sf.begin(); !can && it != sf.end();
90  ++it)
91  can = can || canComputeObservationLikelihood(it->get());
92  return can;
93 }
94 
95 bool CMetricMap::insertObservation(
96  const CObservation* obs, const CPose3D* robotPose)
97 {
98  if (!genericMapParams.enableObservationInsertion) return false;
99 
100  bool done = internal_insertObservation(obs, robotPose);
101  if (done)
102  {
103  OnPostSuccesfulInsertObs(obs);
104  publishEvent(mrptEventMetricMapInsert(this, obs, robotPose));
105  }
106  return done;
107 }
108 
109 bool CMetricMap::insertObservationPtr(
110  const CObservation::Ptr& obs, const CPose3D* robotPose)
111 {
112  MRPT_START
113  if (!obs)
114  {
115  THROW_EXCEPTION("Trying to pass a null pointer.");
116  }
117  return insertObservation(obs.get(), robotPose);
118  MRPT_END
119 }
120 
121 bool CMetricMap::canComputeObservationLikelihood(
122  const CObservation::Ptr& obs) const
123 {
124  return canComputeObservationLikelihood(obs.get());
125 }
126 
127 void CMetricMap::determineMatching2D(
128  const mrpt::maps::CMetricMap* otherMap, const CPose2D& otherMapPose,
129  TMatchingPairList& correspondences, const TMatchingParams& params,
130  TMatchingExtraResults& extraResults) const
131 {
132  MRPT_UNUSED_PARAM(otherMap);
133  MRPT_UNUSED_PARAM(otherMapPose);
134  MRPT_UNUSED_PARAM(correspondences);
136  MRPT_UNUSED_PARAM(extraResults);
137  MRPT_START
138  THROW_EXCEPTION("Virtual method not implemented in derived class.");
139  MRPT_END
140 }
141 
142 void CMetricMap::determineMatching3D(
143  const mrpt::maps::CMetricMap* otherMap, const CPose3D& otherMapPose,
144  TMatchingPairList& correspondences, const TMatchingParams& params,
145  TMatchingExtraResults& extraResults) const
146 {
147  MRPT_UNUSED_PARAM(otherMap);
148  MRPT_UNUSED_PARAM(otherMapPose);
149  MRPT_UNUSED_PARAM(correspondences);
151  MRPT_UNUSED_PARAM(extraResults);
152  MRPT_START
153  THROW_EXCEPTION("Virtual method not implemented in derived class.");
154  MRPT_END
155 }
156 
157 float CMetricMap::compute3DMatchingRatio(
158  const mrpt::maps::CMetricMap* otherMap,
159  const mrpt::poses::CPose3D& otherMapPose,
160  const TMatchingRatioParams& params) const
161 {
162  MRPT_UNUSED_PARAM(otherMap);
163  MRPT_UNUSED_PARAM(otherMapPose);
165  MRPT_START
166  THROW_EXCEPTION("Virtual method not implemented in derived class.");
167  MRPT_END
168 }
169 
170 float CMetricMap::squareDistanceToClosestCorrespondence(
171  float x0, float y0) const
172 {
173  MRPT_UNUSED_PARAM(x0);
174  MRPT_UNUSED_PARAM(y0);
175  MRPT_START
176  THROW_EXCEPTION("Virtual method not implemented in derived class.");
177  MRPT_END
178 }
179 
180 bool CMetricMap::canComputeObservationLikelihood(
181  const mrpt::obs::CObservation* obs) const
182 {
183  if (genericMapParams.enableObservationLikelihood)
184  return internal_canComputeObservationLikelihood(obs);
185  else
186  return false;
187 }
188 
189 double CMetricMap::computeObservationLikelihood(
190  const mrpt::obs::CObservation* obs, const mrpt::poses::CPose3D& takenFrom)
191 {
192  if (genericMapParams.enableObservationLikelihood)
193  return internal_computeObservationLikelihood(obs, takenFrom);
194  else
195  return false;
196 }
Parameters for CMetricMap::compute3DMatchingRatio()
#define MRPT_START
Definition: exceptions.h:262
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:35
GLenum GLsizei n
Definition: glext.h:5074
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:83
std::deque< CObservation::Ptr >::const_iterator const_iterator
You can use CSensoryFrame::begin to get a iterator to the first element.
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:54
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
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:56
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
#define MRPT_END
Definition: exceptions.h:266
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:3534
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:188
#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: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019