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
mrpt::vision::TStereoCalibParams params
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...
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: 24b95e159 Thu Jan 23 01:15:46 2020 +0100 at jue ene 23 01:30:10 CET 2020