MRPT  2.0.3
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-2020, 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 }
filesystem.h
MapIsEmpty::operator()
void operator()(PTR &ptr)
Definition: CMultiMetricMap.cpp:97
CPoint2D.h
mrpt::serialization::metaprogramming::ObjectReadFromStreamToPtrs
Definition: metaprogramming_serialization.h:40
mrpt::maps::internal::TMetricMapTypesRegistry::Instance
static TMetricMapTypesRegistry & Instance()
Definition: TMetricMapTypesRegistry.cpp:22
mrpt::maps::CMultiMetricMap::serializeFrom
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CMultiMetricMap.cpp:148
mrpt::opengl::CSetOfObjects::Ptr
std::shared_ptr< mrpt::opengl ::CSetOfObjects > Ptr
Definition: CSetOfObjects.h:28
mrpt::maps::CMultiMetricMap::determineMatching2D
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:
Definition: CMultiMetricMap.cpp:215
mrpt::maps::CMultiMetricMap::isEmpty
bool isEmpty() const override
Returns true if all maps returns true in their isEmpty() method.
Definition: CMultiMetricMap.cpp:231
mrpt::maps::CMetricMap::saveMetricMapRepresentationToFile
virtual void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const =0
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >,...
MapCanComputeLikelihood::obs
const CObservation * obs
Definition: CMultiMetricMap.cpp:63
CMultiMetricMap.h
mrpt::maps::CMultiMetricMap::auxParticleFilterCleanUp
void auxParticleFilterCleanUp() override
This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::...
Definition: CMultiMetricMap.cpp:297
mrpt::maps::CMultiMetricMap::serializeTo
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CMultiMetricMap.cpp:141
CConfigFile.h
mrpt::maps::CMultiMetricMap::serializeGetVersion
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CMultiMetricMap.cpp:140
mrpt::maps::TMatchingRatioParams
Parameters for CMetricMap::compute3DMatchingRatio()
Definition: metric_map_types.h:64
mrpt::rtti::TRuntimeClassId::className
const char * className
Definition: CObject.h:34
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
mrpt::tfest
Functions for estimating the optimal transformation between two frames of references given measuremen...
Definition: indiv-compat-decls.h:14
MapAuxPFCleanup
Definition: CMultiMetricMap.cpp:81
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::system::fileNameStripInvalidChars
std::string fileNameStripInvalidChars(const std::string &filename, const char replacement_to_invalid_chars='_')
Replace invalid filename chars by underscores ('_') or any other user-given char.
Definition: filesystem.cpp:329
mrpt::maps::TSetOfMetricMapInitializers
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
Definition: TMetricMapInitializer.h:89
mrpt::maps::CMultiMetricMap::internal_canComputeObservationLikelihood
bool internal_canComputeObservationLikelihood(const mrpt::obs::CObservation &obs) const override
Definition: CMultiMetricMap.cpp:193
mrpt::maps::CMetricMap
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
MapIsEmpty::is_empty
bool & is_empty
Definition: CMultiMetricMap.cpp:93
MapIsEmpty::MapIsEmpty
MapIsEmpty(bool &_is_empty)
Definition: CMultiMetricMap.cpp:95
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::maps::CMultiMetricMap
This class stores any customizable set of metric maps.
Definition: CMultiMetricMap.h:120
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::maps::CMultiMetricMap::compute3DMatchingRatio
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,...
Definition: CMultiMetricMap.cpp:272
mrpt::maps::CSimplePointsMap
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
Definition: CSimplePointsMap.h:30
mrpt::maps::CMetricMap::Ptr
std::shared_ptr< CMetricMap > Ptr
Definition: CMetricMap.h:55
mrpt::maps::CMultiMetricMap::mapByIndex
mrpt::maps::CMetricMap::Ptr mapByIndex(size_t idx) const
Gets the i-th map \excepmapByIndextime_error On out-of-bounds.
Definition: CMultiMetricMap.cpp:318
MapCanComputeLikelihood::operator()
void operator()(PTR &ptr)
Definition: CMultiMetricMap.cpp:74
MapCanComputeLikelihood
Definition: CMultiMetricMap.cpp:61
params
mrpt::vision::TStereoCalibParams params
Definition: chessboard_stereo_camera_calib_unittest.cpp:24
IMPLEMENTS_SERIALIZABLE
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
Definition: CSerializable.h:166
mrpt::maps::TMatchingParams
Parameters for the determination of matchings between point clouds, etc.
Definition: metric_map_types.h:20
metaprogramming_serialization.h
ASSERTMSG_
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
mrpt::maps::CMultiMetricMap::internal_clear
void internal_clear() override
Internal method called by clear()
Definition: CMultiMetricMap.cpp:133
mrpt::maps::CMultiMetricMap::setListOfMaps
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 ...
Definition: CMultiMetricMap.cpp:113
mrpt::maps::CMultiMetricMap::saveMetricMapRepresentationToFile
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >,...
Definition: CMultiMetricMap.cpp:239
mrpt::maps::CMetricMap::compute3DMatchingRatio
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,...
Definition: CMetricMap.cpp:143
MapAuxPFCleanup::operator()
void operator()(PTR &ptr)
Definition: CMultiMetricMap.cpp:85
MapIsEmpty
Definition: CMultiMetricMap.cpp:91
maps-precomp.h
MapExecutor::run
static void run(const CMultiMetricMap &_mmm, OP op)
Definition: CMultiMetricMap.cpp:49
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::maps::CMetricMap::GetRuntimeClass
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
mrpt::maps::CMultiMetricMap::CMultiMetricMap
CMultiMetricMap()=default
Default ctor: empty list of maps.
mrpt::obs::CObservation
Declares a class that represents any robot's observation.
Definition: CObservation.h:43
mrpt::maps::CMultiMetricMap::internal_computeObservationLikelihood
double internal_computeObservationLikelihood(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D &takenFrom) override
Internal method called by computeObservationLikelihood()
Definition: CMultiMetricMap.cpp:178
mrpt::tfest::TMatchingPairList
A list of TMatchingPair.
Definition: TMatchingPair.h:70
mrpt::serialization::metaprogramming
Definition: metaprogramming_serialization.h:13
MapCanComputeLikelihood::can
bool & can
Definition: CMultiMetricMap.cpp:64
mrpt::maps
Definition: CBeacon.h:21
mrpt::maps::CMultiMetricMap::getAs3DObject
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
Definition: CMultiMetricMap.cpp:261
mrpt::maps::TMatchingExtraResults
Additional results from the determination of matchings between point clouds, etc.,...
Definition: metric_map_types.h:51
CArchive.h
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
CSimplePointsMap.h
mrpt::maps::CMultiMetricMap::internal_insertObservation
bool internal_insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr) override
Internal method called by insertObservation()
Definition: CMultiMetricMap.cpp:203
mrpt::maps::CMultiMetricMap::getAsSimplePointsMap
const mrpt::maps::CSimplePointsMap * getAsSimplePointsMap() const override
If the map is a simple points map or it's a multi-metric map that contains EXACTLY one simple points ...
Definition: CMultiMetricMap.cpp:306
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
MapExecutor
Definition: CMultiMetricMap.cpp:44
MapCanComputeLikelihood::MapCanComputeLikelihood
MapCanComputeLikelihood(const CMultiMetricMap &m, const CObservation *_obs, bool &_can)
Definition: CMultiMetricMap.cpp:66



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 23:51:15 UTC 2020