Main MRPT website > C++ reference for MRPT 1.5.7
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-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 "slam-precomp.h" // Precompiled headers
11 
12 #include <mrpt/utils/CConfigFile.h>
13 #include <mrpt/poses/CPoint2D.h>
16 #include <mrpt/utils/CStream.h>
17 
18 using namespace mrpt::maps;
19 using namespace mrpt::utils;
20 using namespace mrpt::poses;
21 using namespace mrpt::obs;
22 using namespace mrpt::utils::metaprogramming;
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 
42 struct MapExecutor {
43  // Apply operation to maps in the same order as declared in CMultiMetricMap.h:
44  template <typename OP>
45  static void run(const CMultiMetricMap &_mmm, OP &op)
46  {
48  CMultiMetricMap &mmm = const_cast<CMultiMetricMap&>(_mmm); // This is to avoid duplicating "::run()" for const and non-const.
49  for_each( mmm.maps.begin(),mmm.maps.end(), op );
50  MRPT_END
51  }
52 }; // end of MapExecutor
53 
54 // ------------------- Begin of map-operations helper templates -------------------
56 {
57  template<typename T>
58  inline void operator()(T &container) {
59  container.clear();
60  }
61 };
62 
64 {
65  const CObservation * obs;
66  const CPose3D & takenFrom;
67  double & total_log_lik;
68 
69  MapComputeLikelihood(const CMultiMetricMap &m,const CObservation * _obs, const CPose3D & _takenFrom, double & _total_log_lik) :
70  obs(_obs), takenFrom(_takenFrom),
71  total_log_lik(_total_log_lik)
72  {
73  total_log_lik=0;
74  }
75 
76  template <typename PTR>
77  inline void operator()(PTR &ptr) {
78  total_log_lik+=ptr->computeObservationLikelihood(obs,takenFrom);
79  }
80 
81 }; // end of MapComputeLikelihood
82 
84 {
85  const CObservation * obs;
86  bool & can;
87 
88  MapCanComputeLikelihood(const CMultiMetricMap &m,const CObservation * _obs, bool & _can) :
89  obs(_obs),
90  can(_can)
91  {
92  can = false;
93  }
94 
95  template <typename PTR>
96  inline void operator()(PTR &ptr) {
97  can = can || ptr->canComputeObservationLikelihood(obs);
98  }
99 
100 }; // end of MapCanComputeLikelihood
101 
102 
104 {
105  const CObservation * obs;
108 
109  MapInsertObservation(const CMultiMetricMap &m,const CObservation * _obs, const CPose3D * _robot_pose, int & _total_insert) :
110  obs(_obs), robot_pose(_robot_pose),
111  total_insert(_total_insert)
112  {
113  total_insert = 0;
114  }
115 
116  template <typename PTR>
117  inline void operator()(PTR &ptr) {
118  bool ret = ptr->insertObservation(obs,robot_pose);
119  if (ret) total_insert++;
120  }
121 }; // end of MapInsertObservation
122 
124 {
125  mrpt::opengl::CSetOfObjectsPtr & obj_gl;
126 
127  MapGetAs3DObject(mrpt::opengl::CSetOfObjectsPtr &_obj_gl) : obj_gl(_obj_gl)
128  {
129  }
130 
131  template <typename PTR>
132  inline void operator()(PTR &ptr) {
133  if (ptr) ptr->getAs3DObject(obj_gl);
134  }
135 }; // end of MapGetAs3DObject
136 
138 {
140 
141  template <typename PTR>
142  inline void operator()(PTR &ptr) {
143  if (ptr) ptr->auxParticleFilterCleanUp();
144  }
145 }; // end of MapAuxPFCleanup
146 
147 
149 {
150  bool & is_empty;
151 
152  MapIsEmpty(bool & _is_empty) : is_empty(_is_empty)
153  {
154  is_empty = true;
155  }
156 
157  template <typename PTR>
158  inline void operator()(PTR &ptr) {
159  if (ptr)
160  is_empty = is_empty && ptr->isEmpty();
161  }
162 }; // end of MapIsEmpty
163 
164 // ------------------- End of map-operations helper templates -------------------
165 
166 #define ALL_PROXIES_INIT \
167  m_pointsMaps(maps), \
168  m_gridMaps(maps), \
169  m_octoMaps(maps), \
170  m_colourOctoMaps(maps), \
171  m_gasGridMaps(maps), \
172  m_wifiGridMaps(maps), \
173  m_heightMaps(maps), \
174  m_heightMRFMaps(maps), \
175  m_reflectivityMaps(maps), \
176  m_colourPointsMap(maps), \
177  m_weightedPointsMap(maps), \
178  m_landmarksMap(maps), \
179  m_beaconMap(maps)
180 
181 // Ctor
183  maps(),
185  m_ID(0)
186 {
187  MRPT_START
188  setListOfMaps(initializers);
189  MRPT_END
190 }
191 
193  maps(o.maps),
195  m_ID(o.m_ID)
196 {
197 }
198 
200 {
201  maps = o.maps;
202  m_ID = o.m_ID;
203  return *this;
204 }
205 
206 #if (__cplusplus>199711L)
208  maps(std::move(o.maps)),
210  m_ID(o.m_ID)
211 {
212 }
213 
215 {
216  maps = std::move(o.maps);
217  m_ID = o.m_ID;
218  return *this;
219 }
220 #endif
221 
222 /*---------------------------------------------------------------
223  setListOfMaps
224  ---------------------------------------------------------------*/
226 {
227  MRPT_START
228 
229  // Erase current list of maps:
230  deleteAllMaps();
231 
233 
234  // Do we have any initializer?
235  if (initializers!=NULL)
236  {
237  // Process each entry in the "initializers" and create maps accordingly:
238  for (TSetOfMetricMapInitializers::const_iterator it = initializers->begin();it!=initializers->end();++it)
239  {
240  // Create map from the list of all params:
241  mrpt::maps::CMetricMap *theMap = mmr.factoryMapObjectFromDefinition(*it->get());
242  ASSERT_(theMap)
243 
244  // Add to the list of maps:
245  this->maps.push_back( mrpt::maps::CMetricMapPtr(theMap) );
246  }
247 
248  } // end if initializers!=NULL
249 
250  MRPT_END
251 }
252 
253 /*---------------------------------------------------------------
254  clear
255  ---------------------------------------------------------------*/
257 {
258  ObjectClear op;
259  MapExecutor::run(*this, op);
260 }
261 
262 // Deletes all maps and clears the internal lists of maps (with clear_unique(), so user copies remain alive)
264 {
265  // Clear smart pointers:
267  MapExecutor::run(*this, op_clear_unique);
268 
269  // Clear list:
270  maps.clear();
271  m_ID = 0;
272 }
273 
274 /*---------------------------------------------------------------
275  Implements the writing to a CStream capability of CSerializable objects
276  ---------------------------------------------------------------*/
278 {
279  if (version)
280  *version = 11;
281  else
282  {
283  // Version 11: simply the list of maps:
284  out << static_cast<uint32_t>(m_ID);
285 
286  const uint32_t n = static_cast<uint32_t>(maps.size());
287  for (uint32_t i=0;i<n;i++)
288  out << *maps[i];
289  }
290 }
291 
292 /*---------------------------------------------------------------
293  Implements the reading from a CStream capability of CSerializable objects
294  ---------------------------------------------------------------*/
296 {
297  switch(version)
298  {
299  case 11:
300  {
301  // ID:
302  {
303  uint32_t ID;
304  in >> ID; m_ID = ID;
305  }
306 
307  // List of maps:
308  uint32_t n;
309  in >> n;
310  this->maps.resize(n);
312 
313  } break;
314  default:
316 
317  };
318 }
319 
320 // Read docs in base class
322  const CObservation *obs,
323  const CPose3D &takenFrom )
324 {
325  double ret_log_lik;
326  MapComputeLikelihood op_likelihood(*this,obs,takenFrom,ret_log_lik);
327 
328  MapExecutor::run(*this,op_likelihood);
329 
330  MRPT_CHECK_NORMAL_NUMBER(ret_log_lik) //-V614
331  return ret_log_lik;
332 }
333 
334 // Read docs in base class
336 {
337  bool can_comp;
338 
339  MapCanComputeLikelihood op_can_likelihood(*this,obs,can_comp);
340  MapExecutor::run(*this,op_can_likelihood);
341  return can_comp; //-V614
342 }
343 
344 /*---------------------------------------------------------------
345  insertObservation
346 
347 Insert the observation information into this map.
348  ---------------------------------------------------------------*/
350  const CObservation *obs,
351  const CPose3D *robotPose)
352 {
353  int total_insert;
354  MapInsertObservation op_insert_obs(*this,obs,robotPose,total_insert);
355  MapExecutor::run(*this,op_insert_obs);
356  return total_insert!=0; //-V614
357 }
358 
359 /*---------------------------------------------------------------
360  computeMatchingWith2D
361  ---------------------------------------------------------------*/
363  const mrpt::maps::CMetricMap * otherMap,
364  const CPose2D & otherMapPose,
365  TMatchingPairList & correspondences,
366  const TMatchingParams & params,
367  TMatchingExtraResults & extraResults ) const
368 {
369  MRPT_START
370  ASSERTMSG_( m_pointsMaps.size()==1, "There is not exactly 1 points maps in the multimetric map." );
371  m_pointsMaps[0]->determineMatching2D(otherMap,otherMapPose,correspondences,params,extraResults);
372  MRPT_END
373 }
374 
375 /*---------------------------------------------------------------
376  isEmpty
377  ---------------------------------------------------------------*/
379 {
380  bool is_empty;
381  MapIsEmpty op_insert_obs(is_empty); //-V614
382  MapExecutor::run(*this,op_insert_obs);
383  return is_empty;
384 }
385 
387 {
388  MRPT_START
389 
390  for (size_t idx=0;idx<maps.size();idx++)
391  {
392  const mrpt::maps::CMetricMap * m = maps[idx].pointer();
393  ASSERT_(m)
394 
395  std::string fil = filNamePrefix;
396  fil += format("_%s_%02u", m->GetRuntimeClass()->className, static_cast<unsigned int>(idx));
398  }
399 
400  MRPT_END
401 }
402 
403 
404 /*---------------------------------------------------------------
405  getAs3DObject
406 ---------------------------------------------------------------*/
407 void CMultiMetricMap::getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const
408 {
409  MRPT_START
410  MapGetAs3DObject op_get_3D(outObj);
411  MapExecutor::run(*this,op_get_3D);
412  MRPT_END
413 }
414 
415 // See docs in base class
417 {
418  MRPT_START
419 
420  float accumResult = 0;
421 
422  for (size_t idx=0;idx<maps.size();idx++)
423  {
424  const mrpt::maps::CMetricMap * m = maps[idx].pointer();
425  ASSERT_(m)
426  accumResult += m->compute3DMatchingRatio( otherMap, otherMapPose,params);
427  }
428 
429  // Return average:
430  const size_t nMapsComputed = maps.size();
431  if (nMapsComputed) accumResult/=nMapsComputed;
432  return accumResult;
433 
434  MRPT_END
435 }
436 
437 /*---------------------------------------------------------------
438  auxParticleFilterCleanUp
439  ---------------------------------------------------------------*/
441 {
442  MRPT_START
443  MapAuxPFCleanup op_cleanup;
444  MapExecutor::run(*this,op_cleanup);
445  MRPT_END
446 }
447 
448 /** If the map is a simple points map or it's a multi-metric map that contains EXACTLY one simple points map, return it.
449 * Otherwise, return NULL
450 */
452 {
453  MRPT_START
455  if (m_pointsMaps.empty()) return NULL;
456  else return m_pointsMaps[0].pointer();
457  MRPT_END
458 }
460 {
461  MRPT_START
463  if (m_pointsMaps.empty()) return NULL;
464  else return m_pointsMaps[0].pointer();
465  MRPT_END
466 }
467 
468 
469 /** Gets the i-th map \exception std::runtime_error On out-of-bounds */
470 mrpt::maps::CMetricMapPtr CMultiMetricMap::getMapByIndex(size_t idx) const
471 {
472  ASSERT_BELOW_(idx,maps.size())
473  return maps[idx].get_ptr();
474 }
virtual void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const =0
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
Parameters for CMetricMap::compute3DMatchingRatio()
void auxParticleFilterCleanUp() MRPT_OVERRIDE
This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::...
void operator()(PTR &ptr)
void deleteAllMaps()
Deletes all maps and clears the internal lists of maps (with clear_unique(), so user copies remain al...
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
An object for making smart pointers unique (ie, making copies if necessary), intended for being used ...
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const MRPT_OVERRIDE
See the definition in the base class: Calls in this class become a call to every single map in this s...
mrpt::maps::CMetricMap * factoryMapObjectFromDefinition(const mrpt::maps::TMetricMapInitializer &mi) const
Return NULL if not found.
void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
Returns a 3D object representing the map.
void operator()(PTR &ptr)
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
#define MRPT_CHECK_NORMAL_NUMBER(val)
virtual const mrpt::maps::CSimplePointsMap * getAsSimplePointsMap() const MRPT_OVERRIDE
If the map is a simple point map or it&#39;s a multi-metric map that contains EXACTLY one simple point ma...
#define ASSERT_BELOW_(__A, __B)
GLenum GLsizei n
Definition: glext.h:4618
const CPose3D * robot_pose
void operator()(PTR &ptr)
std::deque< TMetricMapInitializerPtr >::const_iterator const_iterator
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
mrpt::opengl::CSetOfObjectsPtr & obj_gl
STL namespace.
const CObservation * obs
bool internal_canComputeObservationLikelihood(const mrpt::obs::CObservation *obs) const MRPT_OVERRIDE
Returns true if any of the inner maps is able to compute a sensible likelihood function for this obse...
void operator()(T &container)
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
#define ALL_PROXIES_INIT
MapCanComputeLikelihood(const CMultiMetricMap &m, const CObservation *_obs, bool &_can)
const CPose3D & takenFrom
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
Additional results from the determination of matchings between point clouds, etc., apart from the pairings themselves.
#define MRPT_END
void operator()(PTR &ptr)
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
void internal_clear() MRPT_OVERRIDE
Clear all elements of the map.
A list of TMatchingPair.
Definition: TMatchingPair.h:78
CMultiMetricMap & operator=(const CMultiMetricMap &o)
bool internal_insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=NULL) MRPT_OVERRIDE
Internal method called by insertObservation()
This namespace contains representation of robot actions and observations.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
A set of utility objects for metaprogramming with STL algorithms.
int version
Definition: mrpt_jpeglib.h:898
GLsizei const GLchar ** string
Definition: glext.h:3919
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
virtual void determineMatching2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, mrpt::utils::TMatchingPairList &correspondences, const mrpt::maps::TMatchingParams &params, mrpt::maps::TMatchingExtraResults &extraResults) const MRPT_OVERRIDE
Computes the matching between this and another 2D point map, which includes finding: ...
static void run(const CMultiMetricMap &_mmm, OP &op)
An object for clearing an object (invokes its method "->clear()") given a pointer or smart-pointer...
MapComputeLikelihood(const CMultiMetricMap &m, const CObservation *_obs, const CPose3D &_takenFrom, double &_total_log_lik)
mrpt::maps::CMetricMapPtr getMapByIndex(size_t idx) const
Gets the i-th map.
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
#define MRPT_START
const CObservation * obs
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.
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)
bool isEmpty() const MRPT_OVERRIDE
Returns true if all maps returns true to their isEmpty() method, which is map-dependent. Read the docs of each map class.
void operator()(PTR &ptr)
GLuint in
Definition: glext.h:6301
#define ASSERT_(f)
unsigned int m_ID
An auxiliary variable that can be used freely by the users (this will be copied to other maps using t...
double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) MRPT_OVERRIDE
Internal method called by computeObservationLikelihood()
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE
The implementation in this class just calls all the corresponding method of the contained metric maps...
TListMaps maps
The list of MRPT metric maps in this object.
ProxyFilterContainerByClass< mrpt::maps::CSimplePointsMapPtr, TListMaps > m_pointsMaps
STL-like proxy to access this kind of maps in maps.
This class stores any customizable set of metric maps.
const char * className
Definition: CObject.h:38
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:49
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const
Returns information about the class of an object in runtime.
#define ASSERTMSG_(f, __ERROR_MSG)
GLenum const GLfloat * params
Definition: glext.h:3514
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:168
CMultiMetricMap(const mrpt::maps::TSetOfMetricMapInitializers *initializers=NULL)
Constructor.
MapInsertObservation(const CMultiMetricMap &m, const CObservation *_obs, const CPose3D *_robot_pose, int &_total_insert)
MapGetAs3DObject(mrpt::opengl::CSetOfObjectsPtr &_obj_gl)



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