Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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 
12 #include <mrpt/utils/CStream.h>
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::utils;
24 using namespace mrpt::poses;
25 using namespace mrpt::math;
26 
28 
30 {
31 }
32 
33 /** Erase all the contents of the map */
35 {
36  internal_clear();
37  publishEvent( mrptEventMetricMapClear(this) );
38 }
39 
40 
41 /*---------------------------------------------------------------
42 Load the map contents from a CSensFrameProbSequence object,
43 erasing all previous content of the map.
44 This is automaticed invoking "insertObservation" for each
45 observation at the mean 3D robot pose as given by
46 the "poses::CPosePDF" in the CSensFrameProbSequence object.
47  ---------------------------------------------------------------*/
48 void CMetricMap::loadFromProbabilisticPosesAndObservations(const mrpt::maps::CSimpleMap &sfSeq )
49 {
50  CPose3DPDFPtr posePDF;
51  CSensoryFramePtr sf;
52  const size_t n = sfSeq.size();
53 
54  // Erase previous contents:
55  this->clear();
56 
57  // Insert new content:
58  for (size_t i=0;i<n;i++)
59  {
60  sfSeq.get(i,posePDF, sf);
61 
62  CPose3D robotPose;
63  posePDF->getMean(robotPose);
64 
65  sf->insertObservationsInto(
66  this, // Insert into THIS map.
67  &robotPose // At this pose.
68  );
69  }
70 }
71 
72 /*---------------------------------------------------------------
73  computeObservationsLikelihood
74  ---------------------------------------------------------------*/
75 double CMetricMap::computeObservationsLikelihood(
76  const CSensoryFrame &sf,
77  const CPose2D &takenFrom )
78 {
79  double lik = 0;
80  for (CSensoryFrame::const_iterator it=sf.begin();it!=sf.end();++it)
81  lik += computeObservationLikelihood( it->pointer(), takenFrom );
82 
83  return lik;
84 }
85 
86 double CMetricMap::computeObservationLikelihood( const CObservation *obs, const CPose2D &takenFrom )
87 {
88  return computeObservationLikelihood(obs,CPose3D(takenFrom));
89 }
90 
91 /*---------------------------------------------------------------
92  canComputeObservationLikelihood
93  ---------------------------------------------------------------*/
94 bool CMetricMap::canComputeObservationsLikelihood( const CSensoryFrame &sf ) const
95 {
96  bool can = false;
97  for (CSensoryFrame::const_iterator it=sf.begin();!can && it!=sf.end();++it)
98  can = can || canComputeObservationLikelihood( it->pointer() );
99  return can;
100 }
101 
102 bool CMetricMap::insertObservation(
103  const CObservation *obs,
104  const CPose3D *robotPose)
105 {
106  if (!genericMapParams.enableObservationInsertion)
107  return false;
108 
109  bool done = internal_insertObservation(obs,robotPose);
110  if (done)
111  {
112  OnPostSuccesfulInsertObs(obs);
113  publishEvent( mrptEventMetricMapInsert(this,obs,robotPose) );
114  }
115  return done;
116 }
117 
118 bool CMetricMap::insertObservationPtr(
119  const CObservationPtr &obs,
120  const CPose3D *robotPose)
121 {
122  MRPT_START
123  if (!obs.present()) { THROW_EXCEPTION("Trying to pass a null pointer."); }
124  return insertObservation(obs.pointer(),robotPose);
125  MRPT_END
126 }
127 
128 bool CMetricMap::canComputeObservationLikelihood( const CObservationPtr &obs ) const {
129  return canComputeObservationLikelihood(obs.pointer());
130 }
131 
132 void CMetricMap::determineMatching2D(
133  const mrpt::maps::CMetricMap * otherMap,
134  const CPose2D & otherMapPose,
135  TMatchingPairList & correspondences,
136  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 
150 void CMetricMap::determineMatching3D(
151  const mrpt::maps::CMetricMap * otherMap,
152  const CPose3D & otherMapPose,
153  TMatchingPairList & correspondences,
154  const TMatchingParams & params,
155  TMatchingExtraResults & extraResults ) const
156 {
157  MRPT_UNUSED_PARAM(otherMap);
158  MRPT_UNUSED_PARAM(otherMapPose);
159  MRPT_UNUSED_PARAM(correspondences);
161  MRPT_UNUSED_PARAM(extraResults);
162  MRPT_START
163  THROW_EXCEPTION("Virtual method not implemented in derived class.")
164  MRPT_END
165 }
166 
167 
168 float CMetricMap::compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const
169 {
170  MRPT_UNUSED_PARAM(otherMap);
171  MRPT_UNUSED_PARAM(otherMapPose);
173  MRPT_START
174  THROW_EXCEPTION("Virtual method not implemented in derived class.")
175  MRPT_END
176 }
177 
178 float CMetricMap::squareDistanceToClosestCorrespondence(
179  float x0,
180  float y0 ) const
181 {
182  MRPT_UNUSED_PARAM(x0);
183  MRPT_UNUSED_PARAM(y0);
184  MRPT_START
185  THROW_EXCEPTION("Virtual method not implemented in derived class.")
186  MRPT_END
187 }
188 
189 bool CMetricMap::canComputeObservationLikelihood( const mrpt::obs::CObservation *obs ) const
190 {
191  if (genericMapParams.enableObservationLikelihood)
192  return internal_canComputeObservationLikelihood(obs);
193  else return false;
194 }
195 
196 double CMetricMap::computeObservationLikelihood( const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom )
197 {
198  if (genericMapParams.enableObservationLikelihood)
199  return internal_computeObservationLikelihood(obs,takenFrom);
200  else return false;
201 }
Parameters for CMetricMap::compute3DMatchingRatio()
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:39
#define THROW_EXCEPTION(msg)
GLenum GLsizei n
Definition: glext.h:4618
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
std::deque< CObservationPtr >::const_iterator const_iterator
You can use CSensoryFrame::begin to get a iterator to the first element.
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.
Definition: CArrayNumeric.h:19
#define IMPLEMENTS_VIRTUAL_SERIALIZABLE(class_name, base_class_name, NameSpace)
This must be inserted as implementation of some required members for virtual CSerializable classes: ...
Additional results from the determination of matchings between point clouds, etc., apart from the pairings themselves.
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
A list of TMatchingPair.
Definition: TMatchingPair.h:78
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...
size_t size() const
Returns the count of pairs (pose,sensory data)
Definition: CSimpleMap.cpp:67
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...
Definition: CPoint.h:17
Event emitted by a metric up upon call of clear()
void get(size_t index, mrpt::poses::CPose3DPDFPtr &out_posePDF, mrpt::obs::CSensoryFramePtr &out_SF) const
Access to the i&#39;th pair, first one is index &#39;0&#39;.
Definition: CSimpleMap.cpp:95
#define MRPT_START
Declares a virtual base class for all metric maps storage classes.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
Declares a class that represents any robot&#39;s observation.
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.
GLenum const GLfloat * params
Definition: glext.h:3514



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019