MRPT  1.9.9
CMultiMetricMap.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 "maps-precomp.h" // Precompiled headers
11 
15 #include <mrpt/poses/CPoint2D.h>
18 #include <mrpt/system/filesystem.h>
19 
20 using namespace mrpt::maps;
21 using namespace mrpt::poses;
22 using namespace mrpt::obs;
23 using namespace mrpt::tfest;
25 
27 
28 // ------------------------------------------------------------------------
29 // A few words explaining how all this works:
30 // The main hub for operating with all the maps in the internal list
31 // if MapExecutor.
32 //
33 // All operations go thru MapExecutor::run<OP>() with OP being one of the
34 // possible map operations (clear, matching, likelihood, etc.). The
35 // idea is that when adding new map types to the internal list of
36 // CMultiMetricMap, *only* "MapExecutor" methods must be updated.
37 // (The only exception are readFromStream() & writeToStream())
38 //
39 // The map-specific operations all go into template specializations of
40 // other helper structures or in overloaded methods.
41 // JLBC (7-AUG-2011)
42 // ------------------------------------------------------------------------
43 
45 {
46  // Apply operation to maps in the same order as declared in
47  // CMultiMetricMap.h:
48  template <typename OP>
49  static void run(const CMultiMetricMap& _mmm, OP op)
50  {
52  // This is to avoid duplicating "::run()" for const and non-const.
53  auto& mmm = const_cast<CMultiMetricMap&>(_mmm);
54  std::for_each(mmm.maps.begin(), mmm.maps.end(), op);
55  MRPT_END
56  }
57 }; // end of MapExecutor
58 
59 // ------------------- Begin of map-operations helper templates
60 
62 {
63  const CObservation* obs;
64  bool& can;
65 
67  const CMultiMetricMap& m, const CObservation* _obs, bool& _can)
68  : obs(_obs), can(_can)
69  {
70  can = false;
71  }
72 
73  template <typename PTR>
74  inline void operator()(PTR& ptr)
75  {
76  can = can || ptr->canComputeObservationLikelihood(obs);
77  }
78 
79 }; // end of MapCanComputeLikelihood
80 
82 {
83  MapAuxPFCleanup() = default;
84  template <typename PTR>
85  inline void operator()(PTR& ptr)
86  {
87  if (ptr) ptr->auxParticleFilterCleanUp();
88  }
89 }; // end of MapAuxPFCleanup
90 
91 struct MapIsEmpty
92 {
93  bool& is_empty;
94 
95  MapIsEmpty(bool& _is_empty) : is_empty(_is_empty) { is_empty = true; }
96  template <typename PTR>
97  inline void operator()(PTR& ptr)
98  {
99  if (ptr) is_empty = is_empty && ptr->isEmpty();
100  }
101 }; // end of MapIsEmpty
102 
103 // ---- End of map-operations helper templates
104 
105 // Ctor
107 {
108  MRPT_START
109  setListOfMaps(i);
110  MRPT_END
111 }
112 
114 {
115  MRPT_START
116  // Erase current list of maps:
117  maps.clear();
118 
120 
121  // Process each entry in the "initializers" and create maps accordingly:
122  for (const auto& i : inits)
123  {
124  // Create map from the list of all params:
125  auto* theMap = mmr.factoryMapObjectFromDefinition(*i.get());
126  ASSERT_(theMap);
127  // Add to the list of maps:
128  this->maps.push_back(mrpt::maps::CMetricMap::Ptr(theMap));
129  }
130  MRPT_END
131 }
132 
134 {
135  std::for_each(maps.begin(), maps.end(), [](auto ptr) {
136  if (ptr) ptr->clear();
137  });
138 }
139 
140 uint8_t CMultiMetricMap::serializeGetVersion() const { return 12; }
142 {
143  const auto n = static_cast<uint32_t>(maps.size());
144  out << n;
145  for (uint32_t i = 0; i < n; i++) out << *maps[i];
146 }
147 
149  mrpt::serialization::CArchive& in, uint8_t version)
150 {
151  switch (version)
152  {
153  case 11:
154  case 12:
155  {
156  // ID:
157  if (version < 12) // ID was removed in v12
158  {
159  uint32_t ID;
160  in >> ID;
161  }
162 
163  // List of maps:
164  uint32_t n;
165  in >> n;
166  this->maps.resize(n);
167  for_each(
168  maps.begin(), maps.end(),
170  }
171  break;
172  default:
174  };
175 }
176 
177 // Read docs in base class
179  const CObservation& obs, const CPose3D& takenFrom)
180 {
181  MRPT_START
182  double ret_log_lik = 0;
183 
184  std::for_each(maps.begin(), maps.end(), [&](auto& ptr) {
185  ret_log_lik += ptr->computeObservationLikelihood(obs, takenFrom);
186  });
187  return ret_log_lik;
188 
189  MRPT_END
190 }
191 
192 // Read docs in base class
194  const CObservation& obs) const
195 {
196  bool can_comp = false;
197  std::for_each(maps.begin(), maps.end(), [&](auto& ptr) {
198  can_comp = can_comp || ptr->canComputeObservationLikelihood(obs);
199  });
200  return can_comp; //-V614
201 }
202 
204  const CObservation& obs, const CPose3D* robotPose)
205 {
206  int total_insert = 0;
207 
208  std::for_each(maps.begin(), maps.end(), [&](auto& ptr) {
209  const bool ret = ptr->insertObservation(obs, robotPose);
210  if (ret) total_insert++;
211  });
212  return total_insert != 0;
213 }
214 
216  const mrpt::maps::CMetricMap* otherMap, const CPose2D& otherMapPose,
217  TMatchingPairList& correspondences, const TMatchingParams& params,
218  TMatchingExtraResults& extraResults) const
219 {
220  MRPT_START
221  const auto numPointsMaps = countMapsByClass<CSimplePointsMap>();
222 
223  ASSERTMSG_(
224  numPointsMaps == 1,
225  "There is not exactly 1 points maps in the multimetric map.");
226  mapByClass<CSimplePointsMap>()->determineMatching2D(
227  otherMap, otherMapPose, correspondences, params, extraResults);
228  MRPT_END
229 }
230 
232 {
233  bool is_empty;
234  MapIsEmpty op_insert_obs(is_empty); //-V614
235  MapExecutor::run(*this, op_insert_obs);
236  return is_empty;
237 }
238 
240  const std::string& filNamePrefix) const
241 {
242  MRPT_START
243 
244  for (size_t idx = 0; idx < maps.size(); idx++)
245  {
246  const mrpt::maps::CMetricMap* m = maps[idx].get();
247  ASSERT_(m);
248  std::string fil = filNamePrefix;
249  fil += format(
250  "_%s_%02u",
253  .c_str(),
254  static_cast<unsigned int>(idx));
256  }
257 
258  MRPT_END
259 }
260 
262  mrpt::opengl::CSetOfObjects::Ptr& outObj) const
263 {
264  MRPT_START
265  std::for_each(maps.begin(), maps.end(), [&](auto& ptr) {
266  ptr->getAs3DObject(outObj);
267  });
268  MRPT_END
269 }
270 
271 // See docs in base class
273  const mrpt::maps::CMetricMap* otherMap,
274  const mrpt::poses::CPose3D& otherMapPose,
275  const TMatchingRatioParams& params) const
276 {
277  MRPT_START
278 
279  float accumResult = 0;
280 
281  for (const auto& map : maps)
282  {
283  const mrpt::maps::CMetricMap* m = map.get();
284  ASSERT_(m);
285  accumResult +=
286  m->compute3DMatchingRatio(otherMap, otherMapPose, params);
287  }
288 
289  // Return average:
290  const size_t nMapsComputed = maps.size();
291  if (nMapsComputed) accumResult /= nMapsComputed;
292  return accumResult;
293 
294  MRPT_END
295 }
296 
298 {
299  MRPT_START
300  std::for_each(maps.begin(), maps.end(), [](auto& ptr) {
301  ptr->auxParticleFilterCleanUp();
302  });
303  MRPT_END
304 }
305 
307 {
308  MRPT_START
309  const auto numPointsMaps = countMapsByClass<CSimplePointsMap>();
310  ASSERT_(numPointsMaps == 1 || numPointsMaps == 0);
311  if (!numPointsMaps)
312  return nullptr;
313  else
314  return this->mapByClass<CSimplePointsMap>(0).get();
315  MRPT_END
316 }
317 
319 {
320  MRPT_START
321  return maps.at(idx).get_ptr();
322  MRPT_END
323 }
virtual void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const =0
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
void auxParticleFilterCleanUp() override
This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::...
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
Parameters for CMetricMap::compute3DMatchingRatio()
void operator()(PTR &ptr)
#define MRPT_START
Definition: exceptions.h:241
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
bool internal_insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr) override
Internal method called by insertObservation()
GLenum GLsizei n
Definition: glext.h:5136
void operator()(PTR &ptr)
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
void determineMatching2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, mrpt::tfest::TMatchingPairList &correspondences, const mrpt::maps::TMatchingParams &params, mrpt::maps::TMatchingExtraResults &extraResults) const override
Computes the matching between this and another 2D point map, which includes finding: ...
std::string fileNameStripInvalidChars(const std::string &filename, const char replacement_to_invalid_chars='_')
Replace invalid filename chars by underscores (&#39;_&#39;) or any other user-given char. ...
Definition: filesystem.cpp:329
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
MapCanComputeLikelihood(const CMultiMetricMap &m, const CObservation *_obs, bool &_can)
bool isEmpty() const override
Returns true if all maps returns true in their isEmpty() method.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const override
Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose" In the case of a multi-metric map, this returns the average between the maps.
Additional results from the determination of matchings between point clouds, etc., apart from the pairings themselves.
const char * className
Definition: CObject.h:34
void internal_clear() override
Internal method called by clear()
A list of TMatchingPair.
Definition: TMatchingPair.h:70
This namespace contains representation of robot actions and observations.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
CMultiMetricMap()=default
Default ctor: empty list of maps.
bool internal_canComputeObservationLikelihood(const mrpt::obs::CObservation &obs) const override
Internal method called by canComputeObservationLikelihood()
GLsizei const GLchar ** string
Definition: glext.h:4116
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
double internal_computeObservationLikelihood(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D &takenFrom) override
Internal method called by computeObservationLikelihood()
const CObservation * obs
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
const mrpt::maps::CSimplePointsMap * getAsSimplePointsMap() const override
If the map is a simple points map or it&#39;s a multi-metric map that contains EXACTLY one simple points ...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
MapIsEmpty(bool &_is_empty)
#define MRPT_END
Definition: exceptions.h:245
GLuint in
Definition: glext.h:7391
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
mrpt::maps::CMetricMap::Ptr mapByIndex(size_t idx) const
Gets the i-th map On out-of-bounds.
This class stores any customizable set of metric maps.
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
virtual float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const
Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose" In the case of a multi-metric map, this returns the average between the maps.
Definition: CMetricMap.cpp:149
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
void setListOfMaps(const mrpt::maps::TSetOfMetricMapInitializers &init)
Sets the list of internal map according to the passed list of map initializers (current maps will be ...
static void run(const CMultiMetricMap &_mmm, OP op)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 9b18308f3 Mon Nov 18 23:39:25 2019 +0100 at lun nov 18 23:45:12 CET 2019