Main MRPT website > C++ reference for MRPT 1.5.6
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);
142  MRPT_UNUSED_PARAM(params);
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);
160  MRPT_UNUSED_PARAM(params);
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);
172  MRPT_UNUSED_PARAM(params);
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()
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)
void get(size_t index, mrpt::poses::CPose3DPDFPtr &out_posePDF, mrpt::obs::CSensoryFramePtr &out_SF) const
Access to the i'th pair, first one is index '0'.
Definition: CSimpleMap.cpp:95
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.
size_t size() const
Returns the count of pairs (pose,sensory data)
Definition: CSimpleMap.cpp:67
#define IMPLEMENTS_VIRTUAL_SERIALIZABLE(class_name, base_class_name, NameSpace)
This must be inserted as implementation of some required members for virtual CSerializable classes: ...
GLsizei n
Definition: glew.h:5051
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.
const_iterator end() const
Returns a constant iterator to the end of the list of observations: this is an example of usage: ...
A list of TMatchingPair.
Definition: TMatchingPair.h:78
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Event emitted by a metric up upon a succesful call to insertObservation()
const_iterator begin() const
Returns a constant iterator to the first observation: this is an example of usage: ...
Event emitted by a metric up upon call of clear()
#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's observation.
GLfloat * params
Definition: glew.h:1436
Parameters for the determination of matchings between point clouds, etc.



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018