Main MRPT website > C++ reference for MRPT 1.9.9
CMultiMetricMap.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 "slam-precomp.h" // Precompiled headers
11 
13 #include <mrpt/poses/CPoint2D.h>
17 
18 using namespace mrpt::maps;
19 using namespace mrpt::poses;
20 using namespace mrpt::obs;
21 using namespace mrpt::tfest;
23 
25 
26 // ------------------------------------------------------------------------
27 // A few words explaining how all this works:
28 // The main hub for operating with all the maps in the internal list
29 // if MapExecutor.
30 //
31 // All operations go thru MapExecutor::run<OP>() with OP being one of the
32 // possible map operations (clear, matching, likelihood, etc.). The
33 // idea is that when adding new map types to the internal list of
34 // CMultiMetricMap, *only* "MapExecutor" methods must be updated.
35 // (The only exception are readFromStream() & writeToStream())
36 //
37 // The map-specific operations all go into template specializations of
38 // other helper structures or in overloaded methods.
39 // JLBC (7-AUG-2011)
40 // ------------------------------------------------------------------------
41 
43 {
44  // Apply operation to maps in the same order as declared in
45  // CMultiMetricMap.h:
46  template <typename OP>
47  static void run(const CMultiMetricMap& _mmm, OP op)
48  {
50  CMultiMetricMap& mmm =
51  const_cast<CMultiMetricMap&>(_mmm); // This is to avoid duplicating
52  // "::run()" for const and
53  // non-const.
54  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  template <typename T>
64  inline void operator()(T& container)
65  {
66  container.clear();
67  }
68 };
69 
71 {
72  const CObservation* obs;
74  double& total_log_lik;
75 
77  const CMultiMetricMap& m, const CObservation* _obs,
78  const CPose3D& _takenFrom, double& _total_log_lik)
79  : obs(_obs), takenFrom(_takenFrom), total_log_lik(_total_log_lik)
80  {
81  total_log_lik = 0;
82  }
83 
84  template <typename PTR>
85  inline void operator()(PTR& ptr)
86  {
87  total_log_lik += ptr->computeObservationLikelihood(obs, takenFrom);
88  }
89 
90 }; // end of MapComputeLikelihood
91 
93 {
94  const CObservation* obs;
95  bool& can;
96 
98  const CMultiMetricMap& m, const CObservation* _obs, bool& _can)
99  : obs(_obs), can(_can)
100  {
101  can = false;
102  }
103 
104  template <typename PTR>
105  inline void operator()(PTR& ptr)
106  {
107  can = can || ptr->canComputeObservationLikelihood(obs);
108  }
109 
110 }; // end of MapCanComputeLikelihood
111 
113 {
117 
119  const CMultiMetricMap& m, const CObservation* _obs,
120  const CPose3D* _robot_pose, int& _total_insert)
121  : obs(_obs), robot_pose(_robot_pose), total_insert(_total_insert)
122  {
123  total_insert = 0;
124  }
125 
126  template <typename PTR>
127  inline void operator()(PTR& ptr)
128  {
129  bool ret = ptr->insertObservation(obs, robot_pose);
130  if (ret) total_insert++;
131  }
132 }; // end of MapInsertObservation
133 
135 {
137 
139  : obj_gl(_obj_gl)
140  {
141  }
142 
143  template <typename PTR>
144  inline void operator()(PTR& ptr)
145  {
146  if (ptr) ptr->getAs3DObject(obj_gl);
147  }
148 }; // end of MapGetAs3DObject
149 
151 {
153  template <typename PTR>
154  inline void operator()(PTR& ptr)
155  {
156  if (ptr) ptr->auxParticleFilterCleanUp();
157  }
158 }; // end of MapAuxPFCleanup
159 
161 {
162  bool& is_empty;
163 
164  MapIsEmpty(bool& _is_empty) : is_empty(_is_empty) { is_empty = true; }
165  template <typename PTR>
166  inline void operator()(PTR& ptr)
167  {
168  if (ptr) is_empty = is_empty && ptr->isEmpty();
169  }
170 }; // end of MapIsEmpty
171 
172 // ------------------- End of map-operations helper templates
173 // -------------------
174 
175 #define ALL_PROXIES_INIT \
176  m_pointsMaps(maps), m_gridMaps(maps), m_octoMaps(maps), \
177  m_colourOctoMaps(maps), m_gasGridMaps(maps), m_wifiGridMaps(maps), \
178  m_heightMaps(maps), m_heightMRFMaps(maps), m_reflectivityMaps(maps), \
179  m_colourPointsMap(maps), m_weightedPointsMap(maps), \
180  m_landmarksMap(maps), m_beaconMap(maps)
181 
182 // Ctor
184  const TSetOfMetricMapInitializers* initializers)
185  : maps(), ALL_PROXIES_INIT, m_ID(0)
186 {
187  MRPT_START
188  setListOfMaps(initializers);
189  MRPT_END
190 }
191 
193  : maps(o.maps), ALL_PROXIES_INIT, m_ID(o.m_ID)
194 {
195 }
196 
198 {
199  maps = o.maps;
200  m_ID = o.m_ID;
201  return *this;
202 }
203 
205  : maps(std::move(o.maps)), ALL_PROXIES_INIT, m_ID(o.m_ID)
206 {
207 }
208 
210 {
211  maps = std::move(o.maps);
212  m_ID = o.m_ID;
213  return *this;
214 }
215 
216 /*---------------------------------------------------------------
217  setListOfMaps
218  ---------------------------------------------------------------*/
220  const mrpt::maps::TSetOfMetricMapInitializers* initializers)
221 {
222  MRPT_START
223 
224  // Erase current list of maps:
225  deleteAllMaps();
226 
229 
230  // Do we have any initializer?
231  if (initializers != nullptr)
232  {
233  // Process each entry in the "initializers" and create maps accordingly:
235  initializers->begin();
236  it != initializers->end(); ++it)
237  {
238  // Create map from the list of all params:
239  mrpt::maps::CMetricMap* theMap =
240  mmr.factoryMapObjectFromDefinition(*it->get());
241  ASSERT_(theMap);
242  // Add to the list of maps:
243  this->maps.push_back(mrpt::maps::CMetricMap::Ptr(theMap));
244  }
245 
246  } // end if initializers!=nullptr
247 
248  MRPT_END
249 }
250 
252 {
253  MapExecutor::run(*this, [](auto ptr) {
254  if (ptr) ptr->clear();
255  });
256 }
257 
259 {
260  // Clear list:
261  maps.clear();
262  m_ID = 0;
263 }
264 
267 {
268  // Version 11: simply the list of maps:
269  out << static_cast<uint32_t>(m_ID);
270  const uint32_t n = static_cast<uint32_t>(maps.size());
271  for (uint32_t i = 0; i < n; i++) out << *maps[i];
272 }
273 
276 {
277  switch (version)
278  {
279  case 11:
280  {
281  // ID:
282  {
283  uint32_t ID;
284  in >> ID;
285  m_ID = ID;
286  }
287 
288  // List of maps:
289  uint32_t n;
290  in >> n;
291  this->maps.resize(n);
292  for_each(
293  maps.begin(), maps.end(),
295  }
296  break;
297  default:
299  };
300 }
301 
302 // Read docs in base class
304  const CObservation* obs, const CPose3D& takenFrom)
305 {
306  double ret_log_lik;
307  MapComputeLikelihood op_likelihood(*this, obs, takenFrom, ret_log_lik);
308 
309  MapExecutor::run(*this, op_likelihood);
310 
311  MRPT_CHECK_NORMAL_NUMBER(ret_log_lik); //-V614
312  return ret_log_lik;
313 }
314 
315 // Read docs in base class
317  const CObservation* obs) const
318 {
319  bool can_comp;
320 
321  MapCanComputeLikelihood op_can_likelihood(*this, obs, can_comp);
322  MapExecutor::run(*this, op_can_likelihood);
323  return can_comp; //-V614
324 }
325 
326 /*---------------------------------------------------------------
327  insertObservation
328 
329 Insert the observation information into this map.
330  ---------------------------------------------------------------*/
332  const CObservation* obs, const CPose3D* robotPose)
333 {
334  int total_insert;
335  MapInsertObservation op_insert_obs(*this, obs, robotPose, total_insert);
336  MapExecutor::run(*this, op_insert_obs);
337  return total_insert != 0; //-V614
338 }
339 
340 /*---------------------------------------------------------------
341  computeMatchingWith2D
342  ---------------------------------------------------------------*/
344  const mrpt::maps::CMetricMap* otherMap, const CPose2D& otherMapPose,
345  TMatchingPairList& correspondences, const TMatchingParams& params,
346  TMatchingExtraResults& extraResults) const
347 {
348  MRPT_START
349  ASSERTMSG_(
350  m_pointsMaps.size() == 1,
351  "There is not exactly 1 points maps in the multimetric map.");
352  m_pointsMaps[0]->determineMatching2D(
353  otherMap, otherMapPose, correspondences, params, extraResults);
354  MRPT_END
355 }
356 
357 /*---------------------------------------------------------------
358  isEmpty
359  ---------------------------------------------------------------*/
361 {
362  bool is_empty;
363  MapIsEmpty op_insert_obs(is_empty); //-V614
364  MapExecutor::run(*this, op_insert_obs);
365  return is_empty;
366 }
367 
369  const std::string& filNamePrefix) const
370 {
371  MRPT_START
372 
373  for (size_t idx = 0; idx < maps.size(); idx++)
374  {
375  const mrpt::maps::CMetricMap* m = maps[idx].get();
376  ASSERT_(m);
377  std::string fil = filNamePrefix;
378  fil += format(
379  "_%s_%02u", m->GetRuntimeClass()->className,
380  static_cast<unsigned int>(idx));
382  }
383 
384  MRPT_END
385 }
386 
387 /*---------------------------------------------------------------
388  getAs3DObject
389 ---------------------------------------------------------------*/
391  mrpt::opengl::CSetOfObjects::Ptr& outObj) const
392 {
393  MRPT_START
394  MapGetAs3DObject op_get_3D(outObj);
395  MapExecutor::run(*this, op_get_3D);
396  MRPT_END
397 }
398 
399 // See docs in base class
401  const mrpt::maps::CMetricMap* otherMap,
402  const mrpt::poses::CPose3D& otherMapPose,
403  const TMatchingRatioParams& params) const
404 {
405  MRPT_START
406 
407  float accumResult = 0;
408 
409  for (size_t idx = 0; idx < maps.size(); idx++)
410  {
411  const mrpt::maps::CMetricMap* m = maps[idx].get();
412  ASSERT_(m);
413  accumResult +=
414  m->compute3DMatchingRatio(otherMap, otherMapPose, params);
415  }
416 
417  // Return average:
418  const size_t nMapsComputed = maps.size();
419  if (nMapsComputed) accumResult /= nMapsComputed;
420  return accumResult;
421 
422  MRPT_END
423 }
424 
425 /*---------------------------------------------------------------
426  auxParticleFilterCleanUp
427  ---------------------------------------------------------------*/
429 {
430  MRPT_START
431  MapAuxPFCleanup op_cleanup;
432  MapExecutor::run(*this, op_cleanup);
433  MRPT_END
434 }
435 
436 /** If the map is a simple points map or it's a multi-metric map that contains
437  * EXACTLY one simple points map, return it.
438  * Otherwise, return NULL
439  */
441 {
442  MRPT_START
443  ASSERT_(m_pointsMaps.size() == 1 || m_pointsMaps.size() == 0);
444  if (m_pointsMaps.empty())
445  return nullptr;
446  else
447  return m_pointsMaps[0].get();
448  MRPT_END
449 }
451 {
452  MRPT_START
453  ASSERT_(m_pointsMaps.size() == 1 || m_pointsMaps.size() == 0);
454  if (m_pointsMaps.empty())
455  return nullptr;
456  else
457  return m_pointsMaps[0].get();
458  MRPT_END
459 }
460 
461 /** Gets the i-th map \exception std::runtime_error On out-of-bounds */
463 {
464  ASSERT_BELOW_(idx, maps.size());
465  return maps[idx].get_ptr();
466 }
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)
void deleteAllMaps()
Deletes all maps and clears the internal lists of maps (with clear_unique(), so user copies remain al...
#define MRPT_START
Definition: exceptions.h:262
ProxyFilterContainerByClass< mrpt::maps::CSimplePointsMap::Ptr, TListMaps > m_pointsMaps
STL-like proxy to access this kind of maps in maps.
mrpt::maps::CMetricMap * factoryMapObjectFromDefinition(const mrpt::maps::TMetricMapInitializer &mi) const
Return nullptr if not found.
MapGetAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &_obj_gl)
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:165
void operator()(PTR &ptr)
GLenum GLsizei n
Definition: glext.h:5074
const CPose3D * robot_pose
double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) override
Internal method called by computeObservationLikelihood()
void operator()(PTR &ptr)
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
STL namespace.
virtual 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: ...
const CObservation * obs
mrpt::opengl::CSetOfObjects::Ptr & obj_gl
void operator()(T &container)
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
#define ALL_PROXIES_INIT
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
MapCanComputeLikelihood(const CMultiMetricMap &m, const CObservation *_obs, bool &_can)
const CPose3D & takenFrom
bool isEmpty() const override
Returns true if all maps returns true to their isEmpty() method, which is map-dependent.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
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
See the definition in the base class: Calls in this class become a call to every single map in this s...
Additional results from the determination of matchings between point clouds, etc., apart from the pairings themselves.
#define MRPT_CHECK_NORMAL_NUMBER(v)
Throws an exception if the number is NaN, IND, or +/-INF, or return the same number otherwise...
Definition: exceptions.h:118
const char * className
Definition: CObject.h:33
void operator()(PTR &ptr)
void internal_clear() override
Clear all elements of the map.
CMultiMetricMap & operator=(const CMultiMetricMap &o)
bool internal_insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=nullptr) override
Internal method called by insertObservation()
A list of TMatchingPair.
Definition: TMatchingPair.h:83
This namespace contains representation of robot actions and observations.
CMultiMetricMap(const mrpt::maps::TSetOfMetricMapInitializers *initializers=nullptr)
Constructor.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
bool internal_canComputeObservationLikelihood(const mrpt::obs::CObservation *obs) const override
Returns true if any of the inner maps is able to compute a sensible likelihood function for this obse...
GLsizei const GLchar ** string
Definition: glext.h:4101
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
mrpt::maps::CMetricMap::Ptr getMapByIndex(size_t idx) const
Gets the i-th map.
MapComputeLikelihood(const CMultiMetricMap &m, const CObservation *_obs, const CPose3D &_takenFrom, double &_total_log_lik)
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:48
#define IMPLEMENTS_MRPT_OBJECT_ALT_NAME(class_name, base, NameSpace, alt_name)
This must be inserted in all CObject classes implementation files.
Definition: CObject.h:217
const CObservation * obs
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
virtual const mrpt::maps::CSimplePointsMap * getAsSimplePointsMap() const override
If the map is a simple point map or it&#39;s a multi-metric map that contains EXACTLY one simple point ma...
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
void setListOfMaps(const mrpt::maps::TSetOfMetricMapInitializers *initializers)
Sets the list of internal map according to the passed list of map initializers (Current maps&#39; content...
MapIsEmpty(bool &_is_empty)
void operator()(PTR &ptr)
#define MRPT_END
Definition: exceptions.h:266
GLuint in
Definition: glext.h:7274
unsigned int m_ID
An auxiliary variable that can be used freely by the users (this will be copied to other maps using t...
TListMaps maps
The list of MRPT metric maps in this object.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
This class stores any customizable set of metric maps.
Class factory & registry for map classes.
const CObservation * obs
Parameters for the determination of matchings between point clouds, etc.
unsigned __int32 uint32_t
Definition: rptypes.h:47
std::deque< TMetricMapInitializer::Ptr >::const_iterator const_iterator
Functions for estimating the optimal transformation between two frames of references given measuremen...
GLenum const GLfloat * params
Definition: glext.h:3534
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:157
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
MapInsertObservation(const CMultiMetricMap &m, const CObservation *_obs, const CPose3D *_robot_pose, int &_total_insert)
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
The implementation in this class just calls all the corresponding method of the contained metric maps...
static void run(const CMultiMetricMap &_mmm, OP op)



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