46 template <
typename OP>
54 for_each(mmm.maps.begin(), mmm.maps.end(), op);
78 const CPose3D& _takenFrom,
double& _total_log_lik)
79 : obs(_obs), takenFrom(_takenFrom), total_log_lik(_total_log_lik)
84 template <
typename PTR>
87 total_log_lik += ptr->computeObservationLikelihood(obs, takenFrom);
99 : obs(_obs), can(_can)
104 template <
typename PTR>
107 can = can || ptr->canComputeObservationLikelihood(obs);
120 const CPose3D* _robot_pose,
int& _total_insert)
121 : obs(_obs), robot_pose(_robot_pose), total_insert(_total_insert)
126 template <
typename PTR>
129 bool ret = ptr->insertObservation(obs, robot_pose);
130 if (ret) total_insert++;
143 template <
typename PTR>
146 if (ptr) ptr->getAs3DObject(obj_gl);
153 template <
typename PTR>
156 if (ptr) ptr->auxParticleFilterCleanUp();
164 MapIsEmpty(
bool& _is_empty) : is_empty(_is_empty) { is_empty =
true; }
165 template <
typename PTR>
168 if (ptr) is_empty = is_empty && ptr->isEmpty();
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) 211 maps = std::move(o.maps);
231 if (initializers !=
nullptr)
235 initializers->
begin();
236 it != initializers->
end(); ++it)
254 if (ptr) ptr->clear();
269 out << static_cast<uint32_t>(
m_ID);
291 this->
maps.resize(n);
337 return total_insert != 0;
351 "There is not exactly 1 points maps in the multimetric map.");
353 otherMap, otherMapPose, correspondences,
params, extraResults);
373 for (
size_t idx = 0; idx <
maps.size(); idx++)
380 static_cast<unsigned int>(idx));
407 float accumResult = 0;
409 for (
size_t idx = 0; idx <
maps.size(); idx++)
418 const size_t nMapsComputed =
maps.size();
419 if (nMapsComputed) accumResult /= nMapsComputed;
465 return maps[idx].get_ptr();
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...
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)
void operator()(PTR &ptr)
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
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. ...
virtual void determineMatching2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, mrpt::tfest::TMatchingPairList &correspondences, const mrpt::maps::TMatchingParams ¶ms, mrpt::maps::TMatchingExtraResults &extraResults) const override
Computes the matching between this and another 2D point map, which includes finding: ...
mrpt::opengl::CSetOfObjects::Ptr & obj_gl
void operator()(T &container)
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
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.
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 ¶ms) const override
See the definition in the base class: Calls in this class become a call to every single map in this s...
#define MRPT_CHECK_NORMAL_NUMBER(v)
Throws an exception if the number is NaN, IND, or +/-INF, or return the same number otherwise...
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()
static TMetricMapTypesRegistry & Instance()
This namespace contains representation of robot actions and observations.
CMultiMetricMap(const mrpt::maps::TSetOfMetricMapInitializers *initializers=nullptr)
Constructor.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
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
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.
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...
virtual const mrpt::maps::CSimplePointsMap * getAsSimplePointsMap() const override
If the map is a simple point map or it'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).
Declares a class that represents any robot'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' content...
MapIsEmpty(bool &_is_empty)
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
void operator()(PTR &ptr)
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.
Parameters for the determination of matchings between point clouds, etc.
unsigned __int32 uint32_t
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
virtual float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams ¶ms) 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.
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...
void operator()(PTR &ptr)
static void run(const CMultiMetricMap &_mmm, OP op)