diff --git a/appveyor.yml b/appveyor.yml index 1c960a27aa..c430cbda4b 100644 --- a/appveyor.yml +++ b/appveyor.yml @@ -31,7 +31,7 @@ build_script: - cd c:\projects\mrpt - cmd: IF NOT DEFINED APPVEYOR_REPO_TAG_NAME SET PKG_NAME=branch-%APPVEYOR_REPO_BRANCH% - cmd: IF DEFINED APPVEYOR_REPO_TAG_NAME SET PKG_NAME=release-%APPVEYOR_REPO_TAG_NAME% - - cmd: move c:\projects\mrpt\build\MRPT*.exe c:\projects\mrpt\mrpt-%PKG_NAME%.exe + - cmd: IF EXIST c:\projects\mrpt\build\MRPT*.exe move c:\projects\mrpt\build\MRPT*.exe c:\projects\mrpt\mrpt-%PKG_NAME%.exe - dir install: diff --git a/doc/source/bibliography.bib b/doc/source/bibliography.bib index 662f717301..be172e2b39 100644 --- a/doc/source/bibliography.bib +++ b/doc/source/bibliography.bib @@ -170,4 +170,13 @@ @article{horn1987closed pages={629--642}, year={1987}, publisher={Optica Publishing Group} +} + +@inproceedings{wurm2010octomap, + title={OctoMap: A probabilistic, flexible, and compact 3D map representation for robotic systems}, + author={Wurm, Kai M and Hornung, Armin and Bennewitz, Maren and Stachniss, Cyrill and Burgard, Wolfram}, + booktitle={Proc. of the ICRA 2010 workshop on best practice in 3D perception and modeling for mobile manipulation}, + volume={2}, + pages={3}, + year={2010} } \ No newline at end of file diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index b8fec54d7d..0a3d321d91 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -1,6 +1,13 @@ \page changelog Change Log -# Version 2.10.3: UNRELEASED +# Version 2.11.0: UNRELEASED +- Changes in libraries: + - \ref mrpt_maps_grp + - New voxel map containers, based on Faconti's [Bonxai](https://github.com/facontidavide/Bonxai) header-only libray (MPL-2.0 license): + - mrpt::maps::CVoxelMap + - mrpt::maps::CVoxelMapRGB + - Example: \ref maps_voxelmap_from_tum_dataset + - Example: \ref maps_voxelmap_simple - BUG FIXES: - Fix python wrapper FTBFS in armhf and other architectures. - Fix matrices removeColumns() and removeRows() won't throw if user specified a non-existing index. diff --git a/doc/source/doxygen-docs/example-hwdrivers_taobotics_imu.md b/doc/source/doxygen-docs/example-hwdrivers_taobotics_imu.md index a56c05e2b9..068d8b5f13 100644 --- a/doc/source/doxygen-docs/example-hwdrivers_taobotics_imu.md +++ b/doc/source/doxygen-docs/example-hwdrivers_taobotics_imu.md @@ -4,5 +4,7 @@ This example demonstrates the C++ API to read from Taobotics IMU sensors (e.g. R ![Screenshot](https://mrpt.github.io/imgs/screenshot_hwdrivers_taobotics_imu.jpg) +Short video: [https://www.youtube.com/shorts/qaaP9BmZYmo](https://www.youtube.com/shorts/qaaP9BmZYmo) + C++ example source code: \include hwdrivers_taobotics_imu/test.cpp diff --git a/doc/source/doxygen-docs/example-maps_voxelmap_from_tum_dataset.md b/doc/source/doxygen-docs/example-maps_voxelmap_from_tum_dataset.md new file mode 100644 index 0000000000..ecc279d781 --- /dev/null +++ b/doc/source/doxygen-docs/example-maps_voxelmap_from_tum_dataset.md @@ -0,0 +1,5 @@ +\page maps_voxelmap_from_tum_dataset Example: maps_voxelmap_from_tum_dataset + +![maps_voxelmap_from_tum_dataset screenshot](doc/source/images/maps_voxelmap_from_tum_dataset_screenshot.png) +C++ example source code: +\include maps_voxelmap_from_tum_dataset/test.cpp diff --git a/doc/source/doxygen-docs/example-maps_voxelmap_simple.md b/doc/source/doxygen-docs/example-maps_voxelmap_simple.md new file mode 100644 index 0000000000..9aebd3d0bb --- /dev/null +++ b/doc/source/doxygen-docs/example-maps_voxelmap_simple.md @@ -0,0 +1,5 @@ +\page maps_voxelmap_simple Example: maps_voxelmap_simple + +![maps_voxelmap_simple screenshot](doc/source/images/maps_voxelmap_simple_screenshot.png) +C++ example source code: +\include maps_voxelmap_simple/test.cpp diff --git a/doc/source/examples.rst b/doc/source/examples.rst index d1a51fc348..052a2efd8f 100644 --- a/doc/source/examples.rst +++ b/doc/source/examples.rst @@ -78,6 +78,8 @@ Python examples are `here `_. page_maps_observer_pattern_example.rst page_maps_octomap_simple.rst page_maps_ransac_data_association.rst + page_maps_voxelmap_simple.rst + page_maps_voxelmap_from_tum_dataset.rst page_math_csparse_example.rst page_math_kmeans_example.rst page_math_leastsquares_example.rst diff --git a/doc/source/images/maps_voxelmap_from_tum_dataset_screenshot.png b/doc/source/images/maps_voxelmap_from_tum_dataset_screenshot.png new file mode 100644 index 0000000000..b101a317af Binary files /dev/null and b/doc/source/images/maps_voxelmap_from_tum_dataset_screenshot.png differ diff --git a/doc/source/images/maps_voxelmap_simple_screenshot.png b/doc/source/images/maps_voxelmap_simple_screenshot.png new file mode 100644 index 0000000000..ab32859332 Binary files /dev/null and b/doc/source/images/maps_voxelmap_simple_screenshot.png differ diff --git a/libs/containers/include/mrpt/containers/NonCopiableData.h b/libs/containers/include/mrpt/containers/NonCopiableData.h index c341997b21..00ecb3ed0e 100644 --- a/libs/containers/include/mrpt/containers/NonCopiableData.h +++ b/libs/containers/include/mrpt/containers/NonCopiableData.h @@ -31,10 +31,10 @@ class NonCopiableData T data; NonCopiableData(const NonCopiableData&) {} - NonCopiableData& operator=(const NonCopiableData& o) { return *this; } + NonCopiableData& operator=(const NonCopiableData&) { return *this; } NonCopiableData(NonCopiableData&&) {} - NonCopiableData& operator=(NonCopiableData&& o) { return *this; } + NonCopiableData& operator=(NonCopiableData&&) { return *this; } }; } // namespace mrpt::containers diff --git a/libs/maps/include/mrpt/maps.h b/libs/maps/include/mrpt/maps.h index 7b5d6b5745..a634b82d44 100644 --- a/libs/maps/include/mrpt/maps.h +++ b/libs/maps/include/mrpt/maps.h @@ -32,6 +32,8 @@ MRPT_WARNING( #include #include #include +#include +#include #include #include diff --git a/libs/maps/include/mrpt/maps/CColouredOctoMap.h b/libs/maps/include/mrpt/maps/CColouredOctoMap.h index ec50f5cdc9..5db97db399 100644 --- a/libs/maps/include/mrpt/maps/CColouredOctoMap.h +++ b/libs/maps/include/mrpt/maps/CColouredOctoMap.h @@ -30,6 +30,8 @@ namespace maps * This version stores both, occupancy information and RGB colour data at * each octree node. See the base class mrpt::maps::COctoMapBase. * + * The octomap library was presented in \cite wurm2010octomap + * * \sa CMetricMap, the example in "MRPT/samples/octomap_simple" * \ingroup mrpt_maps_grp */ diff --git a/libs/maps/include/mrpt/maps/CMultiMetricMap.h b/libs/maps/include/mrpt/maps/CMultiMetricMap.h index 608aa6a312..b19e08f09c 100644 --- a/libs/maps/include/mrpt/maps/CMultiMetricMap.h +++ b/libs/maps/include/mrpt/maps/CMultiMetricMap.h @@ -34,6 +34,7 @@ class TSetOfMetricMapInitializers; * - mrpt::maps::COccupancyGridMap3D: 3D occupancy voxel map. * - mrpt::maps::COctoMap: For 3D occupancy grids of variable resolution, * with octrees (based on the library `octomap`). + * - mrpt::maps::CVoxelMap or mrpt::maps::CVoxelMapRGB: 3D sparse voxel maps. * - mrpt::maps::CColouredOctoMap: The same than above, but nodes can store * RGB data appart from occupancy. * - mrpt::maps::CLandmarksMap: For visual landmarks,etc... diff --git a/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h b/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h index 24d06e777e..0f81e67f09 100644 --- a/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h +++ b/libs/maps/include/mrpt/maps/COccupancyGridMap3D.h @@ -28,6 +28,9 @@ namespace mrpt::maps *certainly occupied, 1 means a certainly empty voxel. Initially 0.5 means *uncertainty. * + * An alternative, sparse representation of a 3D map is provided + * via mrpt::maps::CVoxelMap and mrpt::maps::CVoxelMapRGB + * * \ingroup mrpt_maps_grp **/ class COccupancyGridMap3D diff --git a/libs/maps/include/mrpt/maps/COctoMap.h b/libs/maps/include/mrpt/maps/COctoMap.h index 2dce8dfaf9..ddb34a1f08 100644 --- a/libs/maps/include/mrpt/maps/COctoMap.h +++ b/libs/maps/include/mrpt/maps/COctoMap.h @@ -33,6 +33,8 @@ namespace maps * This version only stores occupancy information at each octree node. See the * base class mrpt::maps::COctoMapBase. * + * The octomap library was presented in \cite wurm2010octomap + * * \sa CMetricMap, the example in "MRPT/samples/octomap_simple" * \ingroup mrpt_maps_grp */ @@ -49,13 +51,11 @@ class COctoMap : public COctoMapBase mrpt::opengl::COctoMapVoxels& gl_obj) const override; MAP_DEFINITION_START(COctoMap) - double resolution{ - 0.10}; //!< The finest resolution of the octomap (default: 0.10 - //! meters) - mrpt::maps::COctoMap::TInsertionOptions - insertionOpts; //!< Observations insertion options - mrpt::maps::COctoMap::TLikelihoodOptions - likelihoodOpts; //!< Probabilistic observation likelihood options + /// The finest resolution of the octomap (default: 0.10 meters) + double resolution = 0.10; + mrpt::maps::COctoMap::TInsertionOptions insertionOpts; + /// Probabilistic observation likelihood options + mrpt::maps::COctoMap::TLikelihoodOptions likelihoodOpts; MAP_DEFINITION_END(COctoMap) /** Returns true if the map is empty/no observation has been inserted */ diff --git a/libs/maps/include/mrpt/maps/COctoMapBase.h b/libs/maps/include/mrpt/maps/COctoMapBase.h index a9d023c327..7643925480 100644 --- a/libs/maps/include/mrpt/maps/COctoMapBase.h +++ b/libs/maps/include/mrpt/maps/COctoMapBase.h @@ -30,13 +30,7 @@ namespace mrpt::maps * To use octomap's iterators to go through the voxels, use * COctoMap::getOctomap() * - * The octomap library was presented in: - * - K. M. Wurm, A. Hornung, M. Bennewitz, C. Stachniss, and W. Burgard, - * "OctoMap: A Probabilistic, Flexible, and Compact 3D Map Representation - * for Robotic Systems" - * in Proc. of the ICRA 2010 Workshop on Best Practice in 3D Perception and - * Modeling for Mobile Manipulation, 2010. Software available at - * http://octomap.sf.net/. + * The octomap library was presented in \cite wurm2010octomap * * \sa CMetricMap, the example in "MRPT/samples/octomap_simple" * \ingroup mrpt_maps_grp diff --git a/libs/maps/include/mrpt/maps/CVoxelMap.h b/libs/maps/include/mrpt/maps/CVoxelMap.h new file mode 100644 index 0000000000..8f434e542e --- /dev/null +++ b/libs/maps/include/mrpt/maps/CVoxelMap.h @@ -0,0 +1,64 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#pragma once + +#include + +namespace mrpt::maps +{ +/** Voxel contents for CVoxelMap + */ +struct VoxelNodeOccupancy +{ + int8_t occupancy = 0; + + // ---- API expected by CVoxelMapOccupancyBase ---- + int8_t& occupancyRef() { return occupancy; } + const int8_t& occupancyRef() const { return occupancy; } +}; + +/** + * Log-odds sparse voxel map for cells containing only the *occupancy* of each + * voxel. + * + * \ingroup mrpt_maps_grp + */ +class CVoxelMap : public CVoxelMapOccupancyBase +{ + // This must be added to any CSerializable derived class: + DEFINE_SERIALIZABLE(CVoxelMap, mrpt::maps) + + public: + CVoxelMap( + double resolution = 0.05, uint8_t inner_bits = 2, uint8_t leaf_bits = 3) + : CVoxelMapOccupancyBase(resolution, inner_bits, leaf_bits) + { + } + ~CVoxelMap(); + + MAP_DEFINITION_START(CVoxelMap) + double resolution = 0.10; + uint8_t inner_bits = 2; + uint8_t leaf_bits = 3; + mrpt::maps::TVoxelMap_InsertionOptions insertionOpts; + mrpt::maps::TVoxelMap_LikelihoodOptions likelihoodOpts; + MAP_DEFINITION_END(CVoxelMap) + + protected: + bool internal_insertObservation( + const mrpt::obs::CObservation& obs, + const std::optional& robotPose = + std::nullopt) override; + double internal_computeObservationLikelihood( + const mrpt::obs::CObservation& obs, + const mrpt::poses::CPose3D& takenFrom) const override; +}; + +} // namespace mrpt::maps diff --git a/libs/maps/include/mrpt/maps/CVoxelMapBase.h b/libs/maps/include/mrpt/maps/CVoxelMapBase.h new file mode 100644 index 0000000000..1acbd16eb6 --- /dev/null +++ b/libs/maps/include/mrpt/maps/CVoxelMapBase.h @@ -0,0 +1,132 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#pragma once + +#include +#include +#include +#include +#include + +#include + +#include "bonxai/bonxai.hpp" + +namespace mrpt::maps +{ +/** An mrpt CMetricMap wrapper for Bonxai's VoxelMap container. + * + * Refer to Davide Faconti's [Bonxai + * repository](https://github.com/facontidavide/Bonxai) for publication and + * algorithm details, but in short, this is a sparse generic container for + * voxels, not needing redimensioning when the map grows, and with efficient + * insertion and update operations. + * + * Users normally use the derived classes, not this generic base template. + * This base class implements all common aspects to CMetricMap that do not + * depend on the specific contents to be stored at each voxel. + * + * No multi-threading protection is applied at all in the API. + * + * \sa CMetricMap, the example in "MRPT/samples/maps_voxelmap_simple", + * \ingroup mrpt_maps_grp + */ +template +class CVoxelMapBase : public mrpt::maps::CMetricMap +{ + public: + using myself_t = CVoxelMapBase; + using voxel_node_t = node_t; + + /** Constructor, defines the resolution of the voxelmap + * (length of each voxel side, in meters). + */ + CVoxelMapBase( + double resolution, uint8_t inner_bits = 2, uint8_t leaf_bits = 3) + : m_impl(std::make_unique(resolution, inner_bits, leaf_bits)) + { + } + virtual ~CVoxelMapBase() override = default; + + CVoxelMapBase(const CVoxelMapBase& o) : CVoxelMapBase(o.grid().resolution) + { + *this = o; + } + CVoxelMapBase& operator=(const CVoxelMapBase& o) + { + // grid() = o.grid(); + THROW_EXCEPTION("Bonxai voxel grid copy not implemented"); + return *this; + } + + CVoxelMapBase(CVoxelMapBase&& o) : m_impl(std::move(o.m_impl)) {} + CVoxelMapBase& operator=(CVoxelMapBase&& o) + { + m_impl = std::move(o.m_impl); + return *this; + } + + const Bonxai::VoxelGrid& grid() const { return m_impl->grid; } + + /** Returns a short description of the map. */ + std::string asString() const override { return "Voxelmap"; } + + /** Returns a 3D object representing the map. + * \sa renderingOptions + */ + void getVisualizationInto(mrpt::opengl::CSetOfObjects& o) const override + { + auto gl_obj = mrpt::opengl::COctoMapVoxels::Create(); + this->getAsOctoMapVoxels(*gl_obj); + o.insert(gl_obj); + } + + /** Builds a renderizable representation of the octomap as a + * mrpt::opengl::COctoMapVoxels object. + * Implementation defined for each children class. + * \sa renderingOptions + */ + virtual void getAsOctoMapVoxels( + mrpt::opengl::COctoMapVoxels& gl_obj) const = 0; + + virtual void saveMetricMapRepresentationToFile( + const std::string& filNamePrefix) const override + { + MRPT_START + // Save as 3D Scene: + { + mrpt::opengl::Scene scene; + scene.insert(this->getVisualization()); + const std::string fil = filNamePrefix + std::string("_3D.3Dscene"); + scene.saveToFile(fil); + } + // Save binary data file? + MRPT_END + } + + protected: + struct Impl + { + Impl(double resolution, uint8_t inner_bits, uint8_t leaf_bits) + : grid(resolution, inner_bits, leaf_bits), + accessor(grid.createAccessor()) + { + } + Impl(Bonxai::VoxelGrid&& g) + : grid(std::move(g)), accessor(grid.createAccessor()) + { + } + Bonxai::VoxelGrid grid; + mutable typename Bonxai::VoxelGrid::Accessor accessor; + }; + std::unique_ptr m_impl; +}; + +} // namespace mrpt::maps diff --git a/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h b/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h new file mode 100644 index 0000000000..dad12a161b --- /dev/null +++ b/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h @@ -0,0 +1,631 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace mrpt::maps +{ +struct TVoxelMap_InsertionOptions : public mrpt::config::CLoadableOptions +{ + TVoxelMap_InsertionOptions() = default; + + double max_range = -1; //!< Maximum insertion ray range (<0: none) + + double prob_miss = 0.45; + double prob_hit = 0.65; + double clamp_min = 0.10; + double clamp_max = 0.95; + + bool ray_trace_free_space = true; + uint32_t decimation = 1; + + // See base docs + void loadFromConfigFile( + const mrpt::config::CConfigFileBase& source, + const std::string& section) override; + void saveToConfigFile( + mrpt::config::CConfigFileBase& c, const std::string& s) const override; + + void writeToStream(mrpt::serialization::CArchive& out) const; + void readFromStream(mrpt::serialization::CArchive& in); +}; + +/** Options used when evaluating "computeObservationLikelihood" + * \sa CObservation::computeObservationLikelihood + */ +struct TVoxelMap_LikelihoodOptions : public mrpt::config::CLoadableOptions +{ + TVoxelMap_LikelihoodOptions() = default; + ~TVoxelMap_LikelihoodOptions() override = default; + + // See base docs + void loadFromConfigFile( + const mrpt::config::CConfigFileBase& source, + const std::string& section) override; + void saveToConfigFile( + mrpt::config::CConfigFileBase& c, const std::string& s) const override; + + void writeToStream(mrpt::serialization::CArchive& out) const; + void readFromStream(mrpt::serialization::CArchive& in); + + /// Speed up the likelihood computation by considering only one out of N + /// rays (default=1) + uint32_t decimation = 1; + + /// Minimum occupancy (0,1) for a voxel to be considered occupied. + double occupiedThreshold = 0.60; +}; + +/** Options for the conversion of a mrpt::maps::COctoMap into a + * mrpt::opengl::COctoMapVoxels */ +struct TVoxelMap_RenderingOptions +{ + TVoxelMap_RenderingOptions() = default; + + bool generateOccupiedVoxels = true; + double occupiedThreshold = 0.60; + bool visibleOccupiedVoxels = true; + + bool generateFreeVoxels = true; + double freeThreshold = 0.40; + bool visibleFreeVoxels = true; + + /** Binary dump to stream */ + void writeToStream(mrpt::serialization::CArchive& out) const; + /** Binary dump to stream */ + void readFromStream(mrpt::serialization::CArchive& in); +}; + +namespace internal +{ +template +struct has_color : std::false_type +{ +}; +template +struct has_color> : std::true_type +{ +}; +} // namespace internal + +/** + * Base class for log-odds sparse voxel map for cells containing occupancy, + * and possibly other information, for each voxel. + * + * \sa Use derived classes CVoxelMap, CVoxelMapRGB + * + * \ingroup mrpt_maps_grp + */ +template +class CVoxelMapOccupancyBase : public CVoxelMapBase, + public detail::logoddscell_traits +{ + protected: + using occupancy_value_t = occupancy_t; + using traits_t = detail::logoddscell_traits; + using base_t = CVoxelMapBase; + + public: + CVoxelMapOccupancyBase( + double resolution = 0.05, uint8_t inner_bits = 2, uint8_t leaf_bits = 3) + : CVoxelMapBase(resolution, inner_bits, leaf_bits) + { + } + virtual ~CVoxelMapOccupancyBase() = default; + + bool isEmpty() const override + { + return base_t::m_impl->grid.activeCellsCount() == 0; + } + + void getAsOctoMapVoxels( + mrpt::opengl::COctoMapVoxels& gl_obj) const override; + + /** Manually updates the occupancy of the voxel at (x,y,z) as being occupied + * (true) or free (false), using the log-odds parameters in \a + * insertionOptions */ + void updateVoxel( + const double x, const double y, const double z, bool occupied); + + /** Get the occupancy probability [0,1] of a point + * \return false if the point is not mapped, in which case the returned + * "prob" is undefined. */ + bool getPointOccupancy( + const double x, const double y, const double z, + double& prob_occupancy) const; + + void insertPointCloudAsRays( + const mrpt::maps::CPointsMap& pts, + const mrpt::math::TPoint3D& sensorPt); + + void insertPointCloudAsEndPoints( + const mrpt::maps::CPointsMap& pts, + const mrpt::math::TPoint3D& sensorPt); + + /** Returns all occupied voxels as a point cloud. The shared_ptr is + * also hold and updated internally, so it is not safe to read it + * while also updating the voxel map in another thread. + * + * The point cloud is cached, and invalidated upon map updates. + */ + mrpt::maps::CSimplePointsMap::Ptr getOccupiedVoxels() const; + + /// The options used when inserting observations in the map: + TVoxelMap_InsertionOptions insertionOptions; + + TVoxelMap_LikelihoodOptions likelihoodOptions; + + TVoxelMap_RenderingOptions renderingOptions; + + /** Performs Bayesian fusion of a new observation of a cell. + * This method increases the "occupancy-ness" of a cell, managing possible + * saturation. + * \param theCell The cell to modify + * \param logodd_obs Observation of the cell, in log-odd form as + * transformed by p2l. + * \param thres This must be CELLTYPE_MIN+logodd_obs + * \sa updateCell, updateCell_fast_free + */ + inline void updateCell_fast_occupied( + voxel_node_t* theCell, const occupancy_t logodd_obs, + const occupancy_t thres) + { + if (theCell == nullptr) return; + occupancy_t& occ = theCell->occupancyRef(); + if (occ > thres) occ -= logodd_obs; + else + occ = traits_t::CELLTYPE_MIN; + } + + /** Performs Bayesian fusion of a new observation of a cell. + * This method increases the "occupancy-ness" of a cell, managing possible + * saturation. + * \param coord Cell indexes. + * \param logodd_obs Observation of the cell, in log-odd form as + * transformed by p2l. + * \param thres This must be CELLTYPE_MIN+logodd_obs + * \sa updateCell, updateCell_fast_free + */ + inline void updateCell_fast_occupied( + const Bonxai::CoordT& coord, const occupancy_t logodd_obs, + const occupancy_t thres) + { + if (voxel_node_t* cell = + base_t::m_impl->accessor.value(coord, true /*create*/); + cell) + updateCell_fast_occupied(cell, logodd_obs, thres); + } + + /** Performs Bayesian fusion of a new observation of a cell. + * This method increases the "free-ness" of a cell, managing possible + * saturation. + * \param logodd_obs Observation of the cell, in log-odd form as + * transformed by p2l. + * \param thres This must be CELLTYPE_MAX-logodd_obs + * \sa updateCell_fast_occupied + */ + inline void updateCell_fast_free( + voxel_node_t* theCell, const occupancy_t logodd_obs, + const occupancy_t thres) + { + if (theCell == nullptr) return; + occupancy_t& occ = theCell->occupancyRef(); + if (occ < thres) occ += logodd_obs; + else + occ = traits_t::CELLTYPE_MAX; + } + + /** Performs the Bayesian fusion of a new observation of a cell. + * This method increases the "free-ness" of a cell, managing possible + * saturation. + * \param coord Cell indexes. + * \param logodd_obs Observation of the cell, in log-odd form as + * transformed by p2l. + * \param thres This must be CELLTYPE_MAX-logodd_obs + * \sa updateCell_fast_occupied + */ + inline void updateCell_fast_free( + const Bonxai::CoordT& coord, const occupancy_t logodd_obs, + const occupancy_t thres) + { + if (voxel_node_t* cell = + base_t::m_impl->accessor.value(coord, true /*create*/); + cell) + updateCell_fast_free(cell, logodd_obs, thres); + } + + /** Lookup tables for log-odds */ + static CLogOddsGridMapLUT& get_logodd_lut() + { + // Static lookup tables for log-odds + static CLogOddsGridMapLUT logodd_lut; + return logodd_lut; + } + + /** Scales an integer representation of the log-odd into a real valued + * probability in [0,1], using p=exp(l)/(1+exp(l)) */ + static inline float l2p(const occupancy_value_t l) + { + return get_logodd_lut().l2p(l); + } + + /** Scales an integer representation of the log-odd into a linear scale + * [0,255], using p=exp(l)/(1+exp(l)) */ + static inline uint8_t l2p_255(const occupancy_value_t l) + { + return get_logodd_lut().l2p_255(l); + } + /** Scales a real valued probability in [0,1] to an integer representation + * of: log(p)-log(1-p) in the valid range of voxel_node_t */ + static inline occupancy_value_t p2l(const float p) + { + return get_logodd_lut().p2l(p); + } + + protected: + void internal_clear() override; + + void invalidateOccupiedCache() { m_cachedOccupied.reset(); } + + void updateOccupiedPointsCache() const; + mutable mrpt::maps::CSimplePointsMap::Ptr m_cachedOccupied; +}; + +// ============= Implementations =============== +template +void CVoxelMapOccupancyBase::getAsOctoMapVoxels( + mrpt::opengl::COctoMapVoxels& gl_obj) const +{ + using mrpt::opengl::COctoMapVoxels; + using mrpt::opengl::VOXEL_SET_FREESPACE; + using mrpt::opengl::VOXEL_SET_OCCUPIED; + + const mrpt::img::TColorf general_color = gl_obj.getColor(); + const mrpt::img::TColor general_color_u = general_color.asTColor(); + + gl_obj.clear(); + gl_obj.resizeVoxelSets(2); // 2 sets of voxels: occupied & free + + gl_obj.showVoxels( + mrpt::opengl::VOXEL_SET_OCCUPIED, + renderingOptions.visibleOccupiedVoxels); + gl_obj.showVoxels( + mrpt::opengl::VOXEL_SET_FREESPACE, renderingOptions.visibleFreeVoxels); + + const size_t nLeafs = base_t::m_impl->grid.activeCellsCount(); + gl_obj.reserveVoxels(VOXEL_SET_OCCUPIED, nLeafs); + + mrpt::math::TBoundingBox bbox = + mrpt::math::TBoundingBox::PlusMinusInfinity(); + + // forEachCell() has no const version + auto& grid = + const_cast&>(base_t::m_impl->grid); + + // Go thru all voxels: + auto lmbdPerVoxel = [this, &bbox, &grid, &gl_obj, general_color_u, + general_color]( + voxel_node_t& data, const Bonxai::CoordT& coord) { + using mrpt::img::TColor; + + // log-odds to probability: + const double occ = 1.0 - this->l2p(data.occupancyRef()); + const auto pt = Bonxai::CoordToPos(coord, grid.resolution); + bbox.updateWithPoint({pt.x, pt.y, pt.z}); + + if ((occ >= renderingOptions.occupiedThreshold && + renderingOptions.generateOccupiedVoxels) || + (occ < renderingOptions.freeThreshold && + renderingOptions.generateFreeVoxels)) + { + mrpt::img::TColor vx_color; + double coefc, coeft; + switch (gl_obj.getVisualizationMode()) + { + case COctoMapVoxels::FIXED: vx_color = general_color_u; break; + + case COctoMapVoxels::COLOR_FROM_HEIGHT: // not supported + THROW_EXCEPTION( + "COLOR_FROM_HEIGHT not supported yet for this " + "class"); + break; + + case COctoMapVoxels::COLOR_FROM_OCCUPANCY: + coefc = 240 * (1 - occ) + 15; + vx_color = TColor( + coefc * general_color.R, coefc * general_color.G, + coefc * general_color.B, 255.0 * general_color.A); + break; + + case COctoMapVoxels::TRANSPARENCY_FROM_OCCUPANCY: + coeft = 255 - 510 * (1 - occ); + if (coeft < 0) { coeft = 0; } + vx_color = TColor( + 255 * general_color.R, 255 * general_color.G, + 255 * general_color.B, coeft); + break; + + case COctoMapVoxels::TRANS_AND_COLOR_FROM_OCCUPANCY: + coefc = 240 * (1 - occ) + 15; + vx_color = TColor( + coefc * general_color.R, coefc * general_color.G, + coefc * general_color.B, 50); + break; + + case COctoMapVoxels::MIXED: + THROW_EXCEPTION("MIXED not supported yet for this class"); + break; + + case COctoMapVoxels::COLOR_FROM_RGB_DATA: + if constexpr (internal::has_color::value) + { + vx_color.R = data.color.R; + vx_color.G = data.color.G; + vx_color.B = data.color.B; + } + else + { + THROW_EXCEPTION( + "COLOR_FROM_RGB_DATA used with unsupported voxel " + "data type"); + } + break; + + default: THROW_EXCEPTION("Unknown coloring scheme!"); + } + + const size_t vx_set = (occ > renderingOptions.occupiedThreshold) + ? VOXEL_SET_OCCUPIED + : VOXEL_SET_FREESPACE; + + gl_obj.push_back_Voxel( + vx_set, + COctoMapVoxels::TVoxel( + mrpt::math::TPoint3Df(pt.x, pt.y, pt.z), grid.resolution, + vx_color)); + } + }; // end lambda for each voxel + + grid.forEachCell(lmbdPerVoxel); + + // if we use transparency, sort cubes by "Z" as an approximation to + // far-to-near render ordering: + if (gl_obj.isCubeTransparencyEnabled()) gl_obj.sort_voxels_by_z(); + + // Set bounding box: + gl_obj.setBoundingBox(bbox.min, bbox.max); +} + +template +void CVoxelMapOccupancyBase::internal_clear() +{ + // Is this enough? + base_t::m_impl->grid.root_map.clear(); +} + +template +void CVoxelMapOccupancyBase::updateVoxel( + const double x, const double y, const double z, bool occupied) +{ + invalidateOccupiedCache(); + + voxel_node_t* cell = base_t::m_impl->accessor.value( + Bonxai::PosToCoord({x, y, z}, base_t::m_impl->grid.inv_resolution), + true /*create*/); + if (!cell) return; // should never happen? + + if (occupied) + { + const occupancy_t logodd_observation_occupied = + std::max(1, p2l(insertionOptions.prob_hit)); + const occupancy_t logodd_thres_occupied = + p2l(1.0 - insertionOptions.clamp_max); + + updateCell_fast_occupied( + cell, logodd_observation_occupied, logodd_thres_occupied); + } + else + { + const occupancy_t logodd_observation_free = + std::max(1, p2l(insertionOptions.prob_miss)); + const occupancy_t logodd_thres_free = + p2l(1.0 - insertionOptions.clamp_min); + + updateCell_fast_free(cell, logodd_observation_free, logodd_thres_free); + } +} +template +bool CVoxelMapOccupancyBase::getPointOccupancy( + const double x, const double y, const double z, + double& prob_occupancy) const +{ + voxel_node_t* cell = base_t::m_impl->accessor.value( + Bonxai::PosToCoord({x, y, z}, base_t::m_impl->grid.inv_resolution), + false /*create*/); + + if (!cell) return false; + + prob_occupancy = 1.0 - l2p(cell->occupancyRef()); + return true; +} + +template +void CVoxelMapOccupancyBase::insertPointCloudAsRays( + const mrpt::maps::CPointsMap& pts, const mrpt::math::TPoint3D& sensorPt) +{ + invalidateOccupiedCache(); + + const occupancy_t logodd_observation_occupied = + std::max(1, p2l(insertionOptions.prob_hit)); + const occupancy_t logodd_thres_occupied = + p2l(1.0 - insertionOptions.clamp_max); + + const auto& xs = pts.getPointsBufferRef_x(); + const auto& ys = pts.getPointsBufferRef_y(); + const auto& zs = pts.getPointsBufferRef_z(); + + const auto maxSqrDist = mrpt::square(insertionOptions.max_range); + + // Starting cell index at sensor pose: + Bonxai::CoordT sensorCoord = Bonxai::PosToCoord( + {sensorPt.x, sensorPt.y, sensorPt.z}, + base_t::m_impl->grid.inv_resolution); + + // Use fixed comma for the ray tracing direction: + constexpr unsigned int FRBITS = 9; + + const occupancy_t logodd_observation_free = + std::max(1, p2l(insertionOptions.prob_miss)); + const occupancy_t logodd_thres_free = p2l(1.0 - insertionOptions.clamp_min); + + // for each ray: + for (size_t i = 0; i < xs.size(); i += insertionOptions.decimation) + { + if (insertionOptions.max_range > 0 && + mrpt::math::TPoint3D(xs[i], ys[i], zs[i]).sqrNorm() > maxSqrDist) + continue; // skip + + const Bonxai::CoordT endCoord = Bonxai::PosToCoord( + {xs[i], ys[i], zs[i]}, base_t::m_impl->grid.inv_resolution); + + // jump in discrete steps from sensorCoord to endCoord: + // Use "fractional integers" to approximate float operations + // during the ray tracing: + const Bonxai::CoordT Ac = endCoord - sensorCoord; + + uint32_t Acx_ = std::abs(Ac.x); + uint32_t Acy_ = std::abs(Ac.y); + uint32_t Acz_ = std::abs(Ac.z); + + const auto nStepsRay = std::max(Acx_, std::max(Acy_, Acz_)); + if (!nStepsRay) continue; // May be... + + // Integers store "float values * 128" + float N_1 = 1.0f / nStepsRay; // Avoid division twice. + + // Increments at each raytracing step: + int frAcx = (Ac.x < 0 ? -1 : +1) * round((Acx_ << FRBITS) * N_1); + int frAcy = (Ac.y < 0 ? -1 : +1) * round((Acy_ << FRBITS) * N_1); + int frAcz = (Ac.z < 0 ? -1 : +1) * round((Acz_ << FRBITS) * N_1); + + int frCX = sensorCoord.x << FRBITS; + int frCY = sensorCoord.y << FRBITS; + int frCZ = sensorCoord.z << FRBITS; + + // free space ray: + for (unsigned int nStep = 0; nStep < nStepsRay; nStep++) + { + if (voxel_node_t* cell = base_t::m_impl->accessor.value( + {frCX >> FRBITS, frCY >> FRBITS, frCZ >> FRBITS}, + true /*create*/); + cell) + { + updateCell_fast_free( + cell, logodd_observation_free, logodd_thres_free); + } + + frCX += frAcx; + frCY += frAcy; + frCZ += frAcz; + } + + // and occupied end point: + if (voxel_node_t* cell = + base_t::m_impl->accessor.value(endCoord, true /*create*/); + cell) + { + updateCell_fast_occupied( + cell, logodd_observation_occupied, logodd_thres_occupied); + } + } // for each point/ray +} + +template +void CVoxelMapOccupancyBase:: + insertPointCloudAsEndPoints( + const mrpt::maps::CPointsMap& pts, const mrpt::math::TPoint3D& sensorPt) +{ + invalidateOccupiedCache(); + + const occupancy_t logodd_observation_occupied = + std::max(1, p2l(insertionOptions.prob_hit)); + const occupancy_t logodd_thres_occupied = + p2l(1.0 - insertionOptions.clamp_max); + + const auto& xs = pts.getPointsBufferRef_x(); + const auto& ys = pts.getPointsBufferRef_y(); + const auto& zs = pts.getPointsBufferRef_z(); + + const auto maxSqrDist = mrpt::square(insertionOptions.max_range); + + for (size_t i = 0; i < xs.size(); i += insertionOptions.decimation) + { + if (insertionOptions.max_range > 0 && + (mrpt::math::TPoint3D(xs[i], ys[i], zs[i]) - sensorPt).sqrNorm() > + maxSqrDist) + continue; // skip + + voxel_node_t* cell = base_t::m_impl->accessor.value( + Bonxai::PosToCoord( + {xs[i], ys[i], zs[i]}, base_t::m_impl->grid.inv_resolution), + true /*create*/); + if (!cell) continue; // should never happen? + + updateCell_fast_occupied( + cell, logodd_observation_occupied, logodd_thres_occupied); + } +} + +template +void CVoxelMapOccupancyBase< + voxel_node_t, occupancy_t>::updateOccupiedPointsCache() const +{ + if (m_cachedOccupied) return; // done + + m_cachedOccupied = mrpt::maps::CSimplePointsMap::Create(); + + // forEachCell() has no const version + auto& grid = + const_cast&>(base_t::m_impl->grid); + + // Go thru all voxels: + auto lmbdPerVoxel = [this, &grid]( + voxel_node_t& data, const Bonxai::CoordT& coord) { + using mrpt::img::TColor; + + // log-odds to probability: + const double occFreeness = this->l2p(data.occupancyRef()); + const auto pt = Bonxai::CoordToPos(coord, grid.resolution); + + if (occFreeness < 0.5) + { + m_cachedOccupied->insertPointFast(pt.x, pt.y, pt.z); + } + }; // end lambda for each voxel + + grid.forEachCell(lmbdPerVoxel); +} + +template +mrpt::maps::CSimplePointsMap::Ptr + CVoxelMapOccupancyBase::getOccupiedVoxels() const +{ + updateOccupiedPointsCache(); + return m_cachedOccupied; +} + +} // namespace mrpt::maps diff --git a/libs/maps/include/mrpt/maps/CVoxelMapRGB.h b/libs/maps/include/mrpt/maps/CVoxelMapRGB.h new file mode 100644 index 0000000000..1273fc7df2 --- /dev/null +++ b/libs/maps/include/mrpt/maps/CVoxelMapRGB.h @@ -0,0 +1,80 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#pragma once + +#include +#include + +namespace mrpt::maps +{ +/** Voxel contents for CVoxelMapRGB + */ +struct VoxelNodeOccRGB +{ + int8_t occupancy = 0; + struct TColor + { + uint8_t R = 0, G = 0, B = 0; + } color; + uint32_t numColObs = 0; + + // ---- API expected by CVoxelMapOccupancyBase ---- + int8_t& occupancyRef() { return occupancy; } + const int8_t& occupancyRef() const { return occupancy; } +}; + +/** + * Log-odds sparse voxel map for cells containing the occupancy *and* an RGB + * color for each voxel. + * + * \ingroup mrpt_maps_grp + */ +class CVoxelMapRGB : public CVoxelMapOccupancyBase +{ + // This must be added to any CSerializable derived class: + DEFINE_SERIALIZABLE(CVoxelMapRGB, mrpt::maps) + + public: + CVoxelMapRGB( + double resolution = 0.05, uint8_t inner_bits = 2, uint8_t leaf_bits = 3) + : CVoxelMapOccupancyBase(resolution, inner_bits, leaf_bits) + { + } + ~CVoxelMapRGB(); + + MAP_DEFINITION_START(CVoxelMapRGB) + double resolution = 0.10; + uint8_t inner_bits = 2; + uint8_t leaf_bits = 3; + mrpt::maps::TVoxelMap_InsertionOptions insertionOpts; + mrpt::maps::TVoxelMap_LikelihoodOptions likelihoodOpts; + MAP_DEFINITION_END(CVoxelMapRGB) + + protected: + bool internal_insertObservation( + const mrpt::obs::CObservation& obs, + const std::optional& robotPose = + std::nullopt) override; + + bool internal_insertObservation_3DScan( + const mrpt::obs::CObservation3DRangeScan& obs, + const std::optional& robotPose = + std::nullopt); + bool internal_insertObservation_default( + const mrpt::obs::CObservation& obs, + const std::optional& robotPose = + std::nullopt); + + double internal_computeObservationLikelihood( + const mrpt::obs::CObservation& obs, + const mrpt::poses::CPose3D& takenFrom) const override; +}; + +} // namespace mrpt::maps diff --git a/libs/maps/include/mrpt/maps/bonxai/.clangformat b/libs/maps/include/mrpt/maps/bonxai/.clangformat new file mode 100644 index 0000000000..445f63ed30 --- /dev/null +++ b/libs/maps/include/mrpt/maps/bonxai/.clangformat @@ -0,0 +1,3 @@ +DisableFormat: true +SortIncludes: Never + diff --git a/libs/maps/include/mrpt/maps/bonxai/bonxai.hpp b/libs/maps/include/mrpt/maps/bonxai/bonxai.hpp new file mode 100644 index 0000000000..63a117e164 --- /dev/null +++ b/libs/maps/include/mrpt/maps/bonxai/bonxai.hpp @@ -0,0 +1,1151 @@ +/* + * Copyright Contributors to the Bonxai Project + * Copyright Contributors to the OpenVDB Project + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at https://mozilla.org/MPL/2.0/. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace Bonxai +{ + +// Magically converts any representation of a point in 3D +// (type with x, y and z) to another one. Works with: +// +// - pcl::PointXYZ, pcl::PointXYZI, pcl::PointXYZRGB, etc +// - Eigen::Vector3d, Eigen::Vector3f +// - custom type with x,y,z fields. In this case Bonxai::Point3D +// - arrays or vectors with 3 elements. + +template +PointOut ConvertPoint(const PointIn& v); + +struct Point3D +{ + double x; + double y; + double z; + + Point3D() = default; + + Point3D(const Point3D& v) = default; + Point3D(Point3D&& v) = default; + + Point3D& operator=(const Point3D& v) = default; + Point3D& operator=(Point3D&& v) = default; + + Point3D(double x, double y, double z); + + template + Point3D(const T& v) + { + *this = ConvertPoint(v); + } + + template + Point3D& operator=(const T& v) + { + *this = ConvertPoint(v); + return *this; + } + + // Access to x, y, z, using index 0, 1, 2 + [[nodiscard]] double& operator[](size_t index); +}; + +struct CoordT +{ + int32_t x; + int32_t y; + int32_t z; + + // Access to x, y, z, using index 0, 1, 2 + [[nodiscard]] int32_t& operator[](size_t index); + + [[nodiscard]] bool operator==(const CoordT& other) const; + [[nodiscard]] bool operator!=(const CoordT& other) const; + + [[nodiscard]] CoordT operator+(const CoordT& other) const; + [[nodiscard]] CoordT operator-(const CoordT& other) const; + + CoordT& operator+=(const CoordT& other); + CoordT& operator-=(const CoordT& other); +}; + +[[nodiscard]] inline CoordT PosToCoord(const Point3D& point, double inv_resolution) +{ + return { int32_t(point.x * inv_resolution) - std::signbit(point.x), + int32_t(point.y * inv_resolution) - std::signbit(point.y), + int32_t(point.z * inv_resolution) - std::signbit(point.z) }; +} + +[[nodiscard]] inline Point3D CoordToPos(const CoordT& coord, double resolution) +{ + return { (double(coord.x) + 0.5) * resolution, + (double(coord.y) + 0.5) * resolution, + (double(coord.z) + 0.5) * resolution }; +} + +//---------------------------------------------------------- + +/// Bit-mask to encode active states and facilitate sequential iterators. +class Mask +{ + uint64_t* words_ = nullptr; + // small object optimization, that will be used when + // SIZE <= 512 bits, i.e LOG2DIM <= 3 + uint64_t static_words_[8]; + +public: + // Number of bits in mask + const uint32_t SIZE; + // Number of 64 bit words + const uint32_t WORD_COUNT; + + /// Initialize all bits to zero. + Mask(size_t log2dim); + /// Initialize all bits to a given value. + Mask(size_t log2dim, bool on); + + Mask(const Mask& other); + Mask(Mask&& other); + + ~Mask(); + + /// Return the memory footprint in bytes of this Mask + size_t memUsage() const; + + /// Return the number of bits available in this Mask + uint32_t bitCount() const { return SIZE; } + + /// Return the number of machine words used by this Mask + uint32_t wordCount() const { return WORD_COUNT; } + + uint64_t getWord(size_t n) const { return words_[n]; } + + void setWord(size_t n, uint64_t v) { words_[n] = v; } + + uint32_t countOn() const; + + class Iterator + { + public: + Iterator(const Mask* parent) + : pos_(parent->SIZE) + , parent_(parent) + {} + Iterator(uint32_t pos, const Mask* parent) + : pos_(pos) + , parent_(parent) + {} + Iterator& operator=(const Iterator&) = default; + + uint32_t operator*() const { return pos_; } + + operator bool() const { return pos_ != parent_->SIZE; } + + Iterator& operator++() + { + pos_ = parent_->findNextOn(pos_ + 1); + return *this; + } + + private: + uint32_t pos_; + const Mask* parent_; + }; + + bool operator==(const Mask& other) const; + + bool operator!=(const Mask& other) const { return !((*this) == other); } + + Iterator beginOn() const { return Iterator(this->findFirstOn(), this); } + + /// Return true if the given bit is set. + bool isOn(uint32_t n) const; + /// Return true if any bit is set. + bool isOn() const; + + bool isOff() const; + + bool setOn(uint32_t n); + + bool setOff(uint32_t n); + + void set(uint32_t n, bool On); + + /// Set all bits on + void setOn(); + + /// Set all bits off + void setOff(); + + /// Set all bits too the value "on" + void set(bool on); + + /// Toggle the state of all bits in the mask + void toggle(); + /// Toggle the state of one bit in the mask + void toggle(uint32_t n); + +private: + uint32_t findFirstOn() const; + uint32_t findNextOn(uint32_t start) const; + + static uint32_t FindLowestOn(uint64_t v); + static uint32_t CountOn(uint64_t v); +}; + +//---------------------------------------------------------- +/** + * @brief The Grid class is used to store data in a + * cube. The size (DIM) of the cube can only be a power of 2 + * + * For instance, given Grid(3), + * DIM will be 8 and SIZE 512 (8³) + */ +template +class Grid +{ +private: + uint8_t dim_; + // total number of elements in the cube + uint32_t size_; + + DataT* data_ = nullptr; + Bonxai::Mask mask_; + +public: + Grid(size_t log2dim) + : dim_(1 << log2dim) + , size_(dim_ * dim_ * dim_) + , mask_(log2dim) + { + data_ = new DataT[size_]; + } + + Grid(const Grid& other) = delete; + Grid& operator=(const Grid& other) = delete; + + Grid(Grid&& other); + Grid& operator=(Grid&& other); + + ~Grid(); + + [[nodiscard]] size_t memUsage() const; + + [[nodiscard]] size_t size() const { return size_; } + + [[nodiscard]] Bonxai::Mask& mask() { return mask_; }; + + [[nodiscard]] const Bonxai::Mask& mask() const { return mask_; }; + + [[nodiscard]] DataT& cell(size_t index) { return data_[index]; }; + + [[nodiscard]] const DataT& cell(size_t index) const { return data_[index]; }; +}; + +//---------------------------------------------------------- +template +class VoxelGrid +{ +public: + const uint32_t INNER_BITS; + const uint32_t LEAF_BITS; + const uint32_t Log2N; + const double resolution; + const double inv_resolution; + const uint32_t INNER_MASK; + const uint32_t LEAF_MASK; + + using LeafGrid = Grid; + using InnerGrid = Grid>; + using RootMap = std::unordered_map; + + RootMap root_map; + + /** + * @brief VoxelGrid constructor + * + * @param voxel_size dimension of the voxel. Used to convert between Point3D and + * CoordT + */ + VoxelGrid(double voxel_size, uint8_t inner_bits = 2, uint8_t leaf_bits = 3); + + /// @brief getMemoryUsage returns the amount of bytes used by this data structure + [[nodiscard]] size_t memUsage() const; + + /// @brief Return the total number of active cells + [[nodiscard]] size_t activeCellsCount() const; + + /// @brief posToCoord is used to convert real coordinates to CoordT indexes. + [[nodiscard]] CoordT posToCoord(double x, double y, double z); + + /// @brief posToCoord is used to convert real coordinates to CoordT indexes. + [[nodiscard]] CoordT posToCoord(const Point3D& pos) + { + return posToCoord(pos.x, pos.y, pos.z); + } + + /// @brief coordToPos converts CoordT indexes to Point3D. + [[nodiscard]] Point3D coordToPos(const CoordT& coord); + + /** + * @brief forEachCell apply a function of type: + * + * void(DataT*, const CoordT&) + * + * to each active element of the grid. + */ + template + void forEachCell(VisitorFunction func); + + /** Class to be used to set and get values of a cell of the Grid. + * It uses caching to speed up computation. + * + * Create an instance of this object with the method VoxelGrid::greateAccessor() + */ + class Accessor + { + public: + Accessor(VoxelGrid& grid) + : grid_(grid) + {} + + /** + * @brief setValue of a cell. If the cell did not exist, it is created. + * + * @param coord coordinate of the cell + * @param value value to set. + * @return the previous state of the cell (ON = true). + */ + bool setValue(const CoordT& coord, const DataT& value); + + /** @brief value getter. + * + * @param coord coordinate of the cell. + * @return return the pointer to the value or nullptr if not set. + */ + [[nodiscard]] DataT* value(const CoordT& coord, bool create_if_missing = false); + + /** @brief setCellOn is similar to setValue, but the value is changed only if the + * cell has been created, otherwise, the previous value is used. + * + * @param coord coordinate of the cell. + * @param default_value default value of the cell. Use only if the cell did not + * exist before. + * @return the previous state of the cell (ON = true). + */ + bool setCellOn(const CoordT& coord, const DataT& default_value = DataT()); + + /** @brief setCellOff will disable a cell without deleting its content. + * + * @param coord coordinate of the cell. + * @return the previous state of the cell (ON = true). + */ + bool setCellOff(const CoordT& coord); + + /// @brief lastInnerdGrid returns the pointer to the InnerGrid in the cache. + [[nodiscard]] const InnerGrid* lastInnerdGrid() const { return prev_inner_ptr_; } + + /// @brief lastLeafGrid returns the pointer to the LeafGrid in the cache. + [[nodiscard]] const LeafGrid* lastLeafGrid() const { return prev_leaf_ptr_; } + + /** + * @brief getLeafGrid gets the pointer to the LeafGrid containing the cell. + * It is the basic class used by setValue() and value(). + * + * @param coord Coordinate of the cell. + * @param create_if_missing if true, create the Root, Inner and Leaf, if not + * present. + */ + [[nodiscard]] LeafGrid* getLeafGrid(const CoordT& coord, + bool create_if_missing = false); + + private: + VoxelGrid& grid_; + CoordT prev_root_coord_ = { std::numeric_limits::max(), 0, 0 }; + CoordT prev_inner_coord_ = { std::numeric_limits::max(), 0, 0 }; + InnerGrid* prev_inner_ptr_ = nullptr; + LeafGrid* prev_leaf_ptr_ = nullptr; + }; + + Accessor createAccessor() { return Accessor(*this); } + + [[nodiscard]] CoordT getRootKey(const CoordT& coord); + + [[nodiscard]] CoordT getInnerKey(const CoordT& coord); + + [[nodiscard]] uint32_t getInnerIndex(const CoordT& coord); + + [[nodiscard]] uint32_t getLeafIndex(const CoordT& coord); +}; + +//---------------------------------------------------- +//----------------- Implementations ------------------ +//---------------------------------------------------- + +inline Point3D::Point3D(double _x, double _y, double _z) + : x(_x) + , y(_y) + , z(_z) +{} + +inline double& Point3D::operator[](size_t index) +{ + switch (index) + { + case 0: + return x; + case 1: + return y; + case 2: + return z; + default: + throw std::runtime_error("out of bound index"); + } +} + +// clang-format off +template +struct type_has_method_x : std::false_type {}; +template +struct type_has_method_x> : std::true_type {}; + +template +struct type_has_member_x : std::false_type {}; +template +struct type_has_member_x> : std::true_type {}; + +template +struct type_is_vector : std::false_type {}; +template +struct type_is_vector> : std::true_type {}; +template +struct type_is_vector> : std::true_type {}; +// clang-format on + +template +inline PointOut ConvertPoint(const PointIn& v) +{ + // clang-format off + static_assert(std::is_same_v || + type_has_method_x::value || + type_has_member_x::value || + type_is_vector::value, + "Can't convert from the specified type"); + + static_assert(std::is_same_v || + type_has_method_x::value || + type_has_member_x::value || + type_is_vector::value, + "Can't convert to the specified type"); + + // clang-format on + if constexpr (std::is_same_v) + { + return v; + } + if constexpr (type_has_method_x::value) + { + return { v.x(), v.y(), v.z() }; + } + if constexpr (type_has_member_x::value) + { + return { v.x, v.y, v.z }; + } + if constexpr (type_is_vector::value) + { + return { v[0], v[1], v[2] }; + } +} + +inline int32_t& CoordT::operator[](size_t index) +{ + switch (index) + { + case 0: + return x; + case 1: + return y; + case 2: + return z; + default: + throw std::runtime_error("out of bound index"); + } +} + +inline bool CoordT::operator==(const CoordT& other) const +{ + return x == other.x && y == other.y && z == other.z; +} + +inline bool CoordT::operator!=(const CoordT& other) const +{ + return !(*this == other); +} + +inline CoordT CoordT::operator+(const CoordT& other) const +{ + return { x + other.x, y + other.y, z + other.z }; +} + +inline CoordT CoordT::operator-(const CoordT& other) const +{ + return { x - other.x, y - other.y, z - other.z }; +} + +inline CoordT& CoordT::operator+=(const CoordT& other) +{ + x += other.x; + y += other.y; + z += other.z; + return *this; +} + +inline CoordT& CoordT::operator-=(const CoordT& other) +{ + x -= other.x; + y -= other.y; + z -= other.z; + return *this; +} + +template +inline Grid::Grid(Grid&& other) + : dim_(other.dim_) + , size_(other.size_) + , mask_(std::move(other.mask_)) +{ + std::swap(data_, other.data_); +} + +template +inline Grid& Grid::operator=(Grid&& other) +{ + dim_ = other.dim_; + size_ = other.size_; + mask_ = std::move(other.mask_); + std::swap(data_, other.data_); + return *this; +} + +template +inline Grid::~Grid() +{ + if (data_) + { + delete[] data_; + } +} + +template +inline size_t Grid::memUsage() const +{ + return mask_.memUsage() + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(DataT*) + + sizeof(DataT) * size_; +} + +template +inline VoxelGrid::VoxelGrid(double voxel_size, + uint8_t inner_bits, + uint8_t leaf_bits) + : INNER_BITS(inner_bits) + , LEAF_BITS(leaf_bits) + , Log2N(INNER_BITS + LEAF_BITS) + , resolution(voxel_size) + , inv_resolution(1.0 / resolution) + , INNER_MASK((1 << INNER_BITS) - 1) + , LEAF_MASK((1 << LEAF_BITS) - 1) +{ + if (LEAF_BITS < 1 || INNER_BITS < 1) + { + throw std::runtime_error( + "The minimum value of the inner_bits and leaf_bits should be 1"); + } +} + +template +inline CoordT VoxelGrid::posToCoord(double x, double y, double z) +{ + return { static_cast(x * inv_resolution - std::signbit(x)), + static_cast(y * inv_resolution - std::signbit(y)), + static_cast(z * inv_resolution - std::signbit(z)) }; +} + +template +inline Point3D VoxelGrid::coordToPos(const CoordT& coord) +{ + return { (double(coord.x) + 0.5) * resolution, + (double(coord.y) + 0.5) * resolution, + (double(coord.z) + 0.5) * resolution }; +} + +template +inline CoordT VoxelGrid::getRootKey(const CoordT& coord) +{ + const int32_t MASK = ~((1 << Log2N) - 1); + return { coord.x & MASK, coord.y & MASK, coord.z & MASK }; +} + +template +inline CoordT VoxelGrid::getInnerKey(const CoordT& coord) +{ + const int32_t MASK = ~((1 << LEAF_BITS) - 1); + return { coord.x & MASK, coord.y & MASK, coord.z & MASK }; +} + +template +inline uint32_t VoxelGrid::getInnerIndex(const CoordT& coord) +{ + // clang-format off + return ((coord.x >> LEAF_BITS) & INNER_MASK) | + (((coord.y >> LEAF_BITS) & INNER_MASK) << INNER_BITS) | + (((coord.z >> LEAF_BITS) & INNER_MASK) << (INNER_BITS * 2)); + // clang-format on +} + +template +inline uint32_t VoxelGrid::getLeafIndex(const CoordT& coord) +{ + // clang-format off + return (coord.x & LEAF_MASK) | + ((coord.y & LEAF_MASK) << LEAF_BITS) | + ((coord.z & LEAF_MASK) << (LEAF_BITS * 2)); + // clang-format on +} + +template +inline bool VoxelGrid::Accessor::setValue(const CoordT& coord, + const DataT& value) +{ + const CoordT inner_key = grid_.getInnerKey(coord); + if (inner_key != prev_inner_coord_ || prev_leaf_ptr_ == nullptr) + { + prev_leaf_ptr_ = getLeafGrid(coord, true); + prev_inner_coord_ = inner_key; + } + + const uint32_t index = grid_.getLeafIndex(coord); + const bool was_on = prev_leaf_ptr_->mask().setOn(index); + prev_leaf_ptr_->cell(index) = value; + return was_on; +} + +//---------------------------------- +template +inline DataT* VoxelGrid::Accessor::value(const CoordT& coord, + bool create_if_missing) +{ + const CoordT inner_key = grid_.getInnerKey(coord); + + if (inner_key != prev_inner_coord_) + { + prev_leaf_ptr_ = getLeafGrid(coord, create_if_missing); + prev_inner_coord_ = inner_key; + } + + if (prev_leaf_ptr_) + { + const uint32_t index = grid_.getLeafIndex(coord); + if (prev_leaf_ptr_->mask().isOn(index)) + { + return &(prev_leaf_ptr_->cell(index)); + } + else if (create_if_missing) + { + prev_leaf_ptr_->mask().setOn(index); + prev_leaf_ptr_->cell(index) = {}; + return &(prev_leaf_ptr_->cell(index)); + } + } + return nullptr; +} + +//---------------------------------- +template +inline bool VoxelGrid::Accessor::setCellOn(const CoordT& coord, + const DataT& default_value) +{ + const CoordT inner_key = grid_.getInnerKey(coord); + + if (inner_key != prev_inner_coord_) + { + prev_leaf_ptr_ = getLeafGrid(coord, true); + prev_inner_coord_ = inner_key; + } + uint32_t index = grid_.getLeafIndex(coord); + bool was_on = prev_leaf_ptr_->mask.setOn(index); + if (!was_on) + { + prev_leaf_ptr_->cell(index) = default_value; + } + return was_on; +} + +//---------------------------------- +template +inline bool VoxelGrid::Accessor::setCellOff(const CoordT& coord) +{ + const CoordT inner_key = grid_.getInnerKey(coord); + + if (inner_key != prev_inner_coord_) + { + prev_leaf_ptr_ = getLeafGrid(coord, false); + prev_inner_coord_ = inner_key; + } + if (prev_leaf_ptr_) + { + uint32_t index = grid_.getLeafIndex(coord); + return prev_leaf_ptr_->mask.setOff(index); + } + return false; +} + +//---------------------------------- +template +inline typename VoxelGrid::LeafGrid* +VoxelGrid::Accessor::getLeafGrid(const CoordT& coord, bool create_if_missing) +{ + InnerGrid* inner_ptr = prev_inner_ptr_; + const CoordT root_key = grid_.getRootKey(coord); + + if (root_key != prev_root_coord_ || !inner_ptr) + { + auto it = grid_.root_map.find(root_key); + if (it == grid_.root_map.end()) + { + if (!create_if_missing) + { + return nullptr; + } + it = grid_.root_map.insert({ root_key, InnerGrid(grid_.INNER_BITS) }).first; + } + inner_ptr = &(it->second); + + // update the cache + prev_root_coord_ = root_key; + prev_inner_ptr_ = inner_ptr; + } + + const uint32_t inner_index = grid_.getInnerIndex(coord); + auto& inner_data = inner_ptr->cell(inner_index); + + if (create_if_missing) + { + if (!inner_ptr->mask().setOn(inner_index)) + { + inner_data = std::make_shared(grid_.LEAF_BITS); + } + } + else + { + if (!inner_ptr->mask().isOn(inner_index)) + { + return nullptr; + } + } + + return inner_data.get(); +} + +template +inline size_t VoxelGrid::memUsage() const +{ + size_t total_size = 0; + + for (unsigned i = 0; i < root_map.bucket_count(); ++i) + { + size_t bucket_size = root_map.bucket_size(i); + if (bucket_size == 0) + { + total_size++; + } + else + { + total_size += bucket_size; + } + } + + total_size += root_map.size() * (sizeof(CoordT) + sizeof(void*)); + + for (const auto& [key, inner_grid] : root_map) + { + total_size += inner_grid.memUsage(); + for (auto inner_it = inner_grid.mask().beginOn(); inner_it; ++inner_it) + { + const int32_t inner_index = *inner_it; + auto& leaf_grid = inner_grid.cell(inner_index); + total_size += leaf_grid->memUsage(); + } + } + return total_size; +} + +template +inline size_t VoxelGrid::activeCellsCount() const +{ + size_t total_size = 0; + + for (const auto& [key, inner_grid] : root_map) + { + for (auto inner_it = inner_grid.mask().beginOn(); inner_it; ++inner_it) + { + const int32_t inner_index = *inner_it; + auto& leaf_grid = inner_grid.cell(inner_index); + total_size += leaf_grid->mask().countOn(); + } + } + return total_size; +} + +//---------------------------------- +template +template +inline void VoxelGrid::forEachCell(VisitorFunction func) +{ + const int32_t MASK_LEAF = ((1 << LEAF_BITS) - 1); + const int32_t MASK_INNER = ((1 << INNER_BITS) - 1); + + for (auto& map_it : root_map) + { + const auto& [xA, yA, zA] = (map_it.first); + InnerGrid& inner_grid = map_it.second; + auto& mask2 = inner_grid.mask(); + + for (auto inner_it = mask2.beginOn(); inner_it; ++inner_it) + { + const int32_t inner_index = *inner_it; + const int32_t INNER_BITS_2 = INNER_BITS * 2; + // clang-format off + int32_t xB = xA | ((inner_index & MASK_INNER) << LEAF_BITS); + int32_t yB = yA | (((inner_index >> INNER_BITS) & MASK_INNER) << LEAF_BITS); + int32_t zB = zA | (((inner_index >> (INNER_BITS_2)) & MASK_INNER) << LEAF_BITS); + // clang-format on + + auto& leaf_grid = inner_grid.cell(inner_index); + auto& mask1 = leaf_grid->mask(); + + for (auto leaf_it = mask1.beginOn(); leaf_it; ++leaf_it) + { + const int32_t leaf_index = *leaf_it; + const int32_t LEAF_BITS_2 = LEAF_BITS * 2; + CoordT pos = { xB | (leaf_index & MASK_LEAF), + yB | ((leaf_index >> LEAF_BITS) & MASK_LEAF), + zB | ((leaf_index >> (LEAF_BITS_2)) & MASK_LEAF) }; + // apply the visitor + func(leaf_grid->cell(leaf_index), pos); + } + } + } +} + +//---------------------------------------------------------- + +#define BONXAI_USE_INTRINSICS + +/// Returns the index of the lowest, i.e. least significant, on bit in +/// the specified 64 bit word +/// +/// @warning Assumes that at least one bit is set in the word, i.e. @a v != +/// uint32_t(0)! + +inline uint32_t Mask::FindLowestOn(uint64_t v) +{ +#if defined(_MSC_VER) && defined(BONXAI_USE_INTRINSICS) + unsigned long index; + _BitScanForward64(&index, v); + return static_cast(index); +#elif (defined(__GNUC__) || defined(__clang__)) && defined(BONXAI_USE_INTRINSICS) + return static_cast(__builtin_ctzll(v)); +#else + static const unsigned char DeBruijn[64] = { + 0, 1, 2, 53, 3, 7, 54, 27, 4, 38, 41, 8, 34, 55, 48, 28, + 62, 5, 39, 46, 44, 42, 22, 9, 24, 35, 59, 56, 49, 18, 29, 11, + 63, 52, 6, 26, 37, 40, 33, 47, 61, 45, 43, 21, 23, 58, 17, 10, + 51, 25, 36, 32, 60, 20, 57, 16, 50, 31, 19, 15, 30, 14, 13, 12, + }; +// disable unary minus on unsigned warning +#if defined(_MSC_VER) && !defined(__NVCC__) +#pragma warning(push) +#pragma warning(disable : 4146) +#endif + return DeBruijn[uint64_t((v & -v) * UINT64_C(0x022FDD63CC95386D)) >> 58]; +#if defined(_MSC_VER) && !defined(__NVCC__) +#pragma warning(pop) +#endif + +#endif +} + +/// @return Number of bits that are on in the specified 64-bit word + +inline uint32_t Mask::CountOn(uint64_t v) +{ +#if defined(_MSC_VER) && defined(_M_X64) + v = __popcnt64(v); +#elif (defined(__GNUC__) || defined(__clang__)) + v = __builtin_popcountll(v); +#else + // Software Implementation + v = v - ((v >> 1) & uint64_t(0x5555555555555555)); + v = (v & uint64_t(0x3333333333333333)) + ((v >> 2) & uint64_t(0x3333333333333333)); + v = (((v + (v >> 4)) & uint64_t(0xF0F0F0F0F0F0F0F)) * + uint64_t(0x101010101010101)) >> + 56; +#endif + return static_cast(v); +} + +inline void Mask::setOn() +{ + for (uint32_t i = 0; i < WORD_COUNT; ++i) + { + words_[i] = ~uint64_t(0); + } +} + +inline void Mask::setOff() +{ + for (uint32_t i = 0; i < WORD_COUNT; ++i) + { + words_[i] = uint64_t(0); + } +} + +inline void Mask::set(bool on) +{ + const uint64_t v = on ? ~uint64_t(0) : uint64_t(0); + for (uint32_t i = 0; i < WORD_COUNT; ++i) + { + words_[i] = v; + } +} + +inline void Mask::toggle() +{ + uint32_t n = WORD_COUNT; + for (auto* w = words_; n--; ++w) + { + *w = ~*w; + } +} + +inline void Mask::toggle(uint32_t n) +{ + words_[n >> 6] ^= uint64_t(1) << (n & 63); +} + +inline uint32_t Mask::findFirstOn() const +{ + const uint64_t* w = words_; + uint32_t n = 0; + while (n < WORD_COUNT && !*w) + { + ++w; + ++n; + } + return n == WORD_COUNT ? SIZE : (n << 6) + FindLowestOn(*w); +} + +inline uint32_t Mask::findNextOn(uint32_t start) const +{ + uint32_t n = start >> 6; // initiate + if (n >= WORD_COUNT) + { + return SIZE; // check for out of bounds + } + uint32_t m = start & 63; + uint64_t b = words_[n]; + if (b & (uint64_t(1) << m)) + { + return start; // simple case: start is on + } + b &= ~uint64_t(0) << m; // mask out lower bits + while (!b && ++n < WORD_COUNT) + { + b = words_[n]; + } // find next non-zero word + return (!b ? SIZE : (n << 6) + FindLowestOn(b)); // catch last word=0 +} + +inline Mask::Mask(size_t log2dim) + : SIZE(1U << (3 * log2dim)) + , WORD_COUNT(std::max(SIZE >> 6, 1u)) +{ + words_ = (WORD_COUNT <= 8) ? static_words_ : new uint64_t[WORD_COUNT]; + + for (uint32_t i = 0; i < WORD_COUNT; ++i) + { + words_[i] = 0; + } +} + +inline Mask::Mask(size_t log2dim, bool on) + : SIZE(1U << (3 * log2dim)) + , WORD_COUNT(std::max(SIZE >> 6, 1u)) +{ + words_ = (WORD_COUNT <= 8) ? static_words_ : new uint64_t[WORD_COUNT]; + + const uint64_t v = on ? ~uint64_t(0) : uint64_t(0); + for (uint32_t i = 0; i < WORD_COUNT; ++i) + { + words_[i] = v; + } +} + +inline Mask::Mask(const Mask& other) + : SIZE(other.SIZE) + , WORD_COUNT(other.WORD_COUNT) +{ + words_ = (WORD_COUNT <= 8) ? static_words_ : new uint64_t[WORD_COUNT]; + + for (uint32_t i = 0; i < WORD_COUNT; ++i) + { + words_[i] = other.words_[i]; + } +} + +inline Mask::Mask(Mask&& other) + : SIZE(other.SIZE) + , WORD_COUNT(other.WORD_COUNT) +{ + if (WORD_COUNT <= 8) + { + words_ = static_words_; + for (uint32_t i = 0; i < WORD_COUNT; ++i) + { + words_[i] = other.words_[i]; + } + } + else + { + std::swap(words_, other.words_); + } +} + +inline Mask::~Mask() +{ + if (words_ && WORD_COUNT > 8) + { + delete[] words_; + } +} + +inline size_t Mask::memUsage() const +{ + if (WORD_COUNT > 8) + { + return sizeof(Mask) + sizeof(uint64_t) * WORD_COUNT; + } + return sizeof(Mask); +} + +inline uint32_t Mask::countOn() const +{ + uint32_t sum = 0, n = WORD_COUNT; + for (const uint64_t* w = words_; n--; ++w) + { + sum += CountOn(*w); + } + return sum; +} + +inline bool Mask::operator==(const Mask& other) const +{ + for (uint32_t i = 0; i < WORD_COUNT; ++i) + { + if (words_[i] != other.words_[i]) + { + return false; + } + } + return true; +} + +inline bool Mask::isOn(uint32_t n) const +{ + return 0 != (words_[n >> 6] & (uint64_t(1) << (n & 63))); +} + +inline bool Mask::isOn() const +{ + for (uint32_t i = 0; i < WORD_COUNT; ++i) + { + if (words_[i] != ~uint64_t(0)) + { + return false; + } + } + return true; +} + +inline bool Mask::isOff() const +{ + for (uint32_t i = 0; i < WORD_COUNT; ++i) + { + if (words_[i] != uint64_t(0)) + { + return false; + } + } + return true; +} + +inline bool Mask::setOn(uint32_t n) +{ + uint64_t& word = words_[n >> 6]; + const uint64_t on_bit = (uint64_t(1) << (n & 63)); + bool was_on = word & on_bit; + word |= on_bit; + return was_on; +} + +inline bool Mask::setOff(uint32_t n) +{ + uint64_t& word = words_[n >> 6]; + const uint64_t on_bit = (uint64_t(1) << (n & 63)); + bool was_on = word & on_bit; + word &= ~(on_bit); + return was_on; +} + +inline void Mask::set(uint32_t n, bool On) +{ +#if 1 // switch between branchless + auto& word = words_[n >> 6]; + n &= 63; + word &= ~(uint64_t(1) << n); + word |= uint64_t(On) << n; +#else + On ? this->setOn(n) : this->setOff(n); +#endif +} + +} // namespace Bonxai + +namespace std +{ +template <> +struct hash +{ + std::size_t operator()(const Bonxai::CoordT& p) const + { + // same a OpenVDB + return ((1 << 20) - 1) & (p.x * 73856093 ^ p.y * 19349663 ^ p.z * 83492791); + } +}; +} // namespace std diff --git a/libs/maps/include/mrpt/maps/bonxai/serialization.hpp b/libs/maps/include/mrpt/maps/bonxai/serialization.hpp new file mode 100644 index 0000000000..a684e53d4f --- /dev/null +++ b/libs/maps/include/mrpt/maps/bonxai/serialization.hpp @@ -0,0 +1,231 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include "bonxai.hpp" + +#ifdef __GNUG__ +#include +#include +#include +#endif + +namespace Bonxai +{ +/** + * Serialize a grid to ostream. Easy :) + */ +template +inline void Serialize(std::ostream& out, const VoxelGrid& grid); + +struct HeaderInfo +{ + std::string type_name; + int inner_bits = 0; + int leaf_bits = 0; + double resolution = 0; +}; + +/** + * @brief GetHeaderInfo is used to recover informations from the header of a + * file/stream + * + * @param header first line of the file + */ +inline HeaderInfo GetHeaderInfo(std::string header); + +/** + * @brief Deserialize create a grid. Note that template arguments need to be + * consistent with HeaderInfo + */ +template +inline VoxelGrid Deserialize(std::istream& input, HeaderInfo info); + +//--------------------------------------------------------- +namespace details +{ +#ifdef __GNUG__ +inline std::string demangle(const char* name) +{ + int status = -4; // some arbitrary value to eliminate the compiler warning + + std::unique_ptr res{ + abi::__cxa_demangle(name, NULL, NULL, &status), std::free + }; + return (status == 0) ? res.get() : name; +} + +#else + +// does nothing if not g++ +inline std::string demangle(const char* name) +{ + return name; +} +#endif + +} // namespace details + +template +inline void Write(std::ostream& out, const T& val) +{ + static_assert(std::is_trivially_copyable_v, "Must be trivially copyable"); + out.write(reinterpret_cast(&val), sizeof(T)); +} + +template +inline void Serialize(std::ostream& out, const VoxelGrid& grid) +{ + static_assert(std::is_trivially_copyable_v, + "DataT must be trivially copyable"); + + char header[256]; + std::string type_name = details::demangle(typeid(DataT).name()); + + sprintf(header, + "Bonxai::VoxelGrid<%s,%d,%d>(%lf)\n", + type_name.c_str(), + grid.INNER_BITS, + grid.LEAF_BITS, + grid.resolution); + + out.write(header, std::strlen(header)); + + //------------ + Write(out, uint32_t(grid.root_map.size())); + + for (const auto& it : grid.root_map) + { + const CoordT& root_coord = it.first; + Write(out, root_coord.x); + Write(out, root_coord.y); + Write(out, root_coord.z); + + const auto& inner_grid = it.second; + for (size_t w = 0; w < inner_grid.mask().wordCount(); w++) + { + Write(out, inner_grid.mask().getWord(w)); + } + for (auto inner = inner_grid.mask().beginOn(); inner; ++inner) + { + const uint32_t inner_index = *inner; + const auto& leaf_grid = *(inner_grid.cell(inner_index)); + + for (size_t w = 0; w < leaf_grid.mask().wordCount(); w++) + { + Write(out, leaf_grid.mask().getWord(w)); + } + for (auto leaf = leaf_grid.mask().beginOn(); leaf; ++leaf) + { + const uint32_t leaf_index = *leaf; + Write(out, leaf_grid.cell(leaf_index)); + } + } + } +} + +template +inline T Read(std::istream& input) +{ + T out; + static_assert(std::is_trivially_copyable_v, "Must be trivially copyable"); + input.read(reinterpret_cast(&out), sizeof(T)); + return out; +} + +inline HeaderInfo GetHeaderInfo(std::string header) +{ + const std::string expected_prefix = "Bonxai::VoxelGrid<"; + if (header.rfind(expected_prefix, 0) != 0) + { + throw std::runtime_error("Header wasn't recognized"); + } + int p1 = header.find(",", 18) + 1; + auto part_type = header.substr(18, p1 - 18 - 1); + + int p2 = header.find(",", p1 + 1) + 1; + auto part_ibits = header.substr(p1, p2 - p1 - 1); + + int p3 = header.find(">", p2) + 1; + auto part_lbits = header.substr(p2, p3 - p2 - 1); + + int p4 = header.find("(", p3) + 1; + int p5 = header.find(")", p4); + auto part_res = header.substr(p4, p5 - p4); + + HeaderInfo info; + info.type_name = part_type; + info.inner_bits = std::stoi(part_ibits); + info.leaf_bits = std::stoi(part_lbits); + info.resolution = std::stod(part_res); + + return info; +} + +template +inline VoxelGrid Deserialize(std::istream& input, HeaderInfo info) +{ + std::string type_name = details::demangle(typeid(DataT).name()); + if (type_name != info.type_name) + { + throw std::runtime_error("DataT does not match"); + } + + //------------ + + VoxelGrid grid(info.resolution, info.inner_bits, info.leaf_bits); + + uint32_t root_count = Read(input); + + for (size_t root_index = 0; root_index < root_count; root_index++) + { + CoordT root_coord; + root_coord.x = Read(input); + root_coord.y = Read(input); + root_coord.z = Read(input); + + auto inner_it = grid.root_map.find(root_coord); + if (inner_it == grid.root_map.end()) + { + inner_it = + grid.root_map + .insert({ root_coord, + typename VoxelGrid::InnerGrid(info.inner_bits) }) + .first; + } + auto& inner_grid = inner_it->second; + + for (size_t w = 0; w < inner_grid.mask().wordCount(); w++) + { + uint64_t word = Read(input); + inner_grid.mask().setWord(w, word); + } + for (auto inner = inner_grid.mask().beginOn(); inner; ++inner) + { + const uint32_t inner_index = *inner; + using LeafGridT = typename VoxelGrid::LeafGrid; + inner_grid.cell(inner_index) = std::make_shared(info.leaf_bits); + auto& leaf_grid = inner_grid.cell(inner_index); + + for (size_t w = 0; w < leaf_grid->mask().wordCount(); w++) + { + uint64_t word = Read(input); + leaf_grid->mask().setWord(w, word); + } + for (auto leaf = leaf_grid->mask().beginOn(); leaf; ++leaf) + { + const uint32_t leaf_index = *leaf; + leaf_grid->cell(leaf_index) = Read(input); + } + } + } + return grid; +} + +} // namespace Bonxai + diff --git a/libs/maps/src/maps/CVoxelMap.cpp b/libs/maps/src/maps/CVoxelMap.cpp new file mode 100644 index 0000000000..328811621f --- /dev/null +++ b/libs/maps/src/maps/CVoxelMap.cpp @@ -0,0 +1,150 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "maps-precomp.h" // Precomp header +// +#include +#include + +#include + +using namespace mrpt::maps; +using namespace std::string_literals; // "..."s + +// =========== Begin of Map definition ============ +MAP_DEFINITION_REGISTER("mrpt::maps::CVoxelMap,voxelMap", mrpt::maps::CVoxelMap) + +CVoxelMap::TMapDefinition::TMapDefinition() = default; +void CVoxelMap::TMapDefinition::loadFromConfigFile_map_specific( + const mrpt::config::CConfigFileBase& source, + const std::string& sectionNamePrefix) +{ + // [+"_creationOpts"] + const std::string sSectCreation = sectionNamePrefix + "_creationOpts"s; + MRPT_LOAD_CONFIG_VAR(resolution, double, source, sSectCreation); + + insertionOpts.loadFromConfigFile( + source, sectionNamePrefix + "_insertOpts"s); + likelihoodOpts.loadFromConfigFile( + source, sectionNamePrefix + "_likelihoodOpts"s); +} + +void CVoxelMap::TMapDefinition::dumpToTextStream_map_specific( + std::ostream& out) const +{ + LOADABLEOPTS_DUMP_VAR(resolution, double); + + this->insertionOpts.dumpToTextStream(out); + this->likelihoodOpts.dumpToTextStream(out); +} + +mrpt::maps::CMetricMap* CVoxelMap::internal_CreateFromMapDefinition( + const mrpt::maps::TMetricMapInitializer& _def) +{ + const CVoxelMap::TMapDefinition& def = + *dynamic_cast(&_def); + auto* obj = new CVoxelMap(def.resolution); + obj->insertionOptions = def.insertionOpts; + obj->likelihoodOptions = def.likelihoodOpts; + return obj; +} +// =========== End of Map definition Block ========= + +IMPLEMENTS_SERIALIZABLE(CVoxelMap, CMetricMap, mrpt::maps) + +/*--------------------------------------------------------------- + Constructor + ---------------------------------------------------------------*/ +CVoxelMap::~CVoxelMap() = default; + +uint8_t CVoxelMap::serializeGetVersion() const { return 0; } +void CVoxelMap::serializeTo(mrpt::serialization::CArchive& out) const +{ + insertionOptions.writeToStream(out); + likelihoodOptions.writeToStream(out); + renderingOptions.writeToStream(out); // Added in v1 + out << genericMapParams; + + // grid data: + std::stringstream ss; + Bonxai::Serialize(ss, grid()); + out << ss.str(); +} + +void CVoxelMap::serializeFrom( + mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + { + insertionOptions.readFromStream(in); + likelihoodOptions.readFromStream(in); + renderingOptions.readFromStream(in); + in >> genericMapParams; + + this->clear(); + + // grid data: + std::string msg; + in >> msg; + std::istringstream ifile(msg, std::ios::binary); + + char header[256]; + ifile.getline(header, 256); + Bonxai::HeaderInfo info = Bonxai::GetHeaderInfo(header); + + m_impl = std::make_unique( + Bonxai::Deserialize(ifile, info)); + } + break; + default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; +} + +bool CVoxelMap::internal_insertObservation( + const mrpt::obs::CObservation& obs, + const std::optional& robotPose) +{ + // Auxiliary 3D point cloud: + MRPT_TODO("Handle special cases and avoid duplicating pointcloud?"); + + mrpt::maps::CSimplePointsMap pts; + pts.insertObservation(obs, robotPose); + + if (pts.empty()) return false; + + mrpt::math::TPoint3D sensorPt; + mrpt::poses::CPose3D localSensorPose; + obs.getSensorPose(localSensorPose); + if (robotPose) + { + // compose: + sensorPt = (*robotPose + localSensorPose).translation(); + } + else + { + sensorPt = localSensorPose.translation(); + } + + // Insert rays: + if (insertionOptions.ray_trace_free_space) + insertPointCloudAsRays(pts, sensorPt); + else + insertPointCloudAsEndPoints(pts, sensorPt); + return true; +} + +double CVoxelMap::internal_computeObservationLikelihood( + const mrpt::obs::CObservation& obs, + const mrpt::poses::CPose3D& takenFrom) const +{ + THROW_EXCEPTION("TODO"); + return 0; +} diff --git a/libs/maps/src/maps/CVoxelMapOccupancyBase.cpp b/libs/maps/src/maps/CVoxelMapOccupancyBase.cpp new file mode 100644 index 0000000000..e098e59b96 --- /dev/null +++ b/libs/maps/src/maps/CVoxelMapOccupancyBase.cpp @@ -0,0 +1,112 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "maps-precomp.h" // Precomp header +// +#include + +using namespace mrpt::maps; + +void TVoxelMap_InsertionOptions::loadFromConfigFile( + const mrpt::config::CConfigFileBase& c, const std::string& s) +{ + MRPT_LOAD_CONFIG_VAR(max_range, double, c, s); + MRPT_LOAD_CONFIG_VAR(prob_miss, double, c, s); + MRPT_LOAD_CONFIG_VAR(prob_hit, double, c, s); + MRPT_LOAD_CONFIG_VAR(clamp_min, double, c, s); + MRPT_LOAD_CONFIG_VAR(clamp_max, double, c, s); + MRPT_LOAD_CONFIG_VAR(ray_trace_free_space, bool, c, s); + MRPT_LOAD_CONFIG_VAR(decimation, uint64_t, c, s); +} +void TVoxelMap_InsertionOptions::saveToConfigFile( + mrpt::config::CConfigFileBase& c, const std::string& s) const +{ + MRPT_SAVE_CONFIG_VAR(max_range, c, s); + MRPT_SAVE_CONFIG_VAR(prob_miss, c, s); + MRPT_SAVE_CONFIG_VAR(prob_hit, c, s); + MRPT_SAVE_CONFIG_VAR(clamp_min, c, s); + MRPT_SAVE_CONFIG_VAR(clamp_max, c, s); + MRPT_SAVE_CONFIG_VAR(ray_trace_free_space, c, s); + MRPT_SAVE_CONFIG_VAR(decimation, c, s); +} + +void TVoxelMap_InsertionOptions::writeToStream( + mrpt::serialization::CArchive& out) const +{ + const uint8_t version = 0; + out << version; + + out << max_range << prob_miss << prob_hit << clamp_min << clamp_max; + out << ray_trace_free_space << decimation; +} + +void TVoxelMap_InsertionOptions::readFromStream( + mrpt::serialization::CArchive& in) +{ + const uint8_t version = in.ReadAs(); + switch (version) + { + case 0: + in >> max_range >> prob_miss >> prob_hit >> clamp_min >> clamp_max; + in >> ray_trace_free_space >> decimation; + break; + default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + } +} + +void TVoxelMap_RenderingOptions::writeToStream( + mrpt::serialization::CArchive& out) const +{ + const uint8_t version = 0; + out << version; + + out << generateOccupiedVoxels << visibleOccupiedVoxels; + out << generateFreeVoxels << visibleFreeVoxels; + out << occupiedThreshold << freeThreshold; +} + +void TVoxelMap_RenderingOptions::readFromStream( + mrpt::serialization::CArchive& in) +{ + const uint8_t version = in.ReadAs(); + switch (version) + { + case 0: + in >> generateOccupiedVoxels >> visibleOccupiedVoxels; + in >> generateFreeVoxels >> visibleFreeVoxels; + in >> occupiedThreshold >> freeThreshold; + break; + default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + } +} + +void TVoxelMap_LikelihoodOptions::loadFromConfigFile( + const mrpt::config::CConfigFileBase& c, const std::string& s) +{ + MRPT_LOAD_CONFIG_VAR(decimation, int, c, s); + MRPT_LOAD_CONFIG_VAR(occupiedThreshold, double, c, s); +} +void TVoxelMap_LikelihoodOptions::saveToConfigFile( + mrpt::config::CConfigFileBase& c, const std::string& s) const +{ + MRPT_SAVE_CONFIG_VAR(decimation, c, s); + MRPT_SAVE_CONFIG_VAR(occupiedThreshold, c, s); +} + +void TVoxelMap_LikelihoodOptions::writeToStream( + mrpt::serialization::CArchive& out) const +{ + out << decimation << occupiedThreshold; +} + +void TVoxelMap_LikelihoodOptions::readFromStream( + mrpt::serialization::CArchive& in) +{ + in >> decimation >> occupiedThreshold; +} diff --git a/libs/maps/src/maps/CVoxelMapRGB.cpp b/libs/maps/src/maps/CVoxelMapRGB.cpp new file mode 100644 index 0000000000..fa5a05d001 --- /dev/null +++ b/libs/maps/src/maps/CVoxelMapRGB.cpp @@ -0,0 +1,255 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include "maps-precomp.h" // Precomp header +// +#include +#include +#include +#include + +#include + +using namespace mrpt::maps; +using namespace std::string_literals; // "..."s + +// =========== Begin of Map definition ============ +MAP_DEFINITION_REGISTER( + "mrpt::maps::CVoxelMapRGB,voxelMapRGB", mrpt::maps::CVoxelMapRGB) + +CVoxelMapRGB::TMapDefinition::TMapDefinition() = default; +void CVoxelMapRGB::TMapDefinition::loadFromConfigFile_map_specific( + const mrpt::config::CConfigFileBase& source, + const std::string& sectionNamePrefix) +{ + // [+"_creationOpts"] + const std::string sSectCreation = sectionNamePrefix + "_creationOpts"s; + MRPT_LOAD_CONFIG_VAR(resolution, double, source, sSectCreation); + + insertionOpts.loadFromConfigFile( + source, sectionNamePrefix + "_insertOpts"s); + likelihoodOpts.loadFromConfigFile( + source, sectionNamePrefix + "_likelihoodOpts"s); +} + +void CVoxelMapRGB::TMapDefinition::dumpToTextStream_map_specific( + std::ostream& out) const +{ + LOADABLEOPTS_DUMP_VAR(resolution, double); + + this->insertionOpts.dumpToTextStream(out); + this->likelihoodOpts.dumpToTextStream(out); +} + +mrpt::maps::CMetricMap* CVoxelMapRGB::internal_CreateFromMapDefinition( + const mrpt::maps::TMetricMapInitializer& _def) +{ + const CVoxelMapRGB::TMapDefinition& def = + *dynamic_cast(&_def); + auto* obj = new CVoxelMapRGB(def.resolution); + obj->insertionOptions = def.insertionOpts; + obj->likelihoodOptions = def.likelihoodOpts; + return obj; +} +// =========== End of Map definition Block ========= + +IMPLEMENTS_SERIALIZABLE(CVoxelMapRGB, CMetricMap, mrpt::maps) + +/*--------------------------------------------------------------- + Constructor + ---------------------------------------------------------------*/ +CVoxelMapRGB::~CVoxelMapRGB() = default; + +uint8_t CVoxelMapRGB::serializeGetVersion() const { return 0; } +void CVoxelMapRGB::serializeTo(mrpt::serialization::CArchive& out) const +{ + insertionOptions.writeToStream(out); + likelihoodOptions.writeToStream(out); + renderingOptions.writeToStream(out); // Added in v1 + out << genericMapParams; + + // grid data: + std::stringstream ss; + Bonxai::Serialize(ss, grid()); + out << ss.str(); +} + +void CVoxelMapRGB::serializeFrom( + mrpt::serialization::CArchive& in, uint8_t version) +{ + switch (version) + { + case 0: + { + insertionOptions.readFromStream(in); + likelihoodOptions.readFromStream(in); + renderingOptions.readFromStream(in); + in >> genericMapParams; + + this->clear(); + + // grid data: + std::string msg; + in >> msg; + std::istringstream ifile(msg, std::ios::binary); + + char header[256]; + ifile.getline(header, 256); + Bonxai::HeaderInfo info = Bonxai::GetHeaderInfo(header); + + m_impl = std::make_unique( + Bonxai::Deserialize(ifile, info)); + } + break; + default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + }; +} + +bool CVoxelMapRGB::internal_insertObservation_3DScan( + const mrpt::obs::CObservation3DRangeScan& obs, + const std::optional& robotPose) +{ + mrpt::maps::CColouredPointsMap colPts; + + mrpt::obs::T3DPointsProjectionParams pp; + pp.takeIntoAccountSensorPoseOnRobot = true; + pp.robotPoseInTheWorld = robotPose; + + // remove const (keep walking...) + auto& oobs = const_cast(obs); + oobs.unprojectInto(colPts, pp); + + if (colPts.empty()) return false; + + // Insert occupancy info + RGB color: + invalidateOccupiedCache(); + + const occupancy_value_t logodd_observation_occupied = + std::max(1, p2l(insertionOptions.prob_hit)); + const occupancy_value_t logodd_thres_occupied = + p2l(1.0 - insertionOptions.clamp_max); + + const auto& xs = colPts.getPointsBufferRef_x(); + const auto& ys = colPts.getPointsBufferRef_y(); + const auto& zs = colPts.getPointsBufferRef_z(); + + const auto maxSqrDist = mrpt::square(insertionOptions.max_range); + mrpt::math::TPoint3D sensorPt; + mrpt::poses::CPose3D localSensorPose; + obs.getSensorPose(localSensorPose); + if (robotPose) + { + // compose: + sensorPt = (*robotPose + localSensorPose).translation(); + } + else + { + sensorPt = localSensorPose.translation(); + } + + for (size_t i = 0; i < xs.size(); i += insertionOptions.decimation) + { + if (insertionOptions.max_range > 0 && + (mrpt::math::TPoint3D(xs[i], ys[i], zs[i]) - sensorPt).sqrNorm() > + maxSqrDist) + continue; // skip + + voxel_node_t* cell = base_t::m_impl->accessor.value( + Bonxai::PosToCoord( + {xs[i], ys[i], zs[i]}, base_t::m_impl->grid.inv_resolution), + true /*create*/); + if (!cell) continue; // should never happen? + + // Update occupancy: + updateCell_fast_occupied( + cell, logodd_observation_occupied, logodd_thres_occupied); + + // and merge color: + mrpt::img::TColorf colF; + colPts.getPointColor(i, colF.R, colF.G, colF.B); +#if 1 // fuse colors: + mrpt::img::TColorf oldCol( + mrpt::img::TColor(cell->color.R, cell->color.G, cell->color.B)); + + mrpt::img::TColorf newF; + const float N_1 = 1.0f / (cell->numColObs + 1); + + newF.R = N_1 * (oldCol.R * cell->numColObs + colF.R); + newF.G = N_1 * (oldCol.G * cell->numColObs + colF.G); + newF.B = N_1 * (oldCol.B * cell->numColObs + colF.B); + + cell->numColObs++; + const auto nCol = newF.asTColor(); + cell->color.R = nCol.R; + cell->color.G = nCol.G; + cell->color.B = nCol.B; +#else + // just copy latest color: + cell->color = colF.asTColor(); +#endif + } + + return true; +} + +bool CVoxelMapRGB::internal_insertObservation( + const mrpt::obs::CObservation& obs, + const std::optional& robotPose) +{ + // Auxiliary 3D point cloud: + if (auto obs3Dscan = + dynamic_cast(&obs); + obs3Dscan) + { + return internal_insertObservation_3DScan(*obs3Dscan, robotPose); + } + else + { + return internal_insertObservation_default(obs, robotPose); + } +} + +bool CVoxelMapRGB::internal_insertObservation_default( + const mrpt::obs::CObservation& obs, + const std::optional& robotPose) +{ + // build aux 3D pointcloud: + mrpt::maps::CSimplePointsMap pts; + pts.insertObservation(obs, robotPose); + + if (pts.empty()) return false; + + mrpt::math::TPoint3D sensorPt; + mrpt::poses::CPose3D localSensorPose; + obs.getSensorPose(localSensorPose); + if (robotPose) + { + // compose: + sensorPt = (*robotPose + localSensorPose).translation(); + } + else + { + sensorPt = localSensorPose.translation(); + } + + // Insert rays: + if (insertionOptions.ray_trace_free_space) + insertPointCloudAsRays(pts, sensorPt); + else + insertPointCloudAsEndPoints(pts, sensorPt); + return true; +} + +double CVoxelMapRGB::internal_computeObservationLikelihood( + const mrpt::obs::CObservation& obs, + const mrpt::poses::CPose3D& takenFrom) const +{ + THROW_EXCEPTION("TODO"); + return 0; +} diff --git a/libs/maps/src/maps/serializations_unittest.cpp b/libs/maps/src/maps/serializations_unittest.cpp index 5ed0bfd113..94b223ca8c 100644 --- a/libs/maps/src/maps/serializations_unittest.cpp +++ b/libs/maps/src/maps/serializations_unittest.cpp @@ -44,6 +44,8 @@ TEST_CLASS_MOVE_COPY_CTORS(CWeightedPointsMap); TEST_CLASS_MOVE_COPY_CTORS(CPointsMapXYZI); TEST_CLASS_MOVE_COPY_CTORS(COctoMap); TEST_CLASS_MOVE_COPY_CTORS(CColouredOctoMap); +TEST_CLASS_MOVE_COPY_CTORS(CVoxelMap); +TEST_CLASS_MOVE_COPY_CTORS(CVoxelMapRGB); TEST_CLASS_MOVE_COPY_CTORS(CSinCosLookUpTableFor2DScans); // obs: TEST_CLASS_MOVE_COPY_CTORS(CObservationPointCloud); @@ -72,6 +74,8 @@ TEST(SerializeTestMaps, WriteReadToMem) CLASS_ID(CPointsMapXYZI), CLASS_ID(COctoMap), CLASS_ID(CColouredOctoMap), + CLASS_ID(CVoxelMap), + CLASS_ID(CVoxelMapRGB), // obs: CLASS_ID(CObservationPointCloud), CLASS_ID(CObservationRotatingScan), diff --git a/libs/maps/src/registerAllClasses.cpp b/libs/maps/src/registerAllClasses.cpp index 41a56b4a10..44f9b776cf 100644 --- a/libs/maps/src/registerAllClasses.cpp +++ b/libs/maps/src/registerAllClasses.cpp @@ -46,6 +46,9 @@ MRPT_INITIALIZER(registerAllClasses_mrpt_maps) registerClass(CLASS_ID(COctoMap)); registerClass(CLASS_ID(CColouredOctoMap)); + registerClass(CLASS_ID(CVoxelMap)); + registerClass(CLASS_ID(CVoxelMapRGB)); + registerClass(CLASS_ID(CAngularObservationMesh)); registerClass(CLASS_ID(CPlanarLaserScan)); diff --git a/libs/obs/include/mrpt/maps/TMetricMapInitializer.h b/libs/obs/include/mrpt/maps/TMetricMapInitializer.h index c41b18103a..77c2e18364 100644 --- a/libs/obs/include/mrpt/maps/TMetricMapInitializer.h +++ b/libs/obs/include/mrpt/maps/TMetricMapInitializer.h @@ -135,6 +135,8 @@ class TSetOfMetricMapInitializers : public mrpt::config::CLoadableOptions *mrpt::maps::CColouredPointsMap map> * weightedPointsMap_count=<0 or 1, for creating a *mrpt::maps::CWeightedPointsMap map> + * mrpt::maps::CVoxelMap_count=[0|1] + * mrpt::maps::CVoxelMapRGB_count=[0|1] * * // ==================================================== * // Creation Options for OccupancyGridMap ##: diff --git a/libs/opengl/include/mrpt/opengl/COctoMapVoxels.h b/libs/opengl/include/mrpt/opengl/COctoMapVoxels.h index 967af3da2a..d600bca8c2 100644 --- a/libs/opengl/include/mrpt/opengl/COctoMapVoxels.h +++ b/libs/opengl/include/mrpt/opengl/COctoMapVoxels.h @@ -86,7 +86,9 @@ class COctoMapVoxels : public CRenderizableShaderTriangles, /** Combination of COLOR_FROM_HEIGHT and TRANSPARENCY_FROM_OCCUPANCY */ MIXED, /** All cubes are of identical color. */ - FIXED + FIXED, + /** Color from RGB data */ + COLOR_FROM_RGB_DATA }; /** The info of each of the voxels */ diff --git a/python/all_mrpt_maps3.cpp b/python/all_mrpt_maps3.cpp index 8a737b5b77..c6073548dd 100644 --- a/python/all_mrpt_maps3.cpp +++ b/python/all_mrpt_maps3.cpp @@ -8,3 +8,9 @@ #include "src/mrpt/maps/CWeightedPointsMap.cpp" #include "src/mrpt/maps/CWirelessPowerGridMap2D.cpp" #include "src/mrpt/maps/metric_map_types.cpp" +#include "src/mrpt/maps/CVoxelMap.cpp" +#include "src/mrpt/maps/CVoxelMapBase.cpp" +#include "src/mrpt/maps/CVoxelMapOccupancyBase.cpp" +#include "src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp" +#include "src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp" + diff --git a/python/all_mrpt_opengl1.cpp b/python/all_mrpt_opengl1.cpp index 91de4dd1f1..946ffd8031 100644 --- a/python/all_mrpt_opengl1.cpp +++ b/python/all_mrpt_opengl1.cpp @@ -12,4 +12,4 @@ #include "src/mrpt/opengl/COctreePointRenderer.cpp" #include "src/mrpt/opengl/CPlanarLaserScan.cpp" #include "src/mrpt/opengl/CPointCloudColoured.cpp" -#include "src/mrpt/opengl/CPointCloud.cpp" +#include "src/mrpt/opengl/CPointCloud.cpp" \ No newline at end of file diff --git a/python/all_mrpt_rtti.cpp b/python/all_mrpt_rtti.cpp index 2843ef7e86..8f33bf4adb 100644 --- a/python/all_mrpt_rtti.cpp +++ b/python/all_mrpt_rtti.cpp @@ -2,4 +2,5 @@ #include "src/mrpt/rtti/CObject_1.cpp" #include "src/mrpt/rtti/CObject_2.cpp" #include "src/mrpt/rtti/CObject_3.cpp" +#include "src/mrpt/rtti/CObject_4.cpp" #include "src/mrpt/rtti/CObject.cpp" diff --git a/python/all_wrapped_mrpt_headers.hpp b/python/all_wrapped_mrpt_headers.hpp index ed53215b05..6d1711eaa1 100644 --- a/python/all_wrapped_mrpt_headers.hpp +++ b/python/all_wrapped_mrpt_headers.hpp @@ -166,6 +166,8 @@ #include #include #include +#include +#include #include #include #include diff --git a/python/python.conf b/python/python.conf index ccba8abe7d..3050455b82 100644 --- a/python/python.conf +++ b/python/python.conf @@ -298,3 +298,5 @@ -class std::monostate # +function mrpt::poses::operator+(const mrpt::poses::CPose3DPDFGaussian&, const mrpt::poses::CPose3DPDFGaussian&) +# +-namespace Bonxai \ No newline at end of file diff --git a/python/src/mrpt/maps/CColouredOctoMap.cpp b/python/src/mrpt/maps/CColouredOctoMap.cpp index 0cb3e5fb89..42867d818b 100644 --- a/python/src/mrpt/maps/CColouredOctoMap.cpp +++ b/python/src/mrpt/maps/CColouredOctoMap.cpp @@ -99,7 +99,7 @@ PYBIND11_MAKE_OPAQUE(std::shared_ptr) #endif -// mrpt::maps::CColouredOctoMap file:mrpt/maps/CColouredOctoMap.h line:36 +// mrpt::maps::CColouredOctoMap file:mrpt/maps/CColouredOctoMap.h line:38 struct PyCallBack_mrpt_maps_CColouredOctoMap : public mrpt::maps::CColouredOctoMap { using mrpt::maps::CColouredOctoMap::CColouredOctoMap; @@ -1106,8 +1106,8 @@ struct PyCallBack_mrpt_maps_CColouredPointsMap_TMapDefinition : public mrpt::map void bind_mrpt_maps_CColouredOctoMap(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::maps::CColouredOctoMap file:mrpt/maps/CColouredOctoMap.h line:36 - pybind11::class_, PyCallBack_mrpt_maps_CColouredOctoMap, mrpt::maps::COctoMapBase> cl(M("mrpt::maps"), "CColouredOctoMap", "A three-dimensional probabilistic occupancy grid, implemented as an\n octo-tree with the \"octomap\" C++ library.\n This version stores both, occupancy information and RGB colour data at\n each octree node. See the base class mrpt::maps::COctoMapBase.\n\n \n CMetricMap, the example in \"MRPT/samples/octomap_simple\"\n \n\n\n "); + { // mrpt::maps::CColouredOctoMap file:mrpt/maps/CColouredOctoMap.h line:38 + pybind11::class_, PyCallBack_mrpt_maps_CColouredOctoMap, mrpt::maps::COctoMapBase> cl(M("mrpt::maps"), "CColouredOctoMap", "A three-dimensional probabilistic occupancy grid, implemented as an\n octo-tree with the \"octomap\" C++ library.\n This version stores both, occupancy information and RGB colour data at\n each octree node. See the base class mrpt::maps::COctoMapBase.\n\n The octomap library was presented in \n\n \n CMetricMap, the example in \"MRPT/samples/octomap_simple\"\n \n\n\n "); cl.def( pybind11::init( [](){ return new mrpt::maps::CColouredOctoMap(); }, [](){ return new PyCallBack_mrpt_maps_CColouredOctoMap(); } ), "doc"); cl.def( pybind11::init(), pybind11::arg("resolution") ); diff --git a/python/src/mrpt/maps/CMultiMetricMap.cpp b/python/src/mrpt/maps/CMultiMetricMap.cpp index 5061aaa119..e620ace65d 100644 --- a/python/src/mrpt/maps/CMultiMetricMap.cpp +++ b/python/src/mrpt/maps/CMultiMetricMap.cpp @@ -95,7 +95,7 @@ PYBIND11_MAKE_OPAQUE(std::shared_ptr) #endif -// mrpt::maps::CMultiMetricMap file:mrpt/maps/CMultiMetricMap.h line:119 +// mrpt::maps::CMultiMetricMap file:mrpt/maps/CMultiMetricMap.h line:120 struct PyCallBack_mrpt_maps_CMultiMetricMap : public mrpt::maps::CMultiMetricMap { using mrpt::maps::CMultiMetricMap::CMultiMetricMap; @@ -421,8 +421,8 @@ struct PyCallBack_mrpt_maps_CSimpleMap : public mrpt::maps::CSimpleMap { void bind_mrpt_maps_CMultiMetricMap(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::maps::CMultiMetricMap file:mrpt/maps/CMultiMetricMap.h line:119 - pybind11::class_, PyCallBack_mrpt_maps_CMultiMetricMap, mrpt::maps::CMetricMap> cl(M("mrpt::maps"), "CMultiMetricMap", "This class stores any customizable set of metric maps.\n The internal metric maps can be accessed directly by the user as smart\npointers with CMultiMetricMap::mapByIndex() or via `iterator`s.\n The utility of this container is to operate on several maps simultaneously:\nupdate them by inserting observations,\n evaluate the likelihood of one observation by fusing (multiplying) the\nlikelihoods over the different maps, etc.\n\n These kinds of metric maps can be kept inside (list may be\n incomplete, refer to classes derived from mrpt::maps::CMetricMap):\n - mrpt::maps::CSimplePointsMap: For 2D or 3D range scans, ...\n - mrpt::maps::COccupancyGridMap2D: 2D, horizontal laser range\n scans, at different altitudes.\n - mrpt::maps::COccupancyGridMap3D: 3D occupancy voxel map.\n - mrpt::maps::COctoMap: For 3D occupancy grids of variable resolution,\n with octrees (based on the library `octomap`).\n - mrpt::maps::CColouredOctoMap: The same than above, but nodes can store\n RGB data appart from occupancy.\n - mrpt::maps::CLandmarksMap: For visual landmarks,etc...\n - mrpt::maps::CGasConcentrationGridMap2D: For gas concentration maps.\n - mrpt::maps::CWirelessPowerGridMap2D: For wifi power maps.\n - mrpt::maps::CBeaconMap: For range-only SLAM.\n - mrpt::maps::CHeightGridMap2D: For elevation maps of height for each\n (x,y) location (Digital elevation model, DEM)\n - mrpt::maps::CHeightGridMap2D_MRF: DEMs as Markov Random Field (MRF)\n - mrpt::maps::CReflectivityGridMap2D: For maps of \"reflectivity\" for\n each (x,y) location.\n - mrpt::maps::CColouredPointsMap: For point map with color.\n - mrpt::maps::CWeightedPointsMap: For point map with weights (capable of\n \"fusing\").\n\n See CMultiMetricMap::setListOfMaps() for the method for initializing this\nclass programmatically.\n See also TSetOfMetricMapInitializers::loadFromConfigFile for a template of\n\".ini\"-like configuration\n file that can be used to define which maps to create and all their\nparameters.\n Alternatively, the list of maps is public so it can be directly\nmanipulated/accessed in CMultiMetricMap::maps\n\n Configuring the list of maps: Alternatives\n --------------------------------------------\n\n **Method #1: Using map definition structures**\n \n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n **Method #2: Using a configuration file**\n See TSetOfMetricMapInitializers::loadFromConfigFile() for details on expected\nfile format.\n\n \n\n\n\n\n\n\n\n **Method #3: Manual manipulation**\n\n \n\n\n\n\n\n\n \n [New in MRPT 1.3.0]: `likelihoodMapSelection`, which selected the map\nto be used when\n computing the likelihood of an observation, has been removed. Use the\n`enableObservationLikelihood`\n property of each individual map declaration.\n\n \n [New in MRPT 1.3.0]: `enableInsertion_{pointsMap,...}` have been also\nremoved.\n Use the `enableObservationInsertion` property of each map declaration.\n\n \n This class belongs to [mrpt-slam] instead of [mrpt-maps] due to the\ndependency on map classes in mrpt-vision.\n \n\n CMetricMap \n\n "); + { // mrpt::maps::CMultiMetricMap file:mrpt/maps/CMultiMetricMap.h line:120 + pybind11::class_, PyCallBack_mrpt_maps_CMultiMetricMap, mrpt::maps::CMetricMap> cl(M("mrpt::maps"), "CMultiMetricMap", "This class stores any customizable set of metric maps.\n The internal metric maps can be accessed directly by the user as smart\npointers with CMultiMetricMap::mapByIndex() or via `iterator`s.\n The utility of this container is to operate on several maps simultaneously:\nupdate them by inserting observations,\n evaluate the likelihood of one observation by fusing (multiplying) the\nlikelihoods over the different maps, etc.\n\n These kinds of metric maps can be kept inside (list may be\n incomplete, refer to classes derived from mrpt::maps::CMetricMap):\n - mrpt::maps::CSimplePointsMap: For 2D or 3D range scans, ...\n - mrpt::maps::COccupancyGridMap2D: 2D, horizontal laser range\n scans, at different altitudes.\n - mrpt::maps::COccupancyGridMap3D: 3D occupancy voxel map.\n - mrpt::maps::COctoMap: For 3D occupancy grids of variable resolution,\n with octrees (based on the library `octomap`).\n - mrpt::maps::CVoxelMap or mrpt::maps::CVoxelMapRGB: 3D sparse voxel maps.\n - mrpt::maps::CColouredOctoMap: The same than above, but nodes can store\n RGB data appart from occupancy.\n - mrpt::maps::CLandmarksMap: For visual landmarks,etc...\n - mrpt::maps::CGasConcentrationGridMap2D: For gas concentration maps.\n - mrpt::maps::CWirelessPowerGridMap2D: For wifi power maps.\n - mrpt::maps::CBeaconMap: For range-only SLAM.\n - mrpt::maps::CHeightGridMap2D: For elevation maps of height for each\n (x,y) location (Digital elevation model, DEM)\n - mrpt::maps::CHeightGridMap2D_MRF: DEMs as Markov Random Field (MRF)\n - mrpt::maps::CReflectivityGridMap2D: For maps of \"reflectivity\" for\n each (x,y) location.\n - mrpt::maps::CColouredPointsMap: For point map with color.\n - mrpt::maps::CWeightedPointsMap: For point map with weights (capable of\n \"fusing\").\n\n See CMultiMetricMap::setListOfMaps() for the method for initializing this\nclass programmatically.\n See also TSetOfMetricMapInitializers::loadFromConfigFile for a template of\n\".ini\"-like configuration\n file that can be used to define which maps to create and all their\nparameters.\n Alternatively, the list of maps is public so it can be directly\nmanipulated/accessed in CMultiMetricMap::maps\n\n Configuring the list of maps: Alternatives\n --------------------------------------------\n\n **Method #1: Using map definition structures**\n \n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n **Method #2: Using a configuration file**\n See TSetOfMetricMapInitializers::loadFromConfigFile() for details on expected\nfile format.\n\n \n\n\n\n\n\n\n\n **Method #3: Manual manipulation**\n\n \n\n\n\n\n\n\n \n [New in MRPT 1.3.0]: `likelihoodMapSelection`, which selected the map\nto be used when\n computing the likelihood of an observation, has been removed. Use the\n`enableObservationLikelihood`\n property of each individual map declaration.\n\n \n [New in MRPT 1.3.0]: `enableInsertion_{pointsMap,...}` have been also\nremoved.\n Use the `enableObservationInsertion` property of each map declaration.\n\n \n This class belongs to [mrpt-slam] instead of [mrpt-maps] due to the\ndependency on map classes in mrpt-vision.\n \n\n CMetricMap \n\n "); cl.def( pybind11::init( [](){ return new mrpt::maps::CMultiMetricMap(); }, [](){ return new PyCallBack_mrpt_maps_CMultiMetricMap(); } ) ); cl.def( pybind11::init(), pybind11::arg("initializers") ); diff --git a/python/src/mrpt/maps/COccupancyGridMap3D.cpp b/python/src/mrpt/maps/COccupancyGridMap3D.cpp index 63540bcf93..b95c368820 100644 --- a/python/src/mrpt/maps/COccupancyGridMap3D.cpp +++ b/python/src/mrpt/maps/COccupancyGridMap3D.cpp @@ -89,7 +89,7 @@ PYBIND11_MAKE_OPAQUE(std::shared_ptr) #endif -// mrpt::maps::COccupancyGridMap3D file:mrpt/maps/COccupancyGridMap3D.h line:33 +// mrpt::maps::COccupancyGridMap3D file:mrpt/maps/COccupancyGridMap3D.h line:36 struct PyCallBack_mrpt_maps_COccupancyGridMap3D : public mrpt::maps::COccupancyGridMap3D { using mrpt::maps::COccupancyGridMap3D::COccupancyGridMap3D; @@ -316,7 +316,7 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap3D : public mrpt::maps::COccupancyG } }; -// mrpt::maps::COccupancyGridMap3D::TInsertionOptions file:mrpt/maps/COccupancyGridMap3D.h line:197 +// mrpt::maps::COccupancyGridMap3D::TInsertionOptions file:mrpt/maps/COccupancyGridMap3D.h line:200 struct PyCallBack_mrpt_maps_COccupancyGridMap3D_TInsertionOptions : public mrpt::maps::COccupancyGridMap3D::TInsertionOptions { using mrpt::maps::COccupancyGridMap3D::TInsertionOptions::TInsertionOptions; @@ -348,7 +348,7 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap3D_TInsertionOptions : public mrpt: } }; -// mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions file:mrpt/maps/COccupancyGridMap3D.h line:254 +// mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions file:mrpt/maps/COccupancyGridMap3D.h line:257 struct PyCallBack_mrpt_maps_COccupancyGridMap3D_TLikelihoodOptions : public mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions { using mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::TLikelihoodOptions; @@ -427,8 +427,8 @@ struct PyCallBack_mrpt_maps_COccupancyGridMap3D_TMapDefinition : public mrpt::ma void bind_mrpt_maps_COccupancyGridMap3D(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::maps::COccupancyGridMap3D file:mrpt/maps/COccupancyGridMap3D.h line:33 - pybind11::class_, PyCallBack_mrpt_maps_COccupancyGridMap3D, mrpt::maps::CMetricMap, mrpt::maps::CLogOddsGridMap3D> cl(M("mrpt::maps"), "COccupancyGridMap3D", "A 3D occupancy grid map with a regular, even distribution of voxels.\n\n This is a faster alternative to COctoMap, but use with caution with limited\nmap extensions, since it could easily exaust available memory.\n\n Each voxel follows a Bernoulli probability distribution: a value of 0 means\ncertainly occupied, 1 means a certainly empty voxel. Initially 0.5 means\nuncertainty.\n\n "); + { // mrpt::maps::COccupancyGridMap3D file:mrpt/maps/COccupancyGridMap3D.h line:36 + pybind11::class_, PyCallBack_mrpt_maps_COccupancyGridMap3D, mrpt::maps::CMetricMap, mrpt::maps::CLogOddsGridMap3D> cl(M("mrpt::maps"), "COccupancyGridMap3D", "A 3D occupancy grid map with a regular, even distribution of voxels.\n\n This is a faster alternative to COctoMap, but use with caution with limited\nmap extensions, since it could easily exaust available memory.\n\n Each voxel follows a Bernoulli probability distribution: a value of 0 means\ncertainly occupied, 1 means a certainly empty voxel. Initially 0.5 means\nuncertainty.\n\n An alternative, sparse representation of a 3D map is provided\n via mrpt::maps::CVoxelMap and mrpt::maps::CVoxelMapRGB\n\n "); cl.def( pybind11::init( [](){ return new mrpt::maps::COccupancyGridMap3D(); }, [](){ return new PyCallBack_mrpt_maps_COccupancyGridMap3D(); } ), "doc"); cl.def( pybind11::init( [](const struct mrpt::math::TPoint3D_ & a0){ return new mrpt::maps::COccupancyGridMap3D(a0); }, [](const struct mrpt::math::TPoint3D_ & a0){ return new PyCallBack_mrpt_maps_COccupancyGridMap3D(a0); } ), "doc"); cl.def( pybind11::init( [](const struct mrpt::math::TPoint3D_ & a0, const struct mrpt::math::TPoint3D_ & a1){ return new mrpt::maps::COccupancyGridMap3D(a0, a1); }, [](const struct mrpt::math::TPoint3D_ & a0, const struct mrpt::math::TPoint3D_ & a1){ return new PyCallBack_mrpt_maps_COccupancyGridMap3D(a0, a1); } ), "doc"); @@ -474,7 +474,7 @@ void bind_mrpt_maps_COccupancyGridMap3D(std::function< pybind11::module &(std::s cl.def("asString", (std::string (mrpt::maps::COccupancyGridMap3D::*)() const) &mrpt::maps::COccupancyGridMap3D::asString, "Returns a short description of the map. \n\nC++: mrpt::maps::COccupancyGridMap3D::asString() const --> std::string"); cl.def("assign", (class mrpt::maps::COccupancyGridMap3D & (mrpt::maps::COccupancyGridMap3D::*)(const class mrpt::maps::COccupancyGridMap3D &)) &mrpt::maps::COccupancyGridMap3D::operator=, "C++: mrpt::maps::COccupancyGridMap3D::operator=(const class mrpt::maps::COccupancyGridMap3D &) --> class mrpt::maps::COccupancyGridMap3D &", pybind11::return_value_policy::automatic, pybind11::arg("")); - { // mrpt::maps::COccupancyGridMap3D::TInsertionOptions file:mrpt/maps/COccupancyGridMap3D.h line:197 + { // mrpt::maps::COccupancyGridMap3D::TInsertionOptions file:mrpt/maps/COccupancyGridMap3D.h line:200 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_maps_COccupancyGridMap3D_TInsertionOptions, mrpt::config::CLoadableOptions> cl(enclosing_class, "TInsertionOptions", "With this struct options are provided to the observation insertion\n process.\n \n\n CObservation::insertIntoGridMap "); cl.def( pybind11::init( [](){ return new mrpt::maps::COccupancyGridMap3D::TInsertionOptions(); }, [](){ return new PyCallBack_mrpt_maps_COccupancyGridMap3D_TInsertionOptions(); } ) ); @@ -490,7 +490,7 @@ void bind_mrpt_maps_COccupancyGridMap3D(std::function< pybind11::module &(std::s cl.def("assign", (class mrpt::maps::COccupancyGridMap3D::TInsertionOptions & (mrpt::maps::COccupancyGridMap3D::TInsertionOptions::*)(const class mrpt::maps::COccupancyGridMap3D::TInsertionOptions &)) &mrpt::maps::COccupancyGridMap3D::TInsertionOptions::operator=, "C++: mrpt::maps::COccupancyGridMap3D::TInsertionOptions::operator=(const class mrpt::maps::COccupancyGridMap3D::TInsertionOptions &) --> class mrpt::maps::COccupancyGridMap3D::TInsertionOptions &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions file:mrpt/maps/COccupancyGridMap3D.h line:254 + { // mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions file:mrpt/maps/COccupancyGridMap3D.h line:257 auto & enclosing_class = cl; pybind11::class_, PyCallBack_mrpt_maps_COccupancyGridMap3D_TLikelihoodOptions, mrpt::config::CLoadableOptions> cl(enclosing_class, "TLikelihoodOptions", "With this struct options are provided to the observation likelihood\n computation process "); cl.def( pybind11::init( [](){ return new mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions(); }, [](){ return new PyCallBack_mrpt_maps_COccupancyGridMap3D_TLikelihoodOptions(); } ) ); @@ -511,7 +511,7 @@ void bind_mrpt_maps_COccupancyGridMap3D(std::function< pybind11::module &(std::s cl.def("assign", (class mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions & (mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::*)(const class mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions &)) &mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::operator=, "C++: mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::operator=(const class mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions &) --> class mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::maps::COccupancyGridMap3D::TRenderingOptions file:mrpt/maps/COccupancyGridMap3D.h line:308 + { // mrpt::maps::COccupancyGridMap3D::TRenderingOptions file:mrpt/maps/COccupancyGridMap3D.h line:311 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TRenderingOptions", "Options for the conversion of a mrpt::maps::COccupancyGridMap3D into a\n mrpt::opengl::COctoMapVoxels "); cl.def( pybind11::init( [](){ return new mrpt::maps::COccupancyGridMap3D::TRenderingOptions(); } ) ); diff --git a/python/src/mrpt/maps/COctoMap.cpp b/python/src/mrpt/maps/COctoMap.cpp index d2e9c76d41..f247f84d41 100644 --- a/python/src/mrpt/maps/COctoMap.cpp +++ b/python/src/mrpt/maps/COctoMap.cpp @@ -98,7 +98,7 @@ PYBIND11_MAKE_OPAQUE(std::shared_ptr) #endif -// mrpt::maps::COctoMap file:mrpt/maps/COctoMap.h line:39 +// mrpt::maps::COctoMap file:mrpt/maps/COctoMap.h line:41 struct PyCallBack_mrpt_maps_COctoMap : public mrpt::maps::COctoMap { using mrpt::maps::COctoMap::COctoMap; @@ -1105,8 +1105,8 @@ struct PyCallBack_mrpt_maps_CSimplePointsMap_TMapDefinition : public mrpt::maps: void bind_mrpt_maps_COctoMap(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::maps::COctoMap file:mrpt/maps/COctoMap.h line:39 - pybind11::class_, PyCallBack_mrpt_maps_COctoMap, mrpt::maps::COctoMapBase> cl(M("mrpt::maps"), "COctoMap", "A three-dimensional probabilistic occupancy grid, implemented as an\n octo-tree with the \"octomap\" C++ library.\n This version only stores occupancy information at each octree node. See the\n base class mrpt::maps::COctoMapBase.\n\n \n CMetricMap, the example in \"MRPT/samples/octomap_simple\"\n \n\n\n "); + { // mrpt::maps::COctoMap file:mrpt/maps/COctoMap.h line:41 + pybind11::class_, PyCallBack_mrpt_maps_COctoMap, mrpt::maps::COctoMapBase> cl(M("mrpt::maps"), "COctoMap", "A three-dimensional probabilistic occupancy grid, implemented as an\n octo-tree with the \"octomap\" C++ library.\n This version only stores occupancy information at each octree node. See the\n base class mrpt::maps::COctoMapBase.\n\n The octomap library was presented in \n\n \n CMetricMap, the example in \"MRPT/samples/octomap_simple\"\n \n\n\n "); cl.def( pybind11::init( [](){ return new mrpt::maps::COctoMap(); }, [](){ return new PyCallBack_mrpt_maps_COctoMap(); } ), "doc"); cl.def( pybind11::init(), pybind11::arg("resolution") ); @@ -1190,6 +1190,7 @@ void bind_mrpt_maps_COctoMap(std::function< pybind11::module &(std::string const cl.def( pybind11::init( [](PyCallBack_mrpt_maps_CSimplePointsMap const &o){ return new PyCallBack_mrpt_maps_CSimplePointsMap(o); } ) ); cl.def( pybind11::init( [](mrpt::maps::CSimplePointsMap const &o){ return new mrpt::maps::CSimplePointsMap(o); } ) ); + cl.def_static("Create", (class std::shared_ptr (*)()) &mrpt::maps::CSimplePointsMap::Create, "C++: mrpt::maps::CSimplePointsMap::Create() --> class std::shared_ptr"); cl.def_static("GetRuntimeClassIdStatic", (const struct mrpt::rtti::TRuntimeClassId & (*)()) &mrpt::maps::CSimplePointsMap::GetRuntimeClassIdStatic, "C++: mrpt::maps::CSimplePointsMap::GetRuntimeClassIdStatic() --> const struct mrpt::rtti::TRuntimeClassId &", pybind11::return_value_policy::automatic); cl.def("GetRuntimeClass", (const struct mrpt::rtti::TRuntimeClassId * (mrpt::maps::CSimplePointsMap::*)() const) &mrpt::maps::CSimplePointsMap::GetRuntimeClass, "C++: mrpt::maps::CSimplePointsMap::GetRuntimeClass() const --> const struct mrpt::rtti::TRuntimeClassId *", pybind11::return_value_policy::automatic); cl.def("clone", (class mrpt::rtti::CObject * (mrpt::maps::CSimplePointsMap::*)() const) &mrpt::maps::CSimplePointsMap::clone, "C++: mrpt::maps::CSimplePointsMap::clone() const --> class mrpt::rtti::CObject *", pybind11::return_value_policy::automatic); diff --git a/python/src/mrpt/maps/COctoMapBase.cpp b/python/src/mrpt/maps/COctoMapBase.cpp index 5803e22da8..df0f5fabe7 100644 --- a/python/src/mrpt/maps/COctoMapBase.cpp +++ b/python/src/mrpt/maps/COctoMapBase.cpp @@ -63,7 +63,7 @@ PYBIND11_MAKE_OPAQUE(std::shared_ptr) #endif -// mrpt::maps::COctoMapBase::TInsertionOptions file:mrpt/maps/COctoMapBase.h line:73 +// mrpt::maps::COctoMapBase::TInsertionOptions file:mrpt/maps/COctoMapBase.h line:67 struct PyCallBack_mrpt_maps_COctoMapBase_octomap_ColorOcTree_octomap_ColorOcTreeNode__TInsertionOptions : public mrpt::maps::COctoMapBase::TInsertionOptions { using mrpt::maps::COctoMapBase::TInsertionOptions::TInsertionOptions; @@ -95,7 +95,7 @@ struct PyCallBack_mrpt_maps_COctoMapBase_octomap_ColorOcTree_octomap_ColorOcTree } }; -// mrpt::maps::COctoMapBase::TLikelihoodOptions file:mrpt/maps/COctoMapBase.h line:231 +// mrpt::maps::COctoMapBase::TLikelihoodOptions file:mrpt/maps/COctoMapBase.h line:225 struct PyCallBack_mrpt_maps_COctoMapBase_octomap_ColorOcTree_octomap_ColorOcTreeNode__TLikelihoodOptions : public mrpt::maps::COctoMapBase::TLikelihoodOptions { using mrpt::maps::COctoMapBase::TLikelihoodOptions::TLikelihoodOptions; @@ -129,7 +129,7 @@ struct PyCallBack_mrpt_maps_COctoMapBase_octomap_ColorOcTree_octomap_ColorOcTree void bind_mrpt_maps_COctoMapBase(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::maps::COctoMapBase file:mrpt/maps/COctoMapBase.h line:45 + { // mrpt::maps::COctoMapBase file:mrpt/maps/COctoMapBase.h line:39 pybind11::class_, std::shared_ptr>, mrpt::maps::CMetricMap> cl(M("mrpt::maps"), "COctoMapBase_octomap_ColorOcTree_octomap_ColorOcTreeNode_t", ""); cl.def_readwrite("insertionOptions", &mrpt::maps::COctoMapBase::insertionOptions); cl.def_readwrite("likelihoodOptions", &mrpt::maps::COctoMapBase::likelihoodOptions); @@ -183,7 +183,7 @@ void bind_mrpt_maps_COctoMapBase(std::function< pybind11::module &(std::string c cl.def("getAsSimplePointsMap", (class mrpt::maps::CSimplePointsMap * (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::getAsSimplePointsMap, "C++: mrpt::maps::CMetricMap::getAsSimplePointsMap() --> class mrpt::maps::CSimplePointsMap *", pybind11::return_value_policy::automatic); cl.def("assign", (class mrpt::maps::CMetricMap & (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap &)) &mrpt::maps::CMetricMap::operator=, "C++: mrpt::maps::CMetricMap::operator=(const class mrpt::maps::CMetricMap &) --> class mrpt::maps::CMetricMap &", pybind11::return_value_policy::automatic, pybind11::arg("")); - { // mrpt::maps::COctoMapBase::TInsertionOptions file:mrpt/maps/COctoMapBase.h line:73 + { // mrpt::maps::COctoMapBase::TInsertionOptions file:mrpt/maps/COctoMapBase.h line:67 auto & enclosing_class = cl; pybind11::class_::TInsertionOptions, std::shared_ptr::TInsertionOptions>, PyCallBack_mrpt_maps_COctoMapBase_octomap_ColorOcTree_octomap_ColorOcTreeNode__TInsertionOptions, mrpt::config::CLoadableOptions> cl(enclosing_class, "TInsertionOptions", ""); cl.def( pybind11::init &>(), pybind11::arg("parent") ); @@ -212,7 +212,7 @@ void bind_mrpt_maps_COctoMapBase(std::function< pybind11::module &(std::string c cl.def("getClampingThresMaxLog", (float (mrpt::maps::COctoMapBase::TInsertionOptions::*)() const) &mrpt::maps::COctoMapBase::TInsertionOptions::getClampingThresMaxLog, "C++: mrpt::maps::COctoMapBase::TInsertionOptions::getClampingThresMaxLog() const --> float"); } - { // mrpt::maps::COctoMapBase::TLikelihoodOptions file:mrpt/maps/COctoMapBase.h line:231 + { // mrpt::maps::COctoMapBase::TLikelihoodOptions file:mrpt/maps/COctoMapBase.h line:225 auto & enclosing_class = cl; pybind11::class_::TLikelihoodOptions, std::shared_ptr::TLikelihoodOptions>, PyCallBack_mrpt_maps_COctoMapBase_octomap_ColorOcTree_octomap_ColorOcTreeNode__TLikelihoodOptions, mrpt::config::CLoadableOptions> cl(enclosing_class, "TLikelihoodOptions", ""); cl.def( pybind11::init( [](){ return new mrpt::maps::COctoMapBase::TLikelihoodOptions(); }, [](){ return new PyCallBack_mrpt_maps_COctoMapBase_octomap_ColorOcTree_octomap_ColorOcTreeNode__TLikelihoodOptions(); } ) ); @@ -225,7 +225,7 @@ void bind_mrpt_maps_COctoMapBase(std::function< pybind11::module &(std::string c cl.def("assign", (struct mrpt::maps::COctoMapBase::TLikelihoodOptions & (mrpt::maps::COctoMapBase::TLikelihoodOptions::*)(const struct mrpt::maps::COctoMapBase::TLikelihoodOptions &)) &mrpt::maps::COctoMapBase::TLikelihoodOptions::operator=, "C++: mrpt::maps::COctoMapBase::TLikelihoodOptions::operator=(const struct mrpt::maps::COctoMapBase::TLikelihoodOptions &) --> struct mrpt::maps::COctoMapBase::TLikelihoodOptions &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::maps::COctoMapBase::TRenderingOptions file:mrpt/maps/COctoMapBase.h line:259 + { // mrpt::maps::COctoMapBase::TRenderingOptions file:mrpt/maps/COctoMapBase.h line:253 auto & enclosing_class = cl; pybind11::class_::TRenderingOptions, std::shared_ptr::TRenderingOptions>> cl(enclosing_class, "TRenderingOptions", ""); cl.def( pybind11::init( [](){ return new mrpt::maps::COctoMapBase::TRenderingOptions(); } ) ); diff --git a/python/src/mrpt/maps/COctoMapBase_1.cpp b/python/src/mrpt/maps/COctoMapBase_1.cpp index c9d488f051..3a2ddf2b2a 100644 --- a/python/src/mrpt/maps/COctoMapBase_1.cpp +++ b/python/src/mrpt/maps/COctoMapBase_1.cpp @@ -63,7 +63,7 @@ PYBIND11_MAKE_OPAQUE(std::shared_ptr) #endif -// mrpt::maps::COctoMapBase::TInsertionOptions file:mrpt/maps/COctoMapBase.h line:73 +// mrpt::maps::COctoMapBase::TInsertionOptions file:mrpt/maps/COctoMapBase.h line:67 struct PyCallBack_mrpt_maps_COctoMapBase_octomap_OcTree_octomap_OcTreeNode__TInsertionOptions : public mrpt::maps::COctoMapBase::TInsertionOptions { using mrpt::maps::COctoMapBase::TInsertionOptions::TInsertionOptions; @@ -95,7 +95,7 @@ struct PyCallBack_mrpt_maps_COctoMapBase_octomap_OcTree_octomap_OcTreeNode__TIns } }; -// mrpt::maps::COctoMapBase::TLikelihoodOptions file:mrpt/maps/COctoMapBase.h line:231 +// mrpt::maps::COctoMapBase::TLikelihoodOptions file:mrpt/maps/COctoMapBase.h line:225 struct PyCallBack_mrpt_maps_COctoMapBase_octomap_OcTree_octomap_OcTreeNode__TLikelihoodOptions : public mrpt::maps::COctoMapBase::TLikelihoodOptions { using mrpt::maps::COctoMapBase::TLikelihoodOptions::TLikelihoodOptions; @@ -129,7 +129,7 @@ struct PyCallBack_mrpt_maps_COctoMapBase_octomap_OcTree_octomap_OcTreeNode__TLik void bind_mrpt_maps_COctoMapBase_1(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::maps::COctoMapBase file:mrpt/maps/COctoMapBase.h line:45 + { // mrpt::maps::COctoMapBase file:mrpt/maps/COctoMapBase.h line:39 pybind11::class_, std::shared_ptr>, mrpt::maps::CMetricMap> cl(M("mrpt::maps"), "COctoMapBase_octomap_OcTree_octomap_OcTreeNode_t", ""); cl.def_readwrite("insertionOptions", &mrpt::maps::COctoMapBase::insertionOptions); cl.def_readwrite("likelihoodOptions", &mrpt::maps::COctoMapBase::likelihoodOptions); @@ -183,7 +183,7 @@ void bind_mrpt_maps_COctoMapBase_1(std::function< pybind11::module &(std::string cl.def("getAsSimplePointsMap", (class mrpt::maps::CSimplePointsMap * (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::getAsSimplePointsMap, "C++: mrpt::maps::CMetricMap::getAsSimplePointsMap() --> class mrpt::maps::CSimplePointsMap *", pybind11::return_value_policy::automatic); cl.def("assign", (class mrpt::maps::CMetricMap & (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap &)) &mrpt::maps::CMetricMap::operator=, "C++: mrpt::maps::CMetricMap::operator=(const class mrpt::maps::CMetricMap &) --> class mrpt::maps::CMetricMap &", pybind11::return_value_policy::automatic, pybind11::arg("")); - { // mrpt::maps::COctoMapBase::TInsertionOptions file:mrpt/maps/COctoMapBase.h line:73 + { // mrpt::maps::COctoMapBase::TInsertionOptions file:mrpt/maps/COctoMapBase.h line:67 auto & enclosing_class = cl; pybind11::class_::TInsertionOptions, std::shared_ptr::TInsertionOptions>, PyCallBack_mrpt_maps_COctoMapBase_octomap_OcTree_octomap_OcTreeNode__TInsertionOptions, mrpt::config::CLoadableOptions> cl(enclosing_class, "TInsertionOptions", ""); cl.def( pybind11::init &>(), pybind11::arg("parent") ); @@ -212,7 +212,7 @@ void bind_mrpt_maps_COctoMapBase_1(std::function< pybind11::module &(std::string cl.def("getClampingThresMaxLog", (float (mrpt::maps::COctoMapBase::TInsertionOptions::*)() const) &mrpt::maps::COctoMapBase::TInsertionOptions::getClampingThresMaxLog, "C++: mrpt::maps::COctoMapBase::TInsertionOptions::getClampingThresMaxLog() const --> float"); } - { // mrpt::maps::COctoMapBase::TLikelihoodOptions file:mrpt/maps/COctoMapBase.h line:231 + { // mrpt::maps::COctoMapBase::TLikelihoodOptions file:mrpt/maps/COctoMapBase.h line:225 auto & enclosing_class = cl; pybind11::class_::TLikelihoodOptions, std::shared_ptr::TLikelihoodOptions>, PyCallBack_mrpt_maps_COctoMapBase_octomap_OcTree_octomap_OcTreeNode__TLikelihoodOptions, mrpt::config::CLoadableOptions> cl(enclosing_class, "TLikelihoodOptions", ""); cl.def( pybind11::init( [](){ return new mrpt::maps::COctoMapBase::TLikelihoodOptions(); }, [](){ return new PyCallBack_mrpt_maps_COctoMapBase_octomap_OcTree_octomap_OcTreeNode__TLikelihoodOptions(); } ) ); @@ -225,7 +225,7 @@ void bind_mrpt_maps_COctoMapBase_1(std::function< pybind11::module &(std::string cl.def("assign", (struct mrpt::maps::COctoMapBase::TLikelihoodOptions & (mrpt::maps::COctoMapBase::TLikelihoodOptions::*)(const struct mrpt::maps::COctoMapBase::TLikelihoodOptions &)) &mrpt::maps::COctoMapBase::TLikelihoodOptions::operator=, "C++: mrpt::maps::COctoMapBase::TLikelihoodOptions::operator=(const struct mrpt::maps::COctoMapBase::TLikelihoodOptions &) --> struct mrpt::maps::COctoMapBase::TLikelihoodOptions &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::maps::COctoMapBase::TRenderingOptions file:mrpt/maps/COctoMapBase.h line:259 + { // mrpt::maps::COctoMapBase::TRenderingOptions file:mrpt/maps/COctoMapBase.h line:253 auto & enclosing_class = cl; pybind11::class_::TRenderingOptions, std::shared_ptr::TRenderingOptions>> cl(enclosing_class, "TRenderingOptions", ""); cl.def( pybind11::init( [](){ return new mrpt::maps::COctoMapBase::TRenderingOptions(); } ) ); diff --git a/python/src/mrpt/maps/CVoxelMap.cpp b/python/src/mrpt/maps/CVoxelMap.cpp new file mode 100644 index 0000000000..e690fdf977 --- /dev/null +++ b/python/src/mrpt/maps/CVoxelMap.cpp @@ -0,0 +1,757 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include // __str__ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + + +#ifndef BINDER_PYBIND11_TYPE_CASTER + #define BINDER_PYBIND11_TYPE_CASTER + PYBIND11_DECLARE_HOLDER_TYPE(T, std::shared_ptr) + PYBIND11_DECLARE_HOLDER_TYPE(T, T*) + PYBIND11_MAKE_OPAQUE(std::shared_ptr) +#endif + +// mrpt::maps::CVoxelMap file:mrpt/maps/CVoxelMap.h line:33 +struct PyCallBack_mrpt_maps_CVoxelMap : public mrpt::maps::CVoxelMap { + using mrpt::maps::CVoxelMap::CVoxelMap; + + const struct mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "GetRuntimeClass"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMap::GetRuntimeClass(); + } + class mrpt::rtti::CObject * clone() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "clone"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMap::clone(); + } + uint8_t serializeGetVersion() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "serializeGetVersion"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMap::serializeGetVersion(); + } + void serializeTo(class mrpt::serialization::CArchive & a0) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "serializeTo"); + if (overload) { + auto o = overload.operator()(a0); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMap::serializeTo(a0); + } + void serializeFrom(class mrpt::serialization::CArchive & a0, uint8_t a1) override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "serializeFrom"); + if (overload) { + auto o = overload.operator()(a0, a1); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMap::serializeFrom(a0, a1); + } + double internal_computeObservationLikelihood(const class mrpt::obs::CObservation & a0, const class mrpt::poses::CPose3D & a1) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "internal_computeObservationLikelihood"); + if (overload) { + auto o = overload.operator()(a0, a1); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMap::internal_computeObservationLikelihood(a0, a1); + } + bool isEmpty() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "isEmpty"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapOccupancyBase::isEmpty(); + } + void getAsOctoMapVoxels(class mrpt::opengl::COctoMapVoxels & a0) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "getAsOctoMapVoxels"); + if (overload) { + auto o = overload.operator()(a0); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapOccupancyBase::getAsOctoMapVoxels(a0); + } + void internal_clear() override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "internal_clear"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapOccupancyBase::internal_clear(); + } + std::string asString() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "asString"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapBase::asString(); + } + void getVisualizationInto(class mrpt::opengl::CSetOfObjects & a0) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "getVisualizationInto"); + if (overload) { + auto o = overload.operator()(a0); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapBase::getVisualizationInto(a0); + } + void saveMetricMapRepresentationToFile(const std::string & a0) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "saveMetricMapRepresentationToFile"); + if (overload) { + auto o = overload.operator()(a0); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapBase::saveMetricMapRepresentationToFile(a0); + } + bool canComputeObservationLikelihood(const class mrpt::obs::CObservation & a0) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "canComputeObservationLikelihood"); + if (overload) { + auto o = overload.operator()(a0); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CMetricMap::canComputeObservationLikelihood(a0); + } + void determineMatching2D(const class mrpt::maps::CMetricMap * a0, const class mrpt::poses::CPose2D & a1, class mrpt::tfest::TMatchingPairListTempl & a2, const struct mrpt::maps::TMatchingParams & a3, struct mrpt::maps::TMatchingExtraResults & a4) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "determineMatching2D"); + if (overload) { + auto o = overload.operator()(a0, a1, a2, a3, a4); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CMetricMap::determineMatching2D(a0, a1, a2, a3, a4); + } + void determineMatching3D(const class mrpt::maps::CMetricMap * a0, const class mrpt::poses::CPose3D & a1, class mrpt::tfest::TMatchingPairListTempl & a2, const struct mrpt::maps::TMatchingParams & a3, struct mrpt::maps::TMatchingExtraResults & a4) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "determineMatching3D"); + if (overload) { + auto o = overload.operator()(a0, a1, a2, a3, a4); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CMetricMap::determineMatching3D(a0, a1, a2, a3, a4); + } + float compute3DMatchingRatio(const class mrpt::maps::CMetricMap * a0, const class mrpt::poses::CPose3D & a1, const struct mrpt::maps::TMatchingRatioParams & a2) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "compute3DMatchingRatio"); + if (overload) { + auto o = overload.operator()(a0, a1, a2); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CMetricMap::compute3DMatchingRatio(a0, a1, a2); + } + void auxParticleFilterCleanUp() override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "auxParticleFilterCleanUp"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CMetricMap::auxParticleFilterCleanUp(); + } + float squareDistanceToClosestCorrespondence(float a0, float a1) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "squareDistanceToClosestCorrespondence"); + if (overload) { + auto o = overload.operator()(a0, a1); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CMetricMap::squareDistanceToClosestCorrespondence(a0, a1); + } +}; + +// mrpt::maps::CVoxelMap::TMapDefinition file: line:67 +struct PyCallBack_mrpt_maps_CVoxelMap_TMapDefinition : public mrpt::maps::CVoxelMap::TMapDefinition { + using mrpt::maps::CVoxelMap::TMapDefinition::TMapDefinition; + + void loadFromConfigFile_map_specific(const class mrpt::config::CConfigFileBase & a0, const std::string & a1) override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "loadFromConfigFile_map_specific"); + if (overload) { + auto o = overload.operator()(a0, a1); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return TMapDefinition::loadFromConfigFile_map_specific(a0, a1); + } + void loadFromConfigFile(const class mrpt::config::CConfigFileBase & a0, const std::string & a1) override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "loadFromConfigFile"); + if (overload) { + auto o = overload.operator()(a0, a1); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return TMetricMapInitializer::loadFromConfigFile(a0, a1); + } + void saveToConfigFile(class mrpt::config::CConfigFileBase & a0, const std::string & a1) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "saveToConfigFile"); + if (overload) { + auto o = overload.operator()(a0, a1); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return TMetricMapInitializer::saveToConfigFile(a0, a1); + } +}; + +// mrpt::maps::CVoxelMapRGB file:mrpt/maps/CVoxelMapRGB.h line:39 +struct PyCallBack_mrpt_maps_CVoxelMapRGB : public mrpt::maps::CVoxelMapRGB { + using mrpt::maps::CVoxelMapRGB::CVoxelMapRGB; + + const struct mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "GetRuntimeClass"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapRGB::GetRuntimeClass(); + } + class mrpt::rtti::CObject * clone() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "clone"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapRGB::clone(); + } + uint8_t serializeGetVersion() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "serializeGetVersion"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapRGB::serializeGetVersion(); + } + void serializeTo(class mrpt::serialization::CArchive & a0) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "serializeTo"); + if (overload) { + auto o = overload.operator()(a0); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapRGB::serializeTo(a0); + } + void serializeFrom(class mrpt::serialization::CArchive & a0, uint8_t a1) override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "serializeFrom"); + if (overload) { + auto o = overload.operator()(a0, a1); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapRGB::serializeFrom(a0, a1); + } + double internal_computeObservationLikelihood(const class mrpt::obs::CObservation & a0, const class mrpt::poses::CPose3D & a1) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "internal_computeObservationLikelihood"); + if (overload) { + auto o = overload.operator()(a0, a1); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapRGB::internal_computeObservationLikelihood(a0, a1); + } + bool isEmpty() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "isEmpty"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapOccupancyBase::isEmpty(); + } + void getAsOctoMapVoxels(class mrpt::opengl::COctoMapVoxels & a0) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "getAsOctoMapVoxels"); + if (overload) { + auto o = overload.operator()(a0); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapOccupancyBase::getAsOctoMapVoxels(a0); + } + void internal_clear() override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "internal_clear"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapOccupancyBase::internal_clear(); + } + std::string asString() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "asString"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapBase::asString(); + } + void getVisualizationInto(class mrpt::opengl::CSetOfObjects & a0) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "getVisualizationInto"); + if (overload) { + auto o = overload.operator()(a0); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapBase::getVisualizationInto(a0); + } + void saveMetricMapRepresentationToFile(const std::string & a0) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "saveMetricMapRepresentationToFile"); + if (overload) { + auto o = overload.operator()(a0); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CVoxelMapBase::saveMetricMapRepresentationToFile(a0); + } + bool canComputeObservationLikelihood(const class mrpt::obs::CObservation & a0) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "canComputeObservationLikelihood"); + if (overload) { + auto o = overload.operator()(a0); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CMetricMap::canComputeObservationLikelihood(a0); + } + void determineMatching2D(const class mrpt::maps::CMetricMap * a0, const class mrpt::poses::CPose2D & a1, class mrpt::tfest::TMatchingPairListTempl & a2, const struct mrpt::maps::TMatchingParams & a3, struct mrpt::maps::TMatchingExtraResults & a4) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "determineMatching2D"); + if (overload) { + auto o = overload.operator()(a0, a1, a2, a3, a4); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CMetricMap::determineMatching2D(a0, a1, a2, a3, a4); + } + void determineMatching3D(const class mrpt::maps::CMetricMap * a0, const class mrpt::poses::CPose3D & a1, class mrpt::tfest::TMatchingPairListTempl & a2, const struct mrpt::maps::TMatchingParams & a3, struct mrpt::maps::TMatchingExtraResults & a4) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "determineMatching3D"); + if (overload) { + auto o = overload.operator()(a0, a1, a2, a3, a4); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CMetricMap::determineMatching3D(a0, a1, a2, a3, a4); + } + float compute3DMatchingRatio(const class mrpt::maps::CMetricMap * a0, const class mrpt::poses::CPose3D & a1, const struct mrpt::maps::TMatchingRatioParams & a2) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "compute3DMatchingRatio"); + if (overload) { + auto o = overload.operator()(a0, a1, a2); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CMetricMap::compute3DMatchingRatio(a0, a1, a2); + } + void auxParticleFilterCleanUp() override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "auxParticleFilterCleanUp"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CMetricMap::auxParticleFilterCleanUp(); + } + float squareDistanceToClosestCorrespondence(float a0, float a1) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "squareDistanceToClosestCorrespondence"); + if (overload) { + auto o = overload.operator()(a0, a1); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CMetricMap::squareDistanceToClosestCorrespondence(a0, a1); + } +}; + +// mrpt::maps::CVoxelMapRGB::TMapDefinition file: line:67 +struct PyCallBack_mrpt_maps_CVoxelMapRGB_TMapDefinition : public mrpt::maps::CVoxelMapRGB::TMapDefinition { + using mrpt::maps::CVoxelMapRGB::TMapDefinition::TMapDefinition; + + void loadFromConfigFile_map_specific(const class mrpt::config::CConfigFileBase & a0, const std::string & a1) override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "loadFromConfigFile_map_specific"); + if (overload) { + auto o = overload.operator()(a0, a1); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return TMapDefinition::loadFromConfigFile_map_specific(a0, a1); + } + void loadFromConfigFile(const class mrpt::config::CConfigFileBase & a0, const std::string & a1) override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "loadFromConfigFile"); + if (overload) { + auto o = overload.operator()(a0, a1); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return TMetricMapInitializer::loadFromConfigFile(a0, a1); + } + void saveToConfigFile(class mrpt::config::CConfigFileBase & a0, const std::string & a1) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "saveToConfigFile"); + if (overload) { + auto o = overload.operator()(a0, a1); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return TMetricMapInitializer::saveToConfigFile(a0, a1); + } +}; + +void bind_mrpt_maps_CVoxelMap(std::function< pybind11::module &(std::string const &namespace_) > &M) +{ + { // mrpt::maps::VoxelNodeOccupancy file:mrpt/maps/CVoxelMap.h line:18 + pybind11::class_> cl(M("mrpt::maps"), "VoxelNodeOccupancy", "Voxel contents for CVoxelMap"); + cl.def( pybind11::init( [](){ return new mrpt::maps::VoxelNodeOccupancy(); } ) ); + cl.def_readwrite("occupancy", &mrpt::maps::VoxelNodeOccupancy::occupancy); + cl.def("occupancyRef", (signed char & (mrpt::maps::VoxelNodeOccupancy::*)()) &mrpt::maps::VoxelNodeOccupancy::occupancyRef, "C++: mrpt::maps::VoxelNodeOccupancy::occupancyRef() --> signed char &", pybind11::return_value_policy::automatic); + } + { // mrpt::maps::CVoxelMap file:mrpt/maps/CVoxelMap.h line:33 + pybind11::class_, PyCallBack_mrpt_maps_CVoxelMap, mrpt::maps::CVoxelMapOccupancyBase> cl(M("mrpt::maps"), "CVoxelMap", "Log-odds sparse voxel map for cells containing only the *occupancy* of each\n voxel.\n\n \n\n "); + cl.def( pybind11::init( [](){ return new mrpt::maps::CVoxelMap(); }, [](){ return new PyCallBack_mrpt_maps_CVoxelMap(); } ), "doc"); + cl.def( pybind11::init( [](double const & a0){ return new mrpt::maps::CVoxelMap(a0); }, [](double const & a0){ return new PyCallBack_mrpt_maps_CVoxelMap(a0); } ), "doc"); + cl.def( pybind11::init( [](double const & a0, uint8_t const & a1){ return new mrpt::maps::CVoxelMap(a0, a1); }, [](double const & a0, uint8_t const & a1){ return new PyCallBack_mrpt_maps_CVoxelMap(a0, a1); } ), "doc"); + cl.def( pybind11::init(), pybind11::arg("resolution"), pybind11::arg("inner_bits"), pybind11::arg("leaf_bits") ); + + cl.def( pybind11::init( [](PyCallBack_mrpt_maps_CVoxelMap const &o){ return new PyCallBack_mrpt_maps_CVoxelMap(o); } ) ); + cl.def( pybind11::init( [](mrpt::maps::CVoxelMap const &o){ return new mrpt::maps::CVoxelMap(o); } ) ); + cl.def_static("GetRuntimeClassIdStatic", (const struct mrpt::rtti::TRuntimeClassId & (*)()) &mrpt::maps::CVoxelMap::GetRuntimeClassIdStatic, "C++: mrpt::maps::CVoxelMap::GetRuntimeClassIdStatic() --> const struct mrpt::rtti::TRuntimeClassId &", pybind11::return_value_policy::automatic); + cl.def("GetRuntimeClass", (const struct mrpt::rtti::TRuntimeClassId * (mrpt::maps::CVoxelMap::*)() const) &mrpt::maps::CVoxelMap::GetRuntimeClass, "C++: mrpt::maps::CVoxelMap::GetRuntimeClass() const --> const struct mrpt::rtti::TRuntimeClassId *", pybind11::return_value_policy::automatic); + cl.def("clone", (class mrpt::rtti::CObject * (mrpt::maps::CVoxelMap::*)() const) &mrpt::maps::CVoxelMap::clone, "C++: mrpt::maps::CVoxelMap::clone() const --> class mrpt::rtti::CObject *", pybind11::return_value_policy::automatic); + cl.def_static("CreateObject", (class std::shared_ptr (*)()) &mrpt::maps::CVoxelMap::CreateObject, "C++: mrpt::maps::CVoxelMap::CreateObject() --> class std::shared_ptr"); + cl.def("assign", (class mrpt::maps::CVoxelMap & (mrpt::maps::CVoxelMap::*)(const class mrpt::maps::CVoxelMap &)) &mrpt::maps::CVoxelMap::operator=, "C++: mrpt::maps::CVoxelMap::operator=(const class mrpt::maps::CVoxelMap &) --> class mrpt::maps::CVoxelMap &", pybind11::return_value_policy::automatic, pybind11::arg("")); + + { // mrpt::maps::CVoxelMap::TMapDefinitionBase file: line:62 + auto & enclosing_class = cl; + pybind11::class_> cl(enclosing_class, "TMapDefinitionBase", ""); + } + + { // mrpt::maps::CVoxelMap::TMapDefinition file: line:67 + auto & enclosing_class = cl; + pybind11::class_, PyCallBack_mrpt_maps_CVoxelMap_TMapDefinition, mrpt::maps::CVoxelMap::TMapDefinitionBase> cl(enclosing_class, "TMapDefinition", ""); + cl.def( pybind11::init( [](){ return new mrpt::maps::CVoxelMap::TMapDefinition(); }, [](){ return new PyCallBack_mrpt_maps_CVoxelMap_TMapDefinition(); } ) ); + cl.def( pybind11::init( [](PyCallBack_mrpt_maps_CVoxelMap_TMapDefinition const &o){ return new PyCallBack_mrpt_maps_CVoxelMap_TMapDefinition(o); } ) ); + cl.def( pybind11::init( [](mrpt::maps::CVoxelMap::TMapDefinition const &o){ return new mrpt::maps::CVoxelMap::TMapDefinition(o); } ) ); + cl.def_readwrite("resolution", &mrpt::maps::CVoxelMap::TMapDefinition::resolution); + cl.def_readwrite("inner_bits", &mrpt::maps::CVoxelMap::TMapDefinition::inner_bits); + cl.def_readwrite("leaf_bits", &mrpt::maps::CVoxelMap::TMapDefinition::leaf_bits); + cl.def_readwrite("insertionOpts", &mrpt::maps::CVoxelMap::TMapDefinition::insertionOpts); + cl.def_readwrite("likelihoodOpts", &mrpt::maps::CVoxelMap::TMapDefinition::likelihoodOpts); + } + + } + { // mrpt::maps::VoxelNodeOccRGB file:mrpt/maps/CVoxelMapRGB.h line:19 + pybind11::class_> cl(M("mrpt::maps"), "VoxelNodeOccRGB", "Voxel contents for CVoxelMapRGB"); + cl.def( pybind11::init( [](){ return new mrpt::maps::VoxelNodeOccRGB(); } ) ); + cl.def_readwrite("occupancy", &mrpt::maps::VoxelNodeOccRGB::occupancy); + cl.def_readwrite("color", &mrpt::maps::VoxelNodeOccRGB::color); + cl.def_readwrite("numColObs", &mrpt::maps::VoxelNodeOccRGB::numColObs); + cl.def("occupancyRef", (signed char & (mrpt::maps::VoxelNodeOccRGB::*)()) &mrpt::maps::VoxelNodeOccRGB::occupancyRef, "C++: mrpt::maps::VoxelNodeOccRGB::occupancyRef() --> signed char &", pybind11::return_value_policy::automatic); + + { // mrpt::maps::VoxelNodeOccRGB::TColor file:mrpt/maps/CVoxelMapRGB.h line:22 + auto & enclosing_class = cl; + pybind11::class_> cl(enclosing_class, "TColor", ""); + cl.def( pybind11::init( [](){ return new mrpt::maps::VoxelNodeOccRGB::TColor(); } ) ); + cl.def_readwrite("R", &mrpt::maps::VoxelNodeOccRGB::TColor::R); + cl.def_readwrite("G", &mrpt::maps::VoxelNodeOccRGB::TColor::G); + cl.def_readwrite("B", &mrpt::maps::VoxelNodeOccRGB::TColor::B); + } + + } + { // mrpt::maps::CVoxelMapRGB file:mrpt/maps/CVoxelMapRGB.h line:39 + pybind11::class_, PyCallBack_mrpt_maps_CVoxelMapRGB, mrpt::maps::CVoxelMapOccupancyBase> cl(M("mrpt::maps"), "CVoxelMapRGB", "Log-odds sparse voxel map for cells containing the occupancy *and* an RGB\n color for each voxel.\n\n \n\n "); + cl.def( pybind11::init( [](){ return new mrpt::maps::CVoxelMapRGB(); }, [](){ return new PyCallBack_mrpt_maps_CVoxelMapRGB(); } ), "doc"); + cl.def( pybind11::init( [](double const & a0){ return new mrpt::maps::CVoxelMapRGB(a0); }, [](double const & a0){ return new PyCallBack_mrpt_maps_CVoxelMapRGB(a0); } ), "doc"); + cl.def( pybind11::init( [](double const & a0, uint8_t const & a1){ return new mrpt::maps::CVoxelMapRGB(a0, a1); }, [](double const & a0, uint8_t const & a1){ return new PyCallBack_mrpt_maps_CVoxelMapRGB(a0, a1); } ), "doc"); + cl.def( pybind11::init(), pybind11::arg("resolution"), pybind11::arg("inner_bits"), pybind11::arg("leaf_bits") ); + + cl.def( pybind11::init( [](PyCallBack_mrpt_maps_CVoxelMapRGB const &o){ return new PyCallBack_mrpt_maps_CVoxelMapRGB(o); } ) ); + cl.def( pybind11::init( [](mrpt::maps::CVoxelMapRGB const &o){ return new mrpt::maps::CVoxelMapRGB(o); } ) ); + cl.def_static("GetRuntimeClassIdStatic", (const struct mrpt::rtti::TRuntimeClassId & (*)()) &mrpt::maps::CVoxelMapRGB::GetRuntimeClassIdStatic, "C++: mrpt::maps::CVoxelMapRGB::GetRuntimeClassIdStatic() --> const struct mrpt::rtti::TRuntimeClassId &", pybind11::return_value_policy::automatic); + cl.def("GetRuntimeClass", (const struct mrpt::rtti::TRuntimeClassId * (mrpt::maps::CVoxelMapRGB::*)() const) &mrpt::maps::CVoxelMapRGB::GetRuntimeClass, "C++: mrpt::maps::CVoxelMapRGB::GetRuntimeClass() const --> const struct mrpt::rtti::TRuntimeClassId *", pybind11::return_value_policy::automatic); + cl.def("clone", (class mrpt::rtti::CObject * (mrpt::maps::CVoxelMapRGB::*)() const) &mrpt::maps::CVoxelMapRGB::clone, "C++: mrpt::maps::CVoxelMapRGB::clone() const --> class mrpt::rtti::CObject *", pybind11::return_value_policy::automatic); + cl.def_static("CreateObject", (class std::shared_ptr (*)()) &mrpt::maps::CVoxelMapRGB::CreateObject, "C++: mrpt::maps::CVoxelMapRGB::CreateObject() --> class std::shared_ptr"); + cl.def("assign", (class mrpt::maps::CVoxelMapRGB & (mrpt::maps::CVoxelMapRGB::*)(const class mrpt::maps::CVoxelMapRGB &)) &mrpt::maps::CVoxelMapRGB::operator=, "C++: mrpt::maps::CVoxelMapRGB::operator=(const class mrpt::maps::CVoxelMapRGB &) --> class mrpt::maps::CVoxelMapRGB &", pybind11::return_value_policy::automatic, pybind11::arg("")); + + { // mrpt::maps::CVoxelMapRGB::TMapDefinitionBase file: line:62 + auto & enclosing_class = cl; + pybind11::class_> cl(enclosing_class, "TMapDefinitionBase", ""); + } + + { // mrpt::maps::CVoxelMapRGB::TMapDefinition file: line:67 + auto & enclosing_class = cl; + pybind11::class_, PyCallBack_mrpt_maps_CVoxelMapRGB_TMapDefinition, mrpt::maps::CVoxelMapRGB::TMapDefinitionBase> cl(enclosing_class, "TMapDefinition", ""); + cl.def( pybind11::init( [](){ return new mrpt::maps::CVoxelMapRGB::TMapDefinition(); }, [](){ return new PyCallBack_mrpt_maps_CVoxelMapRGB_TMapDefinition(); } ) ); + cl.def( pybind11::init( [](PyCallBack_mrpt_maps_CVoxelMapRGB_TMapDefinition const &o){ return new PyCallBack_mrpt_maps_CVoxelMapRGB_TMapDefinition(o); } ) ); + cl.def( pybind11::init( [](mrpt::maps::CVoxelMapRGB::TMapDefinition const &o){ return new mrpt::maps::CVoxelMapRGB::TMapDefinition(o); } ) ); + cl.def_readwrite("resolution", &mrpt::maps::CVoxelMapRGB::TMapDefinition::resolution); + cl.def_readwrite("inner_bits", &mrpt::maps::CVoxelMapRGB::TMapDefinition::inner_bits); + cl.def_readwrite("leaf_bits", &mrpt::maps::CVoxelMapRGB::TMapDefinition::leaf_bits); + cl.def_readwrite("insertionOpts", &mrpt::maps::CVoxelMapRGB::TMapDefinition::insertionOpts); + cl.def_readwrite("likelihoodOpts", &mrpt::maps::CVoxelMapRGB::TMapDefinition::likelihoodOpts); + } + + } +} diff --git a/python/src/mrpt/maps/CVoxelMapBase.cpp b/python/src/mrpt/maps/CVoxelMapBase.cpp new file mode 100644 index 0000000000..ccbec05d1e --- /dev/null +++ b/python/src/mrpt/maps/CVoxelMapBase.cpp @@ -0,0 +1,103 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include // __str__ +#include +#include +#include + +#include +#include +#include +#include + + +#ifndef BINDER_PYBIND11_TYPE_CASTER + #define BINDER_PYBIND11_TYPE_CASTER + PYBIND11_DECLARE_HOLDER_TYPE(T, std::shared_ptr) + PYBIND11_DECLARE_HOLDER_TYPE(T, T*) + PYBIND11_MAKE_OPAQUE(std::shared_ptr) +#endif + +void bind_mrpt_maps_CVoxelMapBase(std::function< pybind11::module &(std::string const &namespace_) > &M) +{ + { // mrpt::maps::CVoxelMapBase file:mrpt/maps/CVoxelMapBase.h line:42 + pybind11::class_, std::shared_ptr>, mrpt::maps::CMetricMap> cl(M("mrpt::maps"), "CVoxelMapBase_mrpt_maps_VoxelNodeOccupancy_t", ""); + cl.def("assign", (class mrpt::maps::CVoxelMapBase & (mrpt::maps::CVoxelMapBase::*)(const class mrpt::maps::CVoxelMapBase &)) &mrpt::maps::CVoxelMapBase::operator=, "C++: mrpt::maps::CVoxelMapBase::operator=(const class mrpt::maps::CVoxelMapBase &) --> class mrpt::maps::CVoxelMapBase &", pybind11::return_value_policy::automatic, pybind11::arg("o")); + cl.def("asString", (std::string (mrpt::maps::CVoxelMapBase::*)() const) &mrpt::maps::CVoxelMapBase::asString, "C++: mrpt::maps::CVoxelMapBase::asString() const --> std::string"); + cl.def("getVisualizationInto", (void (mrpt::maps::CVoxelMapBase::*)(class mrpt::opengl::CSetOfObjects &) const) &mrpt::maps::CVoxelMapBase::getVisualizationInto, "C++: mrpt::maps::CVoxelMapBase::getVisualizationInto(class mrpt::opengl::CSetOfObjects &) const --> void", pybind11::arg("o")); + cl.def("getAsOctoMapVoxels", (void (mrpt::maps::CVoxelMapBase::*)(class mrpt::opengl::COctoMapVoxels &) const) &mrpt::maps::CVoxelMapBase::getAsOctoMapVoxels, "C++: mrpt::maps::CVoxelMapBase::getAsOctoMapVoxels(class mrpt::opengl::COctoMapVoxels &) const --> void", pybind11::arg("gl_obj")); + cl.def("saveMetricMapRepresentationToFile", (void (mrpt::maps::CVoxelMapBase::*)(const std::string &) const) &mrpt::maps::CVoxelMapBase::saveMetricMapRepresentationToFile, "C++: mrpt::maps::CVoxelMapBase::saveMetricMapRepresentationToFile(const std::string &) const --> void", pybind11::arg("filNamePrefix")); + cl.def_readwrite("genericMapParams", &mrpt::maps::CMetricMap::genericMapParams); + cl.def("GetRuntimeClass", (const struct mrpt::rtti::TRuntimeClassId * (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::GetRuntimeClass, "C++: mrpt::maps::CMetricMap::GetRuntimeClass() const --> const struct mrpt::rtti::TRuntimeClassId *", pybind11::return_value_policy::automatic); + cl.def_static("GetRuntimeClassIdStatic", (const struct mrpt::rtti::TRuntimeClassId & (*)()) &mrpt::maps::CMetricMap::GetRuntimeClassIdStatic, "C++: mrpt::maps::CMetricMap::GetRuntimeClassIdStatic() --> const struct mrpt::rtti::TRuntimeClassId &", pybind11::return_value_policy::automatic); + cl.def("clear", (void (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::clear, "Erase all the contents of the map \n\nC++: mrpt::maps::CMetricMap::clear() --> void"); + cl.def("isEmpty", (bool (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::isEmpty, "Returns true if the map is empty/no observation has been inserted.\n\nC++: mrpt::maps::CMetricMap::isEmpty() const --> bool"); + cl.def("loadFromProbabilisticPosesAndObservations", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations, "Load the map contents from a CSimpleMap object, erasing all previous\n content of the map. This is done invoking `insertObservation()` for each\n observation at the mean 3D robot pose of each pose-observations pair in\n the CSimpleMap object.\n\n \n insertObservation, CSimpleMap\n \n\n std::exception Some internal steps in invoked methods can\n raise exceptions on invalid parameters, etc...\n\nC++: mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); + cl.def("loadFromSimpleMap", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromSimpleMap, "! \n\nC++: mrpt::maps::CMetricMap::loadFromSimpleMap(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); + cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CObservation & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("obs")); + cl.def("insertObs", (bool (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D *)) &mrpt::maps::CMetricMap::insertObs, "C++: mrpt::maps::CMetricMap::insertObs(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D *) --> bool", pybind11::arg("obs"), pybind11::arg("robotPose")); + cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CSensoryFrame & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("sf")); + cl.def("insertObs", (bool (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CSensoryFrame &, const class mrpt::poses::CPose3D *)) &mrpt::maps::CMetricMap::insertObs, "C++: mrpt::maps::CMetricMap::insertObs(const class mrpt::obs::CSensoryFrame &, const class mrpt::poses::CPose3D *) --> bool", pybind11::arg("sf"), pybind11::arg("robotPose")); + cl.def("computeObservationLikelihood", (double (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D &) const) &mrpt::maps::CMetricMap::computeObservationLikelihood, "Computes the log-likelihood of a given observation given an arbitrary\n robot 3D pose.\n See: \n\n \n The robot's pose the observation is supposed to be taken\n from.\n \n\n The observation.\n \n\n This method returns a log-likelihood.\n\n \n Used in particle filter algorithms, see: CMultiMetricMapPDF::update\n\nC++: mrpt::maps::CMetricMap::computeObservationLikelihood(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D &) const --> double", pybind11::arg("obs"), pybind11::arg("takenFrom")); + cl.def("canComputeObservationLikelihood", (bool (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CObservation &) const) &mrpt::maps::CMetricMap::canComputeObservationLikelihood, "Returns true if this map is able to compute a sensible likelihood\n function for this observation (i.e. an occupancy grid map cannot with an\n image).\n See: \n\n \n The observation.\n \n\n computeObservationLikelihood,\n genericMapParams.enableObservationLikelihood\n\nC++: mrpt::maps::CMetricMap::canComputeObservationLikelihood(const class mrpt::obs::CObservation &) const --> bool", pybind11::arg("obs")); + cl.def("computeObservationsLikelihood", (double (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CSensoryFrame &, const class mrpt::poses::CPose3D &)) &mrpt::maps::CMetricMap::computeObservationsLikelihood, "Returns the sum of the log-likelihoods of each individual observation\n within a mrpt::obs::CSensoryFrame.\n See: \n\n \n The robot's pose the observation is supposed to be taken\n from.\n \n\n The set of observations in a CSensoryFrame.\n \n\n This method returns a log-likelihood.\n \n\n canComputeObservationsLikelihood\n\nC++: mrpt::maps::CMetricMap::computeObservationsLikelihood(const class mrpt::obs::CSensoryFrame &, const class mrpt::poses::CPose3D &) --> double", pybind11::arg("sf"), pybind11::arg("takenFrom")); + cl.def("canComputeObservationsLikelihood", (bool (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CSensoryFrame &) const) &mrpt::maps::CMetricMap::canComputeObservationsLikelihood, "Returns true if this map is able to compute a sensible likelihood\n function for this observation (i.e. an occupancy grid map cannot with an\n image).\n See: \n\n \n The observations.\n \n\n canComputeObservationLikelihood\n\nC++: mrpt::maps::CMetricMap::canComputeObservationsLikelihood(const class mrpt::obs::CSensoryFrame &) const --> bool", pybind11::arg("sf")); + cl.def("determineMatching2D", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose2D &, class mrpt::tfest::TMatchingPairListTempl &, const struct mrpt::maps::TMatchingParams &, struct mrpt::maps::TMatchingExtraResults &) const) &mrpt::maps::CMetricMap::determineMatching2D, "Computes the matching between this and another 2D point map, which\nincludes finding:\n - The set of points pairs in each map\n - The mean squared distance between corresponding pairs.\n\n The algorithm is:\n - For each point in \"otherMap\":\n - Transform the point according to otherMapPose\n - Search with a KD-TREE the closest correspondences in \"this\"\nmap.\n - Add to the set of candidate matchings, if it passes all the\nthresholds in params.\n\n This method is the most time critical one into ICP-like algorithms.\n\n \n [IN] The other map to compute the matching with.\n \n\n [IN] The pose of the other map as seen from\n\"this\".\n \n\n [IN] Parameters for the determination of\npairings.\n \n\n [OUT] The detected matchings pairs.\n \n\n [OUT] Other results.\n \n\n compute3DMatchingRatio\n\nC++: mrpt::maps::CMetricMap::determineMatching2D(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose2D &, class mrpt::tfest::TMatchingPairListTempl &, const struct mrpt::maps::TMatchingParams &, struct mrpt::maps::TMatchingExtraResults &) const --> void", pybind11::arg("otherMap"), pybind11::arg("otherMapPose"), pybind11::arg("correspondences"), pybind11::arg("params"), pybind11::arg("extraResults")); + cl.def("determineMatching3D", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, class mrpt::tfest::TMatchingPairListTempl &, const struct mrpt::maps::TMatchingParams &, struct mrpt::maps::TMatchingExtraResults &) const) &mrpt::maps::CMetricMap::determineMatching3D, "Computes the matchings between this and another 3D points map - method\nused in 3D-ICP.\n This method finds the set of point pairs in each map.\n\n The method is the most time critical one into ICP-like algorithms.\n\n The algorithm is:\n - For each point in \"otherMap\":\n - Transform the point according to otherMapPose\n - Search with a KD-TREE the closest correspondences in \"this\"\nmap.\n - Add to the set of candidate matchings, if it passes all the\nthresholds in params.\n\n \n [IN] The other map to compute the matching with.\n \n\n [IN] The pose of the other map as seen from\n\"this\".\n \n\n [IN] Parameters for the determination of\npairings.\n \n\n [OUT] The detected matchings pairs.\n \n\n [OUT] Other results.\n \n\n compute3DMatchingRatio\n\nC++: mrpt::maps::CMetricMap::determineMatching3D(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, class mrpt::tfest::TMatchingPairListTempl &, const struct mrpt::maps::TMatchingParams &, struct mrpt::maps::TMatchingExtraResults &) const --> void", pybind11::arg("otherMap"), pybind11::arg("otherMapPose"), pybind11::arg("correspondences"), pybind11::arg("params"), pybind11::arg("extraResults")); + cl.def("compute3DMatchingRatio", (float (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, const struct mrpt::maps::TMatchingRatioParams &) const) &mrpt::maps::CMetricMap::compute3DMatchingRatio, "Computes the ratio in [0,1] of correspondences between \"this\" and the\n \"otherMap\" map, whose 6D pose relative to \"this\" is \"otherMapPose\"\n In the case of a multi-metric map, this returns the average between the\n maps. This method always return 0 for grid maps.\n \n\n [IN] The other map to compute the matching with.\n \n\n [IN] The 6D pose of the other map as seen from\n \"this\".\n \n\n [IN] Matching parameters\n \n\n The matching ratio [0,1]\n \n\n determineMatching2D\n\nC++: mrpt::maps::CMetricMap::compute3DMatchingRatio(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, const struct mrpt::maps::TMatchingRatioParams &) const --> float", pybind11::arg("otherMap"), pybind11::arg("otherMapPose"), pybind11::arg("params")); + cl.def("saveMetricMapRepresentationToFile", (void (mrpt::maps::CMetricMap::*)(const std::string &) const) &mrpt::maps::CMetricMap::saveMetricMapRepresentationToFile, "This virtual method saves the map to a file \"filNamePrefix\"+<\n some_file_extension >, as an image or in any other applicable way (Notice\n that other methods to save the map may be implemented in classes\n implementing this virtual interface). \n\nC++: mrpt::maps::CMetricMap::saveMetricMapRepresentationToFile(const std::string &) const --> void", pybind11::arg("filNamePrefix")); + cl.def("auxParticleFilterCleanUp", (void (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::auxParticleFilterCleanUp, "This method is called at the end of each \"prediction-update-map\n insertion\" cycle within\n \"mrpt::slam::CMetricMapBuilderRBPF::processActionObservation\".\n This method should normally do nothing, but in some cases can be used\n to free auxiliary cached variables.\n\nC++: mrpt::maps::CMetricMap::auxParticleFilterCleanUp() --> void"); + cl.def("squareDistanceToClosestCorrespondence", (float (mrpt::maps::CMetricMap::*)(float, float) const) &mrpt::maps::CMetricMap::squareDistanceToClosestCorrespondence, "Returns the square distance from the 2D point (x0,y0) to the closest\n correspondence in the map. \n\nC++: mrpt::maps::CMetricMap::squareDistanceToClosestCorrespondence(float, float) const --> float", pybind11::arg("x0"), pybind11::arg("y0")); + cl.def("getAsSimplePointsMap", (class mrpt::maps::CSimplePointsMap * (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::getAsSimplePointsMap, "C++: mrpt::maps::CMetricMap::getAsSimplePointsMap() --> class mrpt::maps::CSimplePointsMap *", pybind11::return_value_policy::automatic); + cl.def("assign", (class mrpt::maps::CMetricMap & (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap &)) &mrpt::maps::CMetricMap::operator=, "C++: mrpt::maps::CMetricMap::operator=(const class mrpt::maps::CMetricMap &) --> class mrpt::maps::CMetricMap &", pybind11::return_value_policy::automatic, pybind11::arg("")); + } + { // mrpt::maps::CVoxelMapBase file:mrpt/maps/CVoxelMapBase.h line:42 + pybind11::class_, std::shared_ptr>, mrpt::maps::CMetricMap> cl(M("mrpt::maps"), "CVoxelMapBase_mrpt_maps_VoxelNodeOccRGB_t", ""); + cl.def("assign", (class mrpt::maps::CVoxelMapBase & (mrpt::maps::CVoxelMapBase::*)(const class mrpt::maps::CVoxelMapBase &)) &mrpt::maps::CVoxelMapBase::operator=, "C++: mrpt::maps::CVoxelMapBase::operator=(const class mrpt::maps::CVoxelMapBase &) --> class mrpt::maps::CVoxelMapBase &", pybind11::return_value_policy::automatic, pybind11::arg("o")); + cl.def("asString", (std::string (mrpt::maps::CVoxelMapBase::*)() const) &mrpt::maps::CVoxelMapBase::asString, "C++: mrpt::maps::CVoxelMapBase::asString() const --> std::string"); + cl.def("getVisualizationInto", (void (mrpt::maps::CVoxelMapBase::*)(class mrpt::opengl::CSetOfObjects &) const) &mrpt::maps::CVoxelMapBase::getVisualizationInto, "C++: mrpt::maps::CVoxelMapBase::getVisualizationInto(class mrpt::opengl::CSetOfObjects &) const --> void", pybind11::arg("o")); + cl.def("getAsOctoMapVoxels", (void (mrpt::maps::CVoxelMapBase::*)(class mrpt::opengl::COctoMapVoxels &) const) &mrpt::maps::CVoxelMapBase::getAsOctoMapVoxels, "C++: mrpt::maps::CVoxelMapBase::getAsOctoMapVoxels(class mrpt::opengl::COctoMapVoxels &) const --> void", pybind11::arg("gl_obj")); + cl.def("saveMetricMapRepresentationToFile", (void (mrpt::maps::CVoxelMapBase::*)(const std::string &) const) &mrpt::maps::CVoxelMapBase::saveMetricMapRepresentationToFile, "C++: mrpt::maps::CVoxelMapBase::saveMetricMapRepresentationToFile(const std::string &) const --> void", pybind11::arg("filNamePrefix")); + cl.def_readwrite("genericMapParams", &mrpt::maps::CMetricMap::genericMapParams); + cl.def("GetRuntimeClass", (const struct mrpt::rtti::TRuntimeClassId * (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::GetRuntimeClass, "C++: mrpt::maps::CMetricMap::GetRuntimeClass() const --> const struct mrpt::rtti::TRuntimeClassId *", pybind11::return_value_policy::automatic); + cl.def_static("GetRuntimeClassIdStatic", (const struct mrpt::rtti::TRuntimeClassId & (*)()) &mrpt::maps::CMetricMap::GetRuntimeClassIdStatic, "C++: mrpt::maps::CMetricMap::GetRuntimeClassIdStatic() --> const struct mrpt::rtti::TRuntimeClassId &", pybind11::return_value_policy::automatic); + cl.def("clear", (void (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::clear, "Erase all the contents of the map \n\nC++: mrpt::maps::CMetricMap::clear() --> void"); + cl.def("isEmpty", (bool (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::isEmpty, "Returns true if the map is empty/no observation has been inserted.\n\nC++: mrpt::maps::CMetricMap::isEmpty() const --> bool"); + cl.def("loadFromProbabilisticPosesAndObservations", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations, "Load the map contents from a CSimpleMap object, erasing all previous\n content of the map. This is done invoking `insertObservation()` for each\n observation at the mean 3D robot pose of each pose-observations pair in\n the CSimpleMap object.\n\n \n insertObservation, CSimpleMap\n \n\n std::exception Some internal steps in invoked methods can\n raise exceptions on invalid parameters, etc...\n\nC++: mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); + cl.def("loadFromSimpleMap", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromSimpleMap, "! \n\nC++: mrpt::maps::CMetricMap::loadFromSimpleMap(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); + cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CObservation & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("obs")); + cl.def("insertObs", (bool (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D *)) &mrpt::maps::CMetricMap::insertObs, "C++: mrpt::maps::CMetricMap::insertObs(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D *) --> bool", pybind11::arg("obs"), pybind11::arg("robotPose")); + cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CSensoryFrame & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("sf")); + cl.def("insertObs", (bool (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CSensoryFrame &, const class mrpt::poses::CPose3D *)) &mrpt::maps::CMetricMap::insertObs, "C++: mrpt::maps::CMetricMap::insertObs(const class mrpt::obs::CSensoryFrame &, const class mrpt::poses::CPose3D *) --> bool", pybind11::arg("sf"), pybind11::arg("robotPose")); + cl.def("computeObservationLikelihood", (double (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D &) const) &mrpt::maps::CMetricMap::computeObservationLikelihood, "Computes the log-likelihood of a given observation given an arbitrary\n robot 3D pose.\n See: \n\n \n The robot's pose the observation is supposed to be taken\n from.\n \n\n The observation.\n \n\n This method returns a log-likelihood.\n\n \n Used in particle filter algorithms, see: CMultiMetricMapPDF::update\n\nC++: mrpt::maps::CMetricMap::computeObservationLikelihood(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D &) const --> double", pybind11::arg("obs"), pybind11::arg("takenFrom")); + cl.def("canComputeObservationLikelihood", (bool (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CObservation &) const) &mrpt::maps::CMetricMap::canComputeObservationLikelihood, "Returns true if this map is able to compute a sensible likelihood\n function for this observation (i.e. an occupancy grid map cannot with an\n image).\n See: \n\n \n The observation.\n \n\n computeObservationLikelihood,\n genericMapParams.enableObservationLikelihood\n\nC++: mrpt::maps::CMetricMap::canComputeObservationLikelihood(const class mrpt::obs::CObservation &) const --> bool", pybind11::arg("obs")); + cl.def("computeObservationsLikelihood", (double (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CSensoryFrame &, const class mrpt::poses::CPose3D &)) &mrpt::maps::CMetricMap::computeObservationsLikelihood, "Returns the sum of the log-likelihoods of each individual observation\n within a mrpt::obs::CSensoryFrame.\n See: \n\n \n The robot's pose the observation is supposed to be taken\n from.\n \n\n The set of observations in a CSensoryFrame.\n \n\n This method returns a log-likelihood.\n \n\n canComputeObservationsLikelihood\n\nC++: mrpt::maps::CMetricMap::computeObservationsLikelihood(const class mrpt::obs::CSensoryFrame &, const class mrpt::poses::CPose3D &) --> double", pybind11::arg("sf"), pybind11::arg("takenFrom")); + cl.def("canComputeObservationsLikelihood", (bool (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CSensoryFrame &) const) &mrpt::maps::CMetricMap::canComputeObservationsLikelihood, "Returns true if this map is able to compute a sensible likelihood\n function for this observation (i.e. an occupancy grid map cannot with an\n image).\n See: \n\n \n The observations.\n \n\n canComputeObservationLikelihood\n\nC++: mrpt::maps::CMetricMap::canComputeObservationsLikelihood(const class mrpt::obs::CSensoryFrame &) const --> bool", pybind11::arg("sf")); + cl.def("determineMatching2D", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose2D &, class mrpt::tfest::TMatchingPairListTempl &, const struct mrpt::maps::TMatchingParams &, struct mrpt::maps::TMatchingExtraResults &) const) &mrpt::maps::CMetricMap::determineMatching2D, "Computes the matching between this and another 2D point map, which\nincludes finding:\n - The set of points pairs in each map\n - The mean squared distance between corresponding pairs.\n\n The algorithm is:\n - For each point in \"otherMap\":\n - Transform the point according to otherMapPose\n - Search with a KD-TREE the closest correspondences in \"this\"\nmap.\n - Add to the set of candidate matchings, if it passes all the\nthresholds in params.\n\n This method is the most time critical one into ICP-like algorithms.\n\n \n [IN] The other map to compute the matching with.\n \n\n [IN] The pose of the other map as seen from\n\"this\".\n \n\n [IN] Parameters for the determination of\npairings.\n \n\n [OUT] The detected matchings pairs.\n \n\n [OUT] Other results.\n \n\n compute3DMatchingRatio\n\nC++: mrpt::maps::CMetricMap::determineMatching2D(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose2D &, class mrpt::tfest::TMatchingPairListTempl &, const struct mrpt::maps::TMatchingParams &, struct mrpt::maps::TMatchingExtraResults &) const --> void", pybind11::arg("otherMap"), pybind11::arg("otherMapPose"), pybind11::arg("correspondences"), pybind11::arg("params"), pybind11::arg("extraResults")); + cl.def("determineMatching3D", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, class mrpt::tfest::TMatchingPairListTempl &, const struct mrpt::maps::TMatchingParams &, struct mrpt::maps::TMatchingExtraResults &) const) &mrpt::maps::CMetricMap::determineMatching3D, "Computes the matchings between this and another 3D points map - method\nused in 3D-ICP.\n This method finds the set of point pairs in each map.\n\n The method is the most time critical one into ICP-like algorithms.\n\n The algorithm is:\n - For each point in \"otherMap\":\n - Transform the point according to otherMapPose\n - Search with a KD-TREE the closest correspondences in \"this\"\nmap.\n - Add to the set of candidate matchings, if it passes all the\nthresholds in params.\n\n \n [IN] The other map to compute the matching with.\n \n\n [IN] The pose of the other map as seen from\n\"this\".\n \n\n [IN] Parameters for the determination of\npairings.\n \n\n [OUT] The detected matchings pairs.\n \n\n [OUT] Other results.\n \n\n compute3DMatchingRatio\n\nC++: mrpt::maps::CMetricMap::determineMatching3D(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, class mrpt::tfest::TMatchingPairListTempl &, const struct mrpt::maps::TMatchingParams &, struct mrpt::maps::TMatchingExtraResults &) const --> void", pybind11::arg("otherMap"), pybind11::arg("otherMapPose"), pybind11::arg("correspondences"), pybind11::arg("params"), pybind11::arg("extraResults")); + cl.def("compute3DMatchingRatio", (float (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, const struct mrpt::maps::TMatchingRatioParams &) const) &mrpt::maps::CMetricMap::compute3DMatchingRatio, "Computes the ratio in [0,1] of correspondences between \"this\" and the\n \"otherMap\" map, whose 6D pose relative to \"this\" is \"otherMapPose\"\n In the case of a multi-metric map, this returns the average between the\n maps. This method always return 0 for grid maps.\n \n\n [IN] The other map to compute the matching with.\n \n\n [IN] The 6D pose of the other map as seen from\n \"this\".\n \n\n [IN] Matching parameters\n \n\n The matching ratio [0,1]\n \n\n determineMatching2D\n\nC++: mrpt::maps::CMetricMap::compute3DMatchingRatio(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, const struct mrpt::maps::TMatchingRatioParams &) const --> float", pybind11::arg("otherMap"), pybind11::arg("otherMapPose"), pybind11::arg("params")); + cl.def("saveMetricMapRepresentationToFile", (void (mrpt::maps::CMetricMap::*)(const std::string &) const) &mrpt::maps::CMetricMap::saveMetricMapRepresentationToFile, "This virtual method saves the map to a file \"filNamePrefix\"+<\n some_file_extension >, as an image or in any other applicable way (Notice\n that other methods to save the map may be implemented in classes\n implementing this virtual interface). \n\nC++: mrpt::maps::CMetricMap::saveMetricMapRepresentationToFile(const std::string &) const --> void", pybind11::arg("filNamePrefix")); + cl.def("auxParticleFilterCleanUp", (void (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::auxParticleFilterCleanUp, "This method is called at the end of each \"prediction-update-map\n insertion\" cycle within\n \"mrpt::slam::CMetricMapBuilderRBPF::processActionObservation\".\n This method should normally do nothing, but in some cases can be used\n to free auxiliary cached variables.\n\nC++: mrpt::maps::CMetricMap::auxParticleFilterCleanUp() --> void"); + cl.def("squareDistanceToClosestCorrespondence", (float (mrpt::maps::CMetricMap::*)(float, float) const) &mrpt::maps::CMetricMap::squareDistanceToClosestCorrespondence, "Returns the square distance from the 2D point (x0,y0) to the closest\n correspondence in the map. \n\nC++: mrpt::maps::CMetricMap::squareDistanceToClosestCorrespondence(float, float) const --> float", pybind11::arg("x0"), pybind11::arg("y0")); + cl.def("getAsSimplePointsMap", (class mrpt::maps::CSimplePointsMap * (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::getAsSimplePointsMap, "C++: mrpt::maps::CMetricMap::getAsSimplePointsMap() --> class mrpt::maps::CSimplePointsMap *", pybind11::return_value_policy::automatic); + cl.def("assign", (class mrpt::maps::CMetricMap & (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap &)) &mrpt::maps::CMetricMap::operator=, "C++: mrpt::maps::CMetricMap::operator=(const class mrpt::maps::CMetricMap &) --> class mrpt::maps::CMetricMap &", pybind11::return_value_policy::automatic, pybind11::arg("")); + } +} diff --git a/python/src/mrpt/maps/CVoxelMapOccupancyBase.cpp b/python/src/mrpt/maps/CVoxelMapOccupancyBase.cpp new file mode 100644 index 0000000000..90de005fdd --- /dev/null +++ b/python/src/mrpt/maps/CVoxelMapOccupancyBase.cpp @@ -0,0 +1,139 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include // __str__ +#include +#include +#include + +#include +#include +#include +#include + + +#ifndef BINDER_PYBIND11_TYPE_CASTER + #define BINDER_PYBIND11_TYPE_CASTER + PYBIND11_DECLARE_HOLDER_TYPE(T, std::shared_ptr) + PYBIND11_DECLARE_HOLDER_TYPE(T, T*) + PYBIND11_MAKE_OPAQUE(std::shared_ptr) +#endif + +// mrpt::maps::TVoxelMap_InsertionOptions file:mrpt/maps/CVoxelMapOccupancyBase.h line:22 +struct PyCallBack_mrpt_maps_TVoxelMap_InsertionOptions : public mrpt::maps::TVoxelMap_InsertionOptions { + using mrpt::maps::TVoxelMap_InsertionOptions::TVoxelMap_InsertionOptions; + + void loadFromConfigFile(const class mrpt::config::CConfigFileBase & a0, const std::string & a1) override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "loadFromConfigFile"); + if (overload) { + auto o = overload.operator()(a0, a1); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return TVoxelMap_InsertionOptions::loadFromConfigFile(a0, a1); + } + void saveToConfigFile(class mrpt::config::CConfigFileBase & a0, const std::string & a1) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "saveToConfigFile"); + if (overload) { + auto o = overload.operator()(a0, a1); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return TVoxelMap_InsertionOptions::saveToConfigFile(a0, a1); + } +}; + +// mrpt::maps::TVoxelMap_LikelihoodOptions file:mrpt/maps/CVoxelMapOccupancyBase.h line:50 +struct PyCallBack_mrpt_maps_TVoxelMap_LikelihoodOptions : public mrpt::maps::TVoxelMap_LikelihoodOptions { + using mrpt::maps::TVoxelMap_LikelihoodOptions::TVoxelMap_LikelihoodOptions; + + void loadFromConfigFile(const class mrpt::config::CConfigFileBase & a0, const std::string & a1) override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "loadFromConfigFile"); + if (overload) { + auto o = overload.operator()(a0, a1); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return TVoxelMap_LikelihoodOptions::loadFromConfigFile(a0, a1); + } + void saveToConfigFile(class mrpt::config::CConfigFileBase & a0, const std::string & a1) const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "saveToConfigFile"); + if (overload) { + auto o = overload.operator()(a0, a1); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return TVoxelMap_LikelihoodOptions::saveToConfigFile(a0, a1); + } +}; + +void bind_mrpt_maps_CVoxelMapOccupancyBase(std::function< pybind11::module &(std::string const &namespace_) > &M) +{ + { // mrpt::maps::TVoxelMap_InsertionOptions file:mrpt/maps/CVoxelMapOccupancyBase.h line:22 + pybind11::class_, PyCallBack_mrpt_maps_TVoxelMap_InsertionOptions, mrpt::config::CLoadableOptions> cl(M("mrpt::maps"), "TVoxelMap_InsertionOptions", ""); + cl.def( pybind11::init( [](){ return new mrpt::maps::TVoxelMap_InsertionOptions(); }, [](){ return new PyCallBack_mrpt_maps_TVoxelMap_InsertionOptions(); } ) ); + cl.def( pybind11::init( [](PyCallBack_mrpt_maps_TVoxelMap_InsertionOptions const &o){ return new PyCallBack_mrpt_maps_TVoxelMap_InsertionOptions(o); } ) ); + cl.def( pybind11::init( [](mrpt::maps::TVoxelMap_InsertionOptions const &o){ return new mrpt::maps::TVoxelMap_InsertionOptions(o); } ) ); + cl.def_readwrite("max_range", &mrpt::maps::TVoxelMap_InsertionOptions::max_range); + cl.def_readwrite("prob_miss", &mrpt::maps::TVoxelMap_InsertionOptions::prob_miss); + cl.def_readwrite("prob_hit", &mrpt::maps::TVoxelMap_InsertionOptions::prob_hit); + cl.def_readwrite("clamp_min", &mrpt::maps::TVoxelMap_InsertionOptions::clamp_min); + cl.def_readwrite("clamp_max", &mrpt::maps::TVoxelMap_InsertionOptions::clamp_max); + cl.def_readwrite("ray_trace_free_space", &mrpt::maps::TVoxelMap_InsertionOptions::ray_trace_free_space); + cl.def_readwrite("decimation", &mrpt::maps::TVoxelMap_InsertionOptions::decimation); + cl.def("loadFromConfigFile", (void (mrpt::maps::TVoxelMap_InsertionOptions::*)(const class mrpt::config::CConfigFileBase &, const std::string &)) &mrpt::maps::TVoxelMap_InsertionOptions::loadFromConfigFile, "C++: mrpt::maps::TVoxelMap_InsertionOptions::loadFromConfigFile(const class mrpt::config::CConfigFileBase &, const std::string &) --> void", pybind11::arg("source"), pybind11::arg("section")); + cl.def("saveToConfigFile", (void (mrpt::maps::TVoxelMap_InsertionOptions::*)(class mrpt::config::CConfigFileBase &, const std::string &) const) &mrpt::maps::TVoxelMap_InsertionOptions::saveToConfigFile, "C++: mrpt::maps::TVoxelMap_InsertionOptions::saveToConfigFile(class mrpt::config::CConfigFileBase &, const std::string &) const --> void", pybind11::arg("c"), pybind11::arg("s")); + cl.def("writeToStream", (void (mrpt::maps::TVoxelMap_InsertionOptions::*)(class mrpt::serialization::CArchive &) const) &mrpt::maps::TVoxelMap_InsertionOptions::writeToStream, "C++: mrpt::maps::TVoxelMap_InsertionOptions::writeToStream(class mrpt::serialization::CArchive &) const --> void", pybind11::arg("out")); + cl.def("readFromStream", (void (mrpt::maps::TVoxelMap_InsertionOptions::*)(class mrpt::serialization::CArchive &)) &mrpt::maps::TVoxelMap_InsertionOptions::readFromStream, "C++: mrpt::maps::TVoxelMap_InsertionOptions::readFromStream(class mrpt::serialization::CArchive &) --> void", pybind11::arg("in")); + cl.def("assign", (struct mrpt::maps::TVoxelMap_InsertionOptions & (mrpt::maps::TVoxelMap_InsertionOptions::*)(const struct mrpt::maps::TVoxelMap_InsertionOptions &)) &mrpt::maps::TVoxelMap_InsertionOptions::operator=, "C++: mrpt::maps::TVoxelMap_InsertionOptions::operator=(const struct mrpt::maps::TVoxelMap_InsertionOptions &) --> struct mrpt::maps::TVoxelMap_InsertionOptions &", pybind11::return_value_policy::automatic, pybind11::arg("")); + } + { // mrpt::maps::TVoxelMap_LikelihoodOptions file:mrpt/maps/CVoxelMapOccupancyBase.h line:50 + pybind11::class_, PyCallBack_mrpt_maps_TVoxelMap_LikelihoodOptions, mrpt::config::CLoadableOptions> cl(M("mrpt::maps"), "TVoxelMap_LikelihoodOptions", "Options used when evaluating \"computeObservationLikelihood\"\n \n\n CObservation::computeObservationLikelihood"); + cl.def( pybind11::init( [](){ return new mrpt::maps::TVoxelMap_LikelihoodOptions(); }, [](){ return new PyCallBack_mrpt_maps_TVoxelMap_LikelihoodOptions(); } ) ); + cl.def( pybind11::init( [](PyCallBack_mrpt_maps_TVoxelMap_LikelihoodOptions const &o){ return new PyCallBack_mrpt_maps_TVoxelMap_LikelihoodOptions(o); } ) ); + cl.def( pybind11::init( [](mrpt::maps::TVoxelMap_LikelihoodOptions const &o){ return new mrpt::maps::TVoxelMap_LikelihoodOptions(o); } ) ); + cl.def_readwrite("decimation", &mrpt::maps::TVoxelMap_LikelihoodOptions::decimation); + cl.def_readwrite("occupiedThreshold", &mrpt::maps::TVoxelMap_LikelihoodOptions::occupiedThreshold); + cl.def("loadFromConfigFile", (void (mrpt::maps::TVoxelMap_LikelihoodOptions::*)(const class mrpt::config::CConfigFileBase &, const std::string &)) &mrpt::maps::TVoxelMap_LikelihoodOptions::loadFromConfigFile, "C++: mrpt::maps::TVoxelMap_LikelihoodOptions::loadFromConfigFile(const class mrpt::config::CConfigFileBase &, const std::string &) --> void", pybind11::arg("source"), pybind11::arg("section")); + cl.def("saveToConfigFile", (void (mrpt::maps::TVoxelMap_LikelihoodOptions::*)(class mrpt::config::CConfigFileBase &, const std::string &) const) &mrpt::maps::TVoxelMap_LikelihoodOptions::saveToConfigFile, "C++: mrpt::maps::TVoxelMap_LikelihoodOptions::saveToConfigFile(class mrpt::config::CConfigFileBase &, const std::string &) const --> void", pybind11::arg("c"), pybind11::arg("s")); + cl.def("writeToStream", (void (mrpt::maps::TVoxelMap_LikelihoodOptions::*)(class mrpt::serialization::CArchive &) const) &mrpt::maps::TVoxelMap_LikelihoodOptions::writeToStream, "C++: mrpt::maps::TVoxelMap_LikelihoodOptions::writeToStream(class mrpt::serialization::CArchive &) const --> void", pybind11::arg("out")); + cl.def("readFromStream", (void (mrpt::maps::TVoxelMap_LikelihoodOptions::*)(class mrpt::serialization::CArchive &)) &mrpt::maps::TVoxelMap_LikelihoodOptions::readFromStream, "C++: mrpt::maps::TVoxelMap_LikelihoodOptions::readFromStream(class mrpt::serialization::CArchive &) --> void", pybind11::arg("in")); + cl.def("assign", (struct mrpt::maps::TVoxelMap_LikelihoodOptions & (mrpt::maps::TVoxelMap_LikelihoodOptions::*)(const struct mrpt::maps::TVoxelMap_LikelihoodOptions &)) &mrpt::maps::TVoxelMap_LikelihoodOptions::operator=, "C++: mrpt::maps::TVoxelMap_LikelihoodOptions::operator=(const struct mrpt::maps::TVoxelMap_LikelihoodOptions &) --> struct mrpt::maps::TVoxelMap_LikelihoodOptions &", pybind11::return_value_policy::automatic, pybind11::arg("")); + } + { // mrpt::maps::TVoxelMap_RenderingOptions file:mrpt/maps/CVoxelMapOccupancyBase.h line:75 + pybind11::class_> cl(M("mrpt::maps"), "TVoxelMap_RenderingOptions", "Options for the conversion of a mrpt::maps::COctoMap into a\n mrpt::opengl::COctoMapVoxels "); + cl.def( pybind11::init( [](){ return new mrpt::maps::TVoxelMap_RenderingOptions(); } ) ); + cl.def( pybind11::init( [](mrpt::maps::TVoxelMap_RenderingOptions const &o){ return new mrpt::maps::TVoxelMap_RenderingOptions(o); } ) ); + cl.def_readwrite("generateOccupiedVoxels", &mrpt::maps::TVoxelMap_RenderingOptions::generateOccupiedVoxels); + cl.def_readwrite("occupiedThreshold", &mrpt::maps::TVoxelMap_RenderingOptions::occupiedThreshold); + cl.def_readwrite("visibleOccupiedVoxels", &mrpt::maps::TVoxelMap_RenderingOptions::visibleOccupiedVoxels); + cl.def_readwrite("generateFreeVoxels", &mrpt::maps::TVoxelMap_RenderingOptions::generateFreeVoxels); + cl.def_readwrite("freeThreshold", &mrpt::maps::TVoxelMap_RenderingOptions::freeThreshold); + cl.def_readwrite("visibleFreeVoxels", &mrpt::maps::TVoxelMap_RenderingOptions::visibleFreeVoxels); + cl.def("writeToStream", (void (mrpt::maps::TVoxelMap_RenderingOptions::*)(class mrpt::serialization::CArchive &) const) &mrpt::maps::TVoxelMap_RenderingOptions::writeToStream, "Binary dump to stream \n\nC++: mrpt::maps::TVoxelMap_RenderingOptions::writeToStream(class mrpt::serialization::CArchive &) const --> void", pybind11::arg("out")); + cl.def("readFromStream", (void (mrpt::maps::TVoxelMap_RenderingOptions::*)(class mrpt::serialization::CArchive &)) &mrpt::maps::TVoxelMap_RenderingOptions::readFromStream, "Binary dump to stream \n\nC++: mrpt::maps::TVoxelMap_RenderingOptions::readFromStream(class mrpt::serialization::CArchive &) --> void", pybind11::arg("in")); + cl.def("assign", (struct mrpt::maps::TVoxelMap_RenderingOptions & (mrpt::maps::TVoxelMap_RenderingOptions::*)(const struct mrpt::maps::TVoxelMap_RenderingOptions &)) &mrpt::maps::TVoxelMap_RenderingOptions::operator=, "C++: mrpt::maps::TVoxelMap_RenderingOptions::operator=(const struct mrpt::maps::TVoxelMap_RenderingOptions &) --> struct mrpt::maps::TVoxelMap_RenderingOptions &", pybind11::return_value_policy::automatic, pybind11::arg("")); + } +} diff --git a/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp b/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp new file mode 100644 index 0000000000..6798ce7e7f --- /dev/null +++ b/python/src/mrpt/maps/CVoxelMapOccupancyBase_1.cpp @@ -0,0 +1,100 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include // __str__ +#include +#include + +#include +#include +#include +#include + + +#ifndef BINDER_PYBIND11_TYPE_CASTER + #define BINDER_PYBIND11_TYPE_CASTER + PYBIND11_DECLARE_HOLDER_TYPE(T, std::shared_ptr) + PYBIND11_DECLARE_HOLDER_TYPE(T, T*) + PYBIND11_MAKE_OPAQUE(std::shared_ptr) +#endif + +void bind_mrpt_maps_CVoxelMapOccupancyBase_1(std::function< pybind11::module &(std::string const &namespace_) > &M) +{ + { // mrpt::maps::CVoxelMapOccupancyBase file:mrpt/maps/CVoxelMapOccupancyBase.h line:114 + pybind11::class_, std::shared_ptr>, mrpt::maps::CVoxelMapBase> cl(M("mrpt::maps"), "CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccupancy_signed_char_t", ""); + cl.def_readwrite("insertionOptions", &mrpt::maps::CVoxelMapOccupancyBase::insertionOptions); + cl.def_readwrite("likelihoodOptions", &mrpt::maps::CVoxelMapOccupancyBase::likelihoodOptions); + cl.def_readwrite("renderingOptions", &mrpt::maps::CVoxelMapOccupancyBase::renderingOptions); + cl.def("isEmpty", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::isEmpty, "C++: mrpt::maps::CVoxelMapOccupancyBase::isEmpty() const --> bool"); + cl.def("getAsOctoMapVoxels", (void (mrpt::maps::CVoxelMapOccupancyBase::*)(class mrpt::opengl::COctoMapVoxels &) const) &mrpt::maps::CVoxelMapOccupancyBase::getAsOctoMapVoxels, "C++: mrpt::maps::CVoxelMapOccupancyBase::getAsOctoMapVoxels(class mrpt::opengl::COctoMapVoxels &) const --> void", pybind11::arg("gl_obj")); + cl.def("updateVoxel", (void (mrpt::maps::CVoxelMapOccupancyBase::*)(const double, const double, const double, bool)) &mrpt::maps::CVoxelMapOccupancyBase::updateVoxel, "C++: mrpt::maps::CVoxelMapOccupancyBase::updateVoxel(const double, const double, const double, bool) --> void", pybind11::arg("x"), pybind11::arg("y"), pybind11::arg("z"), pybind11::arg("occupied")); + cl.def("getPointOccupancy", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const double, const double, const double, double &) const) &mrpt::maps::CVoxelMapOccupancyBase::getPointOccupancy, "C++: mrpt::maps::CVoxelMapOccupancyBase::getPointOccupancy(const double, const double, const double, double &) const --> bool", pybind11::arg("x"), pybind11::arg("y"), pybind11::arg("z"), pybind11::arg("prob_occupancy")); + cl.def("insertPointCloudAsRays", (void (mrpt::maps::CVoxelMapOccupancyBase::*)(const class mrpt::maps::CPointsMap &, const struct mrpt::math::TPoint3D_ &)) &mrpt::maps::CVoxelMapOccupancyBase::insertPointCloudAsRays, "C++: mrpt::maps::CVoxelMapOccupancyBase::insertPointCloudAsRays(const class mrpt::maps::CPointsMap &, const struct mrpt::math::TPoint3D_ &) --> void", pybind11::arg("pts"), pybind11::arg("sensorPt")); + cl.def("insertPointCloudAsEndPoints", (void (mrpt::maps::CVoxelMapOccupancyBase::*)(const class mrpt::maps::CPointsMap &, const struct mrpt::math::TPoint3D_ &)) &mrpt::maps::CVoxelMapOccupancyBase::insertPointCloudAsEndPoints, "C++: mrpt::maps::CVoxelMapOccupancyBase::insertPointCloudAsEndPoints(const class mrpt::maps::CPointsMap &, const struct mrpt::math::TPoint3D_ &) --> void", pybind11::arg("pts"), pybind11::arg("sensorPt")); + cl.def("getOccupiedVoxels", (class std::shared_ptr (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::getOccupiedVoxels, "C++: mrpt::maps::CVoxelMapOccupancyBase::getOccupiedVoxels() const --> class std::shared_ptr"); + cl.def("updateCell_fast_occupied", (void (mrpt::maps::CVoxelMapOccupancyBase::*)(struct mrpt::maps::VoxelNodeOccupancy *, const signed char, const signed char)) &mrpt::maps::CVoxelMapOccupancyBase::updateCell_fast_occupied, "C++: mrpt::maps::CVoxelMapOccupancyBase::updateCell_fast_occupied(struct mrpt::maps::VoxelNodeOccupancy *, const signed char, const signed char) --> void", pybind11::arg("theCell"), pybind11::arg("logodd_obs"), pybind11::arg("thres")); + cl.def("updateCell_fast_free", (void (mrpt::maps::CVoxelMapOccupancyBase::*)(struct mrpt::maps::VoxelNodeOccupancy *, const signed char, const signed char)) &mrpt::maps::CVoxelMapOccupancyBase::updateCell_fast_free, "C++: mrpt::maps::CVoxelMapOccupancyBase::updateCell_fast_free(struct mrpt::maps::VoxelNodeOccupancy *, const signed char, const signed char) --> void", pybind11::arg("theCell"), pybind11::arg("logodd_obs"), pybind11::arg("thres")); + cl.def_static("get_logodd_lut", (struct mrpt::maps::CLogOddsGridMapLUT & (*)()) &mrpt::maps::CVoxelMapOccupancyBase::get_logodd_lut, "C++: mrpt::maps::CVoxelMapOccupancyBase::get_logodd_lut() --> struct mrpt::maps::CLogOddsGridMapLUT &", pybind11::return_value_policy::automatic); + cl.def_static("l2p", (float (*)(const signed char)) &mrpt::maps::CVoxelMapOccupancyBase::l2p, "C++: mrpt::maps::CVoxelMapOccupancyBase::l2p(const signed char) --> float", pybind11::arg("l")); + cl.def_static("l2p_255", (uint8_t (*)(const signed char)) &mrpt::maps::CVoxelMapOccupancyBase::l2p_255, "C++: mrpt::maps::CVoxelMapOccupancyBase::l2p_255(const signed char) --> uint8_t", pybind11::arg("l")); + cl.def_static("p2l", (signed char (*)(const float)) &mrpt::maps::CVoxelMapOccupancyBase::p2l, "C++: mrpt::maps::CVoxelMapOccupancyBase::p2l(const float) --> signed char", pybind11::arg("p")); + cl.def("assign", (class mrpt::maps::CVoxelMapOccupancyBase & (mrpt::maps::CVoxelMapOccupancyBase::*)(const class mrpt::maps::CVoxelMapOccupancyBase &)) &mrpt::maps::CVoxelMapOccupancyBase::operator=, "C++: mrpt::maps::CVoxelMapOccupancyBase::operator=(const class mrpt::maps::CVoxelMapOccupancyBase &) --> class mrpt::maps::CVoxelMapOccupancyBase &", pybind11::return_value_policy::automatic, pybind11::arg("")); + cl.def("assign", (class mrpt::maps::CVoxelMapBase & (mrpt::maps::CVoxelMapBase::*)(const class mrpt::maps::CVoxelMapBase &)) &mrpt::maps::CVoxelMapBase::operator=, "C++: mrpt::maps::CVoxelMapBase::operator=(const class mrpt::maps::CVoxelMapBase &) --> class mrpt::maps::CVoxelMapBase &", pybind11::return_value_policy::automatic, pybind11::arg("o")); + cl.def("asString", (std::string (mrpt::maps::CVoxelMapBase::*)() const) &mrpt::maps::CVoxelMapBase::asString, "C++: mrpt::maps::CVoxelMapBase::asString() const --> std::string"); + cl.def("getVisualizationInto", (void (mrpt::maps::CVoxelMapBase::*)(class mrpt::opengl::CSetOfObjects &) const) &mrpt::maps::CVoxelMapBase::getVisualizationInto, "C++: mrpt::maps::CVoxelMapBase::getVisualizationInto(class mrpt::opengl::CSetOfObjects &) const --> void", pybind11::arg("o")); + cl.def("getAsOctoMapVoxels", (void (mrpt::maps::CVoxelMapBase::*)(class mrpt::opengl::COctoMapVoxels &) const) &mrpt::maps::CVoxelMapBase::getAsOctoMapVoxels, "C++: mrpt::maps::CVoxelMapBase::getAsOctoMapVoxels(class mrpt::opengl::COctoMapVoxels &) const --> void", pybind11::arg("gl_obj")); + cl.def("saveMetricMapRepresentationToFile", (void (mrpt::maps::CVoxelMapBase::*)(const std::string &) const) &mrpt::maps::CVoxelMapBase::saveMetricMapRepresentationToFile, "C++: mrpt::maps::CVoxelMapBase::saveMetricMapRepresentationToFile(const std::string &) const --> void", pybind11::arg("filNamePrefix")); + cl.def_readwrite("genericMapParams", &mrpt::maps::CMetricMap::genericMapParams); + cl.def("GetRuntimeClass", (const struct mrpt::rtti::TRuntimeClassId * (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::GetRuntimeClass, "C++: mrpt::maps::CMetricMap::GetRuntimeClass() const --> const struct mrpt::rtti::TRuntimeClassId *", pybind11::return_value_policy::automatic); + cl.def_static("GetRuntimeClassIdStatic", (const struct mrpt::rtti::TRuntimeClassId & (*)()) &mrpt::maps::CMetricMap::GetRuntimeClassIdStatic, "C++: mrpt::maps::CMetricMap::GetRuntimeClassIdStatic() --> const struct mrpt::rtti::TRuntimeClassId &", pybind11::return_value_policy::automatic); + cl.def("clear", (void (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::clear, "Erase all the contents of the map \n\nC++: mrpt::maps::CMetricMap::clear() --> void"); + cl.def("isEmpty", (bool (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::isEmpty, "Returns true if the map is empty/no observation has been inserted.\n\nC++: mrpt::maps::CMetricMap::isEmpty() const --> bool"); + cl.def("loadFromProbabilisticPosesAndObservations", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations, "Load the map contents from a CSimpleMap object, erasing all previous\n content of the map. This is done invoking `insertObservation()` for each\n observation at the mean 3D robot pose of each pose-observations pair in\n the CSimpleMap object.\n\n \n insertObservation, CSimpleMap\n \n\n std::exception Some internal steps in invoked methods can\n raise exceptions on invalid parameters, etc...\n\nC++: mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); + cl.def("loadFromSimpleMap", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromSimpleMap, "! \n\nC++: mrpt::maps::CMetricMap::loadFromSimpleMap(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); + cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CObservation & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("obs")); + cl.def("insertObs", (bool (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D *)) &mrpt::maps::CMetricMap::insertObs, "C++: mrpt::maps::CMetricMap::insertObs(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D *) --> bool", pybind11::arg("obs"), pybind11::arg("robotPose")); + cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CSensoryFrame & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("sf")); + cl.def("insertObs", (bool (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CSensoryFrame &, const class mrpt::poses::CPose3D *)) &mrpt::maps::CMetricMap::insertObs, "C++: mrpt::maps::CMetricMap::insertObs(const class mrpt::obs::CSensoryFrame &, const class mrpt::poses::CPose3D *) --> bool", pybind11::arg("sf"), pybind11::arg("robotPose")); + cl.def("computeObservationLikelihood", (double (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D &) const) &mrpt::maps::CMetricMap::computeObservationLikelihood, "Computes the log-likelihood of a given observation given an arbitrary\n robot 3D pose.\n See: \n\n \n The robot's pose the observation is supposed to be taken\n from.\n \n\n The observation.\n \n\n This method returns a log-likelihood.\n\n \n Used in particle filter algorithms, see: CMultiMetricMapPDF::update\n\nC++: mrpt::maps::CMetricMap::computeObservationLikelihood(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D &) const --> double", pybind11::arg("obs"), pybind11::arg("takenFrom")); + cl.def("canComputeObservationLikelihood", (bool (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CObservation &) const) &mrpt::maps::CMetricMap::canComputeObservationLikelihood, "Returns true if this map is able to compute a sensible likelihood\n function for this observation (i.e. an occupancy grid map cannot with an\n image).\n See: \n\n \n The observation.\n \n\n computeObservationLikelihood,\n genericMapParams.enableObservationLikelihood\n\nC++: mrpt::maps::CMetricMap::canComputeObservationLikelihood(const class mrpt::obs::CObservation &) const --> bool", pybind11::arg("obs")); + cl.def("computeObservationsLikelihood", (double (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CSensoryFrame &, const class mrpt::poses::CPose3D &)) &mrpt::maps::CMetricMap::computeObservationsLikelihood, "Returns the sum of the log-likelihoods of each individual observation\n within a mrpt::obs::CSensoryFrame.\n See: \n\n \n The robot's pose the observation is supposed to be taken\n from.\n \n\n The set of observations in a CSensoryFrame.\n \n\n This method returns a log-likelihood.\n \n\n canComputeObservationsLikelihood\n\nC++: mrpt::maps::CMetricMap::computeObservationsLikelihood(const class mrpt::obs::CSensoryFrame &, const class mrpt::poses::CPose3D &) --> double", pybind11::arg("sf"), pybind11::arg("takenFrom")); + cl.def("canComputeObservationsLikelihood", (bool (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CSensoryFrame &) const) &mrpt::maps::CMetricMap::canComputeObservationsLikelihood, "Returns true if this map is able to compute a sensible likelihood\n function for this observation (i.e. an occupancy grid map cannot with an\n image).\n See: \n\n \n The observations.\n \n\n canComputeObservationLikelihood\n\nC++: mrpt::maps::CMetricMap::canComputeObservationsLikelihood(const class mrpt::obs::CSensoryFrame &) const --> bool", pybind11::arg("sf")); + cl.def("determineMatching2D", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose2D &, class mrpt::tfest::TMatchingPairListTempl &, const struct mrpt::maps::TMatchingParams &, struct mrpt::maps::TMatchingExtraResults &) const) &mrpt::maps::CMetricMap::determineMatching2D, "Computes the matching between this and another 2D point map, which\nincludes finding:\n - The set of points pairs in each map\n - The mean squared distance between corresponding pairs.\n\n The algorithm is:\n - For each point in \"otherMap\":\n - Transform the point according to otherMapPose\n - Search with a KD-TREE the closest correspondences in \"this\"\nmap.\n - Add to the set of candidate matchings, if it passes all the\nthresholds in params.\n\n This method is the most time critical one into ICP-like algorithms.\n\n \n [IN] The other map to compute the matching with.\n \n\n [IN] The pose of the other map as seen from\n\"this\".\n \n\n [IN] Parameters for the determination of\npairings.\n \n\n [OUT] The detected matchings pairs.\n \n\n [OUT] Other results.\n \n\n compute3DMatchingRatio\n\nC++: mrpt::maps::CMetricMap::determineMatching2D(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose2D &, class mrpt::tfest::TMatchingPairListTempl &, const struct mrpt::maps::TMatchingParams &, struct mrpt::maps::TMatchingExtraResults &) const --> void", pybind11::arg("otherMap"), pybind11::arg("otherMapPose"), pybind11::arg("correspondences"), pybind11::arg("params"), pybind11::arg("extraResults")); + cl.def("determineMatching3D", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, class mrpt::tfest::TMatchingPairListTempl &, const struct mrpt::maps::TMatchingParams &, struct mrpt::maps::TMatchingExtraResults &) const) &mrpt::maps::CMetricMap::determineMatching3D, "Computes the matchings between this and another 3D points map - method\nused in 3D-ICP.\n This method finds the set of point pairs in each map.\n\n The method is the most time critical one into ICP-like algorithms.\n\n The algorithm is:\n - For each point in \"otherMap\":\n - Transform the point according to otherMapPose\n - Search with a KD-TREE the closest correspondences in \"this\"\nmap.\n - Add to the set of candidate matchings, if it passes all the\nthresholds in params.\n\n \n [IN] The other map to compute the matching with.\n \n\n [IN] The pose of the other map as seen from\n\"this\".\n \n\n [IN] Parameters for the determination of\npairings.\n \n\n [OUT] The detected matchings pairs.\n \n\n [OUT] Other results.\n \n\n compute3DMatchingRatio\n\nC++: mrpt::maps::CMetricMap::determineMatching3D(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, class mrpt::tfest::TMatchingPairListTempl &, const struct mrpt::maps::TMatchingParams &, struct mrpt::maps::TMatchingExtraResults &) const --> void", pybind11::arg("otherMap"), pybind11::arg("otherMapPose"), pybind11::arg("correspondences"), pybind11::arg("params"), pybind11::arg("extraResults")); + cl.def("compute3DMatchingRatio", (float (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, const struct mrpt::maps::TMatchingRatioParams &) const) &mrpt::maps::CMetricMap::compute3DMatchingRatio, "Computes the ratio in [0,1] of correspondences between \"this\" and the\n \"otherMap\" map, whose 6D pose relative to \"this\" is \"otherMapPose\"\n In the case of a multi-metric map, this returns the average between the\n maps. This method always return 0 for grid maps.\n \n\n [IN] The other map to compute the matching with.\n \n\n [IN] The 6D pose of the other map as seen from\n \"this\".\n \n\n [IN] Matching parameters\n \n\n The matching ratio [0,1]\n \n\n determineMatching2D\n\nC++: mrpt::maps::CMetricMap::compute3DMatchingRatio(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, const struct mrpt::maps::TMatchingRatioParams &) const --> float", pybind11::arg("otherMap"), pybind11::arg("otherMapPose"), pybind11::arg("params")); + cl.def("saveMetricMapRepresentationToFile", (void (mrpt::maps::CMetricMap::*)(const std::string &) const) &mrpt::maps::CMetricMap::saveMetricMapRepresentationToFile, "This virtual method saves the map to a file \"filNamePrefix\"+<\n some_file_extension >, as an image or in any other applicable way (Notice\n that other methods to save the map may be implemented in classes\n implementing this virtual interface). \n\nC++: mrpt::maps::CMetricMap::saveMetricMapRepresentationToFile(const std::string &) const --> void", pybind11::arg("filNamePrefix")); + cl.def("auxParticleFilterCleanUp", (void (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::auxParticleFilterCleanUp, "This method is called at the end of each \"prediction-update-map\n insertion\" cycle within\n \"mrpt::slam::CMetricMapBuilderRBPF::processActionObservation\".\n This method should normally do nothing, but in some cases can be used\n to free auxiliary cached variables.\n\nC++: mrpt::maps::CMetricMap::auxParticleFilterCleanUp() --> void"); + cl.def("squareDistanceToClosestCorrespondence", (float (mrpt::maps::CMetricMap::*)(float, float) const) &mrpt::maps::CMetricMap::squareDistanceToClosestCorrespondence, "Returns the square distance from the 2D point (x0,y0) to the closest\n correspondence in the map. \n\nC++: mrpt::maps::CMetricMap::squareDistanceToClosestCorrespondence(float, float) const --> float", pybind11::arg("x0"), pybind11::arg("y0")); + cl.def("getAsSimplePointsMap", (class mrpt::maps::CSimplePointsMap * (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::getAsSimplePointsMap, "C++: mrpt::maps::CMetricMap::getAsSimplePointsMap() --> class mrpt::maps::CSimplePointsMap *", pybind11::return_value_policy::automatic); + cl.def("assign", (class mrpt::maps::CMetricMap & (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap &)) &mrpt::maps::CMetricMap::operator=, "C++: mrpt::maps::CMetricMap::operator=(const class mrpt::maps::CMetricMap &) --> class mrpt::maps::CMetricMap &", pybind11::return_value_policy::automatic, pybind11::arg("")); + } +} diff --git a/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp b/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp new file mode 100644 index 0000000000..2cb5a1f559 --- /dev/null +++ b/python/src/mrpt/maps/CVoxelMapOccupancyBase_2.cpp @@ -0,0 +1,100 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include // __str__ +#include +#include + +#include +#include +#include +#include + + +#ifndef BINDER_PYBIND11_TYPE_CASTER + #define BINDER_PYBIND11_TYPE_CASTER + PYBIND11_DECLARE_HOLDER_TYPE(T, std::shared_ptr) + PYBIND11_DECLARE_HOLDER_TYPE(T, T*) + PYBIND11_MAKE_OPAQUE(std::shared_ptr) +#endif + +void bind_mrpt_maps_CVoxelMapOccupancyBase_2(std::function< pybind11::module &(std::string const &namespace_) > &M) +{ + { // mrpt::maps::CVoxelMapOccupancyBase file:mrpt/maps/CVoxelMapOccupancyBase.h line:114 + pybind11::class_, std::shared_ptr>, mrpt::maps::CVoxelMapBase> cl(M("mrpt::maps"), "CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccRGB_signed_char_t", ""); + cl.def_readwrite("insertionOptions", &mrpt::maps::CVoxelMapOccupancyBase::insertionOptions); + cl.def_readwrite("likelihoodOptions", &mrpt::maps::CVoxelMapOccupancyBase::likelihoodOptions); + cl.def_readwrite("renderingOptions", &mrpt::maps::CVoxelMapOccupancyBase::renderingOptions); + cl.def("isEmpty", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::isEmpty, "C++: mrpt::maps::CVoxelMapOccupancyBase::isEmpty() const --> bool"); + cl.def("getAsOctoMapVoxels", (void (mrpt::maps::CVoxelMapOccupancyBase::*)(class mrpt::opengl::COctoMapVoxels &) const) &mrpt::maps::CVoxelMapOccupancyBase::getAsOctoMapVoxels, "C++: mrpt::maps::CVoxelMapOccupancyBase::getAsOctoMapVoxels(class mrpt::opengl::COctoMapVoxels &) const --> void", pybind11::arg("gl_obj")); + cl.def("updateVoxel", (void (mrpt::maps::CVoxelMapOccupancyBase::*)(const double, const double, const double, bool)) &mrpt::maps::CVoxelMapOccupancyBase::updateVoxel, "C++: mrpt::maps::CVoxelMapOccupancyBase::updateVoxel(const double, const double, const double, bool) --> void", pybind11::arg("x"), pybind11::arg("y"), pybind11::arg("z"), pybind11::arg("occupied")); + cl.def("getPointOccupancy", (bool (mrpt::maps::CVoxelMapOccupancyBase::*)(const double, const double, const double, double &) const) &mrpt::maps::CVoxelMapOccupancyBase::getPointOccupancy, "C++: mrpt::maps::CVoxelMapOccupancyBase::getPointOccupancy(const double, const double, const double, double &) const --> bool", pybind11::arg("x"), pybind11::arg("y"), pybind11::arg("z"), pybind11::arg("prob_occupancy")); + cl.def("insertPointCloudAsRays", (void (mrpt::maps::CVoxelMapOccupancyBase::*)(const class mrpt::maps::CPointsMap &, const struct mrpt::math::TPoint3D_ &)) &mrpt::maps::CVoxelMapOccupancyBase::insertPointCloudAsRays, "C++: mrpt::maps::CVoxelMapOccupancyBase::insertPointCloudAsRays(const class mrpt::maps::CPointsMap &, const struct mrpt::math::TPoint3D_ &) --> void", pybind11::arg("pts"), pybind11::arg("sensorPt")); + cl.def("insertPointCloudAsEndPoints", (void (mrpt::maps::CVoxelMapOccupancyBase::*)(const class mrpt::maps::CPointsMap &, const struct mrpt::math::TPoint3D_ &)) &mrpt::maps::CVoxelMapOccupancyBase::insertPointCloudAsEndPoints, "C++: mrpt::maps::CVoxelMapOccupancyBase::insertPointCloudAsEndPoints(const class mrpt::maps::CPointsMap &, const struct mrpt::math::TPoint3D_ &) --> void", pybind11::arg("pts"), pybind11::arg("sensorPt")); + cl.def("getOccupiedVoxels", (class std::shared_ptr (mrpt::maps::CVoxelMapOccupancyBase::*)() const) &mrpt::maps::CVoxelMapOccupancyBase::getOccupiedVoxels, "C++: mrpt::maps::CVoxelMapOccupancyBase::getOccupiedVoxels() const --> class std::shared_ptr"); + cl.def("updateCell_fast_occupied", (void (mrpt::maps::CVoxelMapOccupancyBase::*)(struct mrpt::maps::VoxelNodeOccRGB *, const signed char, const signed char)) &mrpt::maps::CVoxelMapOccupancyBase::updateCell_fast_occupied, "C++: mrpt::maps::CVoxelMapOccupancyBase::updateCell_fast_occupied(struct mrpt::maps::VoxelNodeOccRGB *, const signed char, const signed char) --> void", pybind11::arg("theCell"), pybind11::arg("logodd_obs"), pybind11::arg("thres")); + cl.def("updateCell_fast_free", (void (mrpt::maps::CVoxelMapOccupancyBase::*)(struct mrpt::maps::VoxelNodeOccRGB *, const signed char, const signed char)) &mrpt::maps::CVoxelMapOccupancyBase::updateCell_fast_free, "C++: mrpt::maps::CVoxelMapOccupancyBase::updateCell_fast_free(struct mrpt::maps::VoxelNodeOccRGB *, const signed char, const signed char) --> void", pybind11::arg("theCell"), pybind11::arg("logodd_obs"), pybind11::arg("thres")); + cl.def_static("get_logodd_lut", (struct mrpt::maps::CLogOddsGridMapLUT & (*)()) &mrpt::maps::CVoxelMapOccupancyBase::get_logodd_lut, "C++: mrpt::maps::CVoxelMapOccupancyBase::get_logodd_lut() --> struct mrpt::maps::CLogOddsGridMapLUT &", pybind11::return_value_policy::automatic); + cl.def_static("l2p", (float (*)(const signed char)) &mrpt::maps::CVoxelMapOccupancyBase::l2p, "C++: mrpt::maps::CVoxelMapOccupancyBase::l2p(const signed char) --> float", pybind11::arg("l")); + cl.def_static("l2p_255", (uint8_t (*)(const signed char)) &mrpt::maps::CVoxelMapOccupancyBase::l2p_255, "C++: mrpt::maps::CVoxelMapOccupancyBase::l2p_255(const signed char) --> uint8_t", pybind11::arg("l")); + cl.def_static("p2l", (signed char (*)(const float)) &mrpt::maps::CVoxelMapOccupancyBase::p2l, "C++: mrpt::maps::CVoxelMapOccupancyBase::p2l(const float) --> signed char", pybind11::arg("p")); + cl.def("assign", (class mrpt::maps::CVoxelMapOccupancyBase & (mrpt::maps::CVoxelMapOccupancyBase::*)(const class mrpt::maps::CVoxelMapOccupancyBase &)) &mrpt::maps::CVoxelMapOccupancyBase::operator=, "C++: mrpt::maps::CVoxelMapOccupancyBase::operator=(const class mrpt::maps::CVoxelMapOccupancyBase &) --> class mrpt::maps::CVoxelMapOccupancyBase &", pybind11::return_value_policy::automatic, pybind11::arg("")); + cl.def("assign", (class mrpt::maps::CVoxelMapBase & (mrpt::maps::CVoxelMapBase::*)(const class mrpt::maps::CVoxelMapBase &)) &mrpt::maps::CVoxelMapBase::operator=, "C++: mrpt::maps::CVoxelMapBase::operator=(const class mrpt::maps::CVoxelMapBase &) --> class mrpt::maps::CVoxelMapBase &", pybind11::return_value_policy::automatic, pybind11::arg("o")); + cl.def("asString", (std::string (mrpt::maps::CVoxelMapBase::*)() const) &mrpt::maps::CVoxelMapBase::asString, "C++: mrpt::maps::CVoxelMapBase::asString() const --> std::string"); + cl.def("getVisualizationInto", (void (mrpt::maps::CVoxelMapBase::*)(class mrpt::opengl::CSetOfObjects &) const) &mrpt::maps::CVoxelMapBase::getVisualizationInto, "C++: mrpt::maps::CVoxelMapBase::getVisualizationInto(class mrpt::opengl::CSetOfObjects &) const --> void", pybind11::arg("o")); + cl.def("getAsOctoMapVoxels", (void (mrpt::maps::CVoxelMapBase::*)(class mrpt::opengl::COctoMapVoxels &) const) &mrpt::maps::CVoxelMapBase::getAsOctoMapVoxels, "C++: mrpt::maps::CVoxelMapBase::getAsOctoMapVoxels(class mrpt::opengl::COctoMapVoxels &) const --> void", pybind11::arg("gl_obj")); + cl.def("saveMetricMapRepresentationToFile", (void (mrpt::maps::CVoxelMapBase::*)(const std::string &) const) &mrpt::maps::CVoxelMapBase::saveMetricMapRepresentationToFile, "C++: mrpt::maps::CVoxelMapBase::saveMetricMapRepresentationToFile(const std::string &) const --> void", pybind11::arg("filNamePrefix")); + cl.def_readwrite("genericMapParams", &mrpt::maps::CMetricMap::genericMapParams); + cl.def("GetRuntimeClass", (const struct mrpt::rtti::TRuntimeClassId * (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::GetRuntimeClass, "C++: mrpt::maps::CMetricMap::GetRuntimeClass() const --> const struct mrpt::rtti::TRuntimeClassId *", pybind11::return_value_policy::automatic); + cl.def_static("GetRuntimeClassIdStatic", (const struct mrpt::rtti::TRuntimeClassId & (*)()) &mrpt::maps::CMetricMap::GetRuntimeClassIdStatic, "C++: mrpt::maps::CMetricMap::GetRuntimeClassIdStatic() --> const struct mrpt::rtti::TRuntimeClassId &", pybind11::return_value_policy::automatic); + cl.def("clear", (void (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::clear, "Erase all the contents of the map \n\nC++: mrpt::maps::CMetricMap::clear() --> void"); + cl.def("isEmpty", (bool (mrpt::maps::CMetricMap::*)() const) &mrpt::maps::CMetricMap::isEmpty, "Returns true if the map is empty/no observation has been inserted.\n\nC++: mrpt::maps::CMetricMap::isEmpty() const --> bool"); + cl.def("loadFromProbabilisticPosesAndObservations", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations, "Load the map contents from a CSimpleMap object, erasing all previous\n content of the map. This is done invoking `insertObservation()` for each\n observation at the mean 3D robot pose of each pose-observations pair in\n the CSimpleMap object.\n\n \n insertObservation, CSimpleMap\n \n\n std::exception Some internal steps in invoked methods can\n raise exceptions on invalid parameters, etc...\n\nC++: mrpt::maps::CMetricMap::loadFromProbabilisticPosesAndObservations(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); + cl.def("loadFromSimpleMap", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CSimpleMap &)) &mrpt::maps::CMetricMap::loadFromSimpleMap, "! \n\nC++: mrpt::maps::CMetricMap::loadFromSimpleMap(const class mrpt::maps::CSimpleMap &) --> void", pybind11::arg("Map")); + cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CObservation & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("obs")); + cl.def("insertObs", (bool (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D *)) &mrpt::maps::CMetricMap::insertObs, "C++: mrpt::maps::CMetricMap::insertObs(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D *) --> bool", pybind11::arg("obs"), pybind11::arg("robotPose")); + cl.def("insertObs", [](mrpt::maps::CMetricMap &o, const class mrpt::obs::CSensoryFrame & a0) -> bool { return o.insertObs(a0); }, "", pybind11::arg("sf")); + cl.def("insertObs", (bool (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CSensoryFrame &, const class mrpt::poses::CPose3D *)) &mrpt::maps::CMetricMap::insertObs, "C++: mrpt::maps::CMetricMap::insertObs(const class mrpt::obs::CSensoryFrame &, const class mrpt::poses::CPose3D *) --> bool", pybind11::arg("sf"), pybind11::arg("robotPose")); + cl.def("computeObservationLikelihood", (double (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D &) const) &mrpt::maps::CMetricMap::computeObservationLikelihood, "Computes the log-likelihood of a given observation given an arbitrary\n robot 3D pose.\n See: \n\n \n The robot's pose the observation is supposed to be taken\n from.\n \n\n The observation.\n \n\n This method returns a log-likelihood.\n\n \n Used in particle filter algorithms, see: CMultiMetricMapPDF::update\n\nC++: mrpt::maps::CMetricMap::computeObservationLikelihood(const class mrpt::obs::CObservation &, const class mrpt::poses::CPose3D &) const --> double", pybind11::arg("obs"), pybind11::arg("takenFrom")); + cl.def("canComputeObservationLikelihood", (bool (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CObservation &) const) &mrpt::maps::CMetricMap::canComputeObservationLikelihood, "Returns true if this map is able to compute a sensible likelihood\n function for this observation (i.e. an occupancy grid map cannot with an\n image).\n See: \n\n \n The observation.\n \n\n computeObservationLikelihood,\n genericMapParams.enableObservationLikelihood\n\nC++: mrpt::maps::CMetricMap::canComputeObservationLikelihood(const class mrpt::obs::CObservation &) const --> bool", pybind11::arg("obs")); + cl.def("computeObservationsLikelihood", (double (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CSensoryFrame &, const class mrpt::poses::CPose3D &)) &mrpt::maps::CMetricMap::computeObservationsLikelihood, "Returns the sum of the log-likelihoods of each individual observation\n within a mrpt::obs::CSensoryFrame.\n See: \n\n \n The robot's pose the observation is supposed to be taken\n from.\n \n\n The set of observations in a CSensoryFrame.\n \n\n This method returns a log-likelihood.\n \n\n canComputeObservationsLikelihood\n\nC++: mrpt::maps::CMetricMap::computeObservationsLikelihood(const class mrpt::obs::CSensoryFrame &, const class mrpt::poses::CPose3D &) --> double", pybind11::arg("sf"), pybind11::arg("takenFrom")); + cl.def("canComputeObservationsLikelihood", (bool (mrpt::maps::CMetricMap::*)(const class mrpt::obs::CSensoryFrame &) const) &mrpt::maps::CMetricMap::canComputeObservationsLikelihood, "Returns true if this map is able to compute a sensible likelihood\n function for this observation (i.e. an occupancy grid map cannot with an\n image).\n See: \n\n \n The observations.\n \n\n canComputeObservationLikelihood\n\nC++: mrpt::maps::CMetricMap::canComputeObservationsLikelihood(const class mrpt::obs::CSensoryFrame &) const --> bool", pybind11::arg("sf")); + cl.def("determineMatching2D", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose2D &, class mrpt::tfest::TMatchingPairListTempl &, const struct mrpt::maps::TMatchingParams &, struct mrpt::maps::TMatchingExtraResults &) const) &mrpt::maps::CMetricMap::determineMatching2D, "Computes the matching between this and another 2D point map, which\nincludes finding:\n - The set of points pairs in each map\n - The mean squared distance between corresponding pairs.\n\n The algorithm is:\n - For each point in \"otherMap\":\n - Transform the point according to otherMapPose\n - Search with a KD-TREE the closest correspondences in \"this\"\nmap.\n - Add to the set of candidate matchings, if it passes all the\nthresholds in params.\n\n This method is the most time critical one into ICP-like algorithms.\n\n \n [IN] The other map to compute the matching with.\n \n\n [IN] The pose of the other map as seen from\n\"this\".\n \n\n [IN] Parameters for the determination of\npairings.\n \n\n [OUT] The detected matchings pairs.\n \n\n [OUT] Other results.\n \n\n compute3DMatchingRatio\n\nC++: mrpt::maps::CMetricMap::determineMatching2D(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose2D &, class mrpt::tfest::TMatchingPairListTempl &, const struct mrpt::maps::TMatchingParams &, struct mrpt::maps::TMatchingExtraResults &) const --> void", pybind11::arg("otherMap"), pybind11::arg("otherMapPose"), pybind11::arg("correspondences"), pybind11::arg("params"), pybind11::arg("extraResults")); + cl.def("determineMatching3D", (void (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, class mrpt::tfest::TMatchingPairListTempl &, const struct mrpt::maps::TMatchingParams &, struct mrpt::maps::TMatchingExtraResults &) const) &mrpt::maps::CMetricMap::determineMatching3D, "Computes the matchings between this and another 3D points map - method\nused in 3D-ICP.\n This method finds the set of point pairs in each map.\n\n The method is the most time critical one into ICP-like algorithms.\n\n The algorithm is:\n - For each point in \"otherMap\":\n - Transform the point according to otherMapPose\n - Search with a KD-TREE the closest correspondences in \"this\"\nmap.\n - Add to the set of candidate matchings, if it passes all the\nthresholds in params.\n\n \n [IN] The other map to compute the matching with.\n \n\n [IN] The pose of the other map as seen from\n\"this\".\n \n\n [IN] Parameters for the determination of\npairings.\n \n\n [OUT] The detected matchings pairs.\n \n\n [OUT] Other results.\n \n\n compute3DMatchingRatio\n\nC++: mrpt::maps::CMetricMap::determineMatching3D(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, class mrpt::tfest::TMatchingPairListTempl &, const struct mrpt::maps::TMatchingParams &, struct mrpt::maps::TMatchingExtraResults &) const --> void", pybind11::arg("otherMap"), pybind11::arg("otherMapPose"), pybind11::arg("correspondences"), pybind11::arg("params"), pybind11::arg("extraResults")); + cl.def("compute3DMatchingRatio", (float (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, const struct mrpt::maps::TMatchingRatioParams &) const) &mrpt::maps::CMetricMap::compute3DMatchingRatio, "Computes the ratio in [0,1] of correspondences between \"this\" and the\n \"otherMap\" map, whose 6D pose relative to \"this\" is \"otherMapPose\"\n In the case of a multi-metric map, this returns the average between the\n maps. This method always return 0 for grid maps.\n \n\n [IN] The other map to compute the matching with.\n \n\n [IN] The 6D pose of the other map as seen from\n \"this\".\n \n\n [IN] Matching parameters\n \n\n The matching ratio [0,1]\n \n\n determineMatching2D\n\nC++: mrpt::maps::CMetricMap::compute3DMatchingRatio(const class mrpt::maps::CMetricMap *, const class mrpt::poses::CPose3D &, const struct mrpt::maps::TMatchingRatioParams &) const --> float", pybind11::arg("otherMap"), pybind11::arg("otherMapPose"), pybind11::arg("params")); + cl.def("saveMetricMapRepresentationToFile", (void (mrpt::maps::CMetricMap::*)(const std::string &) const) &mrpt::maps::CMetricMap::saveMetricMapRepresentationToFile, "This virtual method saves the map to a file \"filNamePrefix\"+<\n some_file_extension >, as an image or in any other applicable way (Notice\n that other methods to save the map may be implemented in classes\n implementing this virtual interface). \n\nC++: mrpt::maps::CMetricMap::saveMetricMapRepresentationToFile(const std::string &) const --> void", pybind11::arg("filNamePrefix")); + cl.def("auxParticleFilterCleanUp", (void (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::auxParticleFilterCleanUp, "This method is called at the end of each \"prediction-update-map\n insertion\" cycle within\n \"mrpt::slam::CMetricMapBuilderRBPF::processActionObservation\".\n This method should normally do nothing, but in some cases can be used\n to free auxiliary cached variables.\n\nC++: mrpt::maps::CMetricMap::auxParticleFilterCleanUp() --> void"); + cl.def("squareDistanceToClosestCorrespondence", (float (mrpt::maps::CMetricMap::*)(float, float) const) &mrpt::maps::CMetricMap::squareDistanceToClosestCorrespondence, "Returns the square distance from the 2D point (x0,y0) to the closest\n correspondence in the map. \n\nC++: mrpt::maps::CMetricMap::squareDistanceToClosestCorrespondence(float, float) const --> float", pybind11::arg("x0"), pybind11::arg("y0")); + cl.def("getAsSimplePointsMap", (class mrpt::maps::CSimplePointsMap * (mrpt::maps::CMetricMap::*)()) &mrpt::maps::CMetricMap::getAsSimplePointsMap, "C++: mrpt::maps::CMetricMap::getAsSimplePointsMap() --> class mrpt::maps::CSimplePointsMap *", pybind11::return_value_policy::automatic); + cl.def("assign", (class mrpt::maps::CMetricMap & (mrpt::maps::CMetricMap::*)(const class mrpt::maps::CMetricMap &)) &mrpt::maps::CMetricMap::operator=, "C++: mrpt::maps::CMetricMap::operator=(const class mrpt::maps::CMetricMap &) --> class mrpt::maps::CMetricMap &", pybind11::return_value_policy::automatic, pybind11::arg("")); + } +} diff --git a/python/src/mrpt/maps/metric_map_types.cpp b/python/src/mrpt/maps/metric_map_types.cpp index 73339d33d8..e8c18b2d66 100644 --- a/python/src/mrpt/maps/metric_map_types.cpp +++ b/python/src/mrpt/maps/metric_map_types.cpp @@ -210,7 +210,7 @@ void bind_mrpt_maps_metric_map_types(std::function< pybind11::module &(std::stri cl.def( pybind11::init( [](mrpt::maps::TSetOfMetricMapInitializers const &o){ return new mrpt::maps::TSetOfMetricMapInitializers(o); } ) ); cl.def("size", (size_t (mrpt::maps::TSetOfMetricMapInitializers::*)() const) &mrpt::maps::TSetOfMetricMapInitializers::size, "C++: mrpt::maps::TSetOfMetricMapInitializers::size() const --> size_t"); cl.def("clear", (void (mrpt::maps::TSetOfMetricMapInitializers::*)()) &mrpt::maps::TSetOfMetricMapInitializers::clear, "C++: mrpt::maps::TSetOfMetricMapInitializers::clear() --> void"); - cl.def("loadFromConfigFile", (void (mrpt::maps::TSetOfMetricMapInitializers::*)(const class mrpt::config::CConfigFileBase &, const std::string &)) &mrpt::maps::TSetOfMetricMapInitializers::loadFromConfigFile, "Loads the configuration for the set of internal maps from a textual\ndefinition in an INI-like file.\n The format of the ini file is defined in CConfigFile. The list\nof maps and their options\n will be loaded from a handle of sections:\n\n \n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n Where:\n - ##: Represents the index of the map (e.g. \"00\",\"01\",...)\n - By default, the variables into each \"TOptions\" structure of the\nmaps\nare defined in textual form by the same name of the corresponding C++\nvariable (e.g. \"float resolution;\" -> \"resolution=0.10\")\n\n \n Examples of map definitions can be found in the '.ini' files\nprovided in the demo directories: \"share/mrpt/config-files/\"\n\nC++: mrpt::maps::TSetOfMetricMapInitializers::loadFromConfigFile(const class mrpt::config::CConfigFileBase &, const std::string &) --> void", pybind11::arg("source"), pybind11::arg("sectionName")); + cl.def("loadFromConfigFile", (void (mrpt::maps::TSetOfMetricMapInitializers::*)(const class mrpt::config::CConfigFileBase &, const std::string &)) &mrpt::maps::TSetOfMetricMapInitializers::loadFromConfigFile, "Loads the configuration for the set of internal maps from a textual\ndefinition in an INI-like file.\n The format of the ini file is defined in CConfigFile. The list\nof maps and their options\n will be loaded from a handle of sections:\n\n \n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n\n Where:\n - ##: Represents the index of the map (e.g. \"00\",\"01\",...)\n - By default, the variables into each \"TOptions\" structure of the\nmaps\nare defined in textual form by the same name of the corresponding C++\nvariable (e.g. \"float resolution;\" -> \"resolution=0.10\")\n\n \n Examples of map definitions can be found in the '.ini' files\nprovided in the demo directories: \"share/mrpt/config-files/\"\n\nC++: mrpt::maps::TSetOfMetricMapInitializers::loadFromConfigFile(const class mrpt::config::CConfigFileBase &, const std::string &) --> void", pybind11::arg("source"), pybind11::arg("sectionName")); cl.def("saveToConfigFile", (void (mrpt::maps::TSetOfMetricMapInitializers::*)(class mrpt::config::CConfigFileBase &, const std::string &) const) &mrpt::maps::TSetOfMetricMapInitializers::saveToConfigFile, "C++: mrpt::maps::TSetOfMetricMapInitializers::saveToConfigFile(class mrpt::config::CConfigFileBase &, const std::string &) const --> void", pybind11::arg("target"), pybind11::arg("section")); cl.def("assign", (class mrpt::maps::TSetOfMetricMapInitializers & (mrpt::maps::TSetOfMetricMapInitializers::*)(const class mrpt::maps::TSetOfMetricMapInitializers &)) &mrpt::maps::TSetOfMetricMapInitializers::operator=, "C++: mrpt::maps::TSetOfMetricMapInitializers::operator=(const class mrpt::maps::TSetOfMetricMapInitializers &) --> class mrpt::maps::TSetOfMetricMapInitializers &", pybind11::return_value_policy::automatic, pybind11::arg("")); } diff --git a/python/src/mrpt/opengl/COctoMapVoxels.cpp b/python/src/mrpt/opengl/COctoMapVoxels.cpp index c15e1eb87c..722a508176 100644 --- a/python/src/mrpt/opengl/COctoMapVoxels.cpp +++ b/python/src/mrpt/opengl/COctoMapVoxels.cpp @@ -336,6 +336,7 @@ void bind_mrpt_opengl_COctoMapVoxels(std::function< pybind11::module &(std::stri .value("TRANS_AND_COLOR_FROM_OCCUPANCY", mrpt::opengl::COctoMapVoxels::TRANS_AND_COLOR_FROM_OCCUPANCY) .value("MIXED", mrpt::opengl::COctoMapVoxels::MIXED) .value("FIXED", mrpt::opengl::COctoMapVoxels::FIXED) + .value("COLOR_FROM_RGB_DATA", mrpt::opengl::COctoMapVoxels::COLOR_FROM_RGB_DATA) .export_values(); cl.def_static("Create", (class std::shared_ptr (*)()) &mrpt::opengl::COctoMapVoxels::Create, "C++: mrpt::opengl::COctoMapVoxels::Create() --> class std::shared_ptr"); @@ -386,7 +387,7 @@ void bind_mrpt_opengl_COctoMapVoxels(std::function< pybind11::module &(std::stri cl.def("internalBoundingBoxLocal", (struct mrpt::math::TBoundingBox_ (mrpt::opengl::COctoMapVoxels::*)() const) &mrpt::opengl::COctoMapVoxels::internalBoundingBoxLocal, "C++: mrpt::opengl::COctoMapVoxels::internalBoundingBoxLocal() const --> struct mrpt::math::TBoundingBox_"); cl.def("assign", (class mrpt::opengl::COctoMapVoxels & (mrpt::opengl::COctoMapVoxels::*)(const class mrpt::opengl::COctoMapVoxels &)) &mrpt::opengl::COctoMapVoxels::operator=, "C++: mrpt::opengl::COctoMapVoxels::operator=(const class mrpt::opengl::COctoMapVoxels &) --> class mrpt::opengl::COctoMapVoxels &", pybind11::return_value_policy::automatic, pybind11::arg("")); - { // mrpt::opengl::COctoMapVoxels::TVoxel file:mrpt/opengl/COctoMapVoxels.h line:93 + { // mrpt::opengl::COctoMapVoxels::TVoxel file:mrpt/opengl/COctoMapVoxels.h line:95 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TVoxel", "The info of each of the voxels "); cl.def( pybind11::init( [](){ return new mrpt::opengl::COctoMapVoxels::TVoxel(); } ) ); @@ -399,7 +400,7 @@ void bind_mrpt_opengl_COctoMapVoxels(std::function< pybind11::module &(std::stri cl.def("assign", (struct mrpt::opengl::COctoMapVoxels::TVoxel & (mrpt::opengl::COctoMapVoxels::TVoxel::*)(const struct mrpt::opengl::COctoMapVoxels::TVoxel &)) &mrpt::opengl::COctoMapVoxels::TVoxel::operator=, "C++: mrpt::opengl::COctoMapVoxels::TVoxel::operator=(const struct mrpt::opengl::COctoMapVoxels::TVoxel &) --> struct mrpt::opengl::COctoMapVoxels::TVoxel &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::opengl::COctoMapVoxels::TGridCube file:mrpt/opengl/COctoMapVoxels.h line:109 + { // mrpt::opengl::COctoMapVoxels::TGridCube file:mrpt/opengl/COctoMapVoxels.h line:111 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TGridCube", "The info of each grid block "); cl.def( pybind11::init( [](){ return new mrpt::opengl::COctoMapVoxels::TGridCube(); } ) ); @@ -411,7 +412,7 @@ void bind_mrpt_opengl_COctoMapVoxels(std::function< pybind11::module &(std::stri cl.def("assign", (struct mrpt::opengl::COctoMapVoxels::TGridCube & (mrpt::opengl::COctoMapVoxels::TGridCube::*)(const struct mrpt::opengl::COctoMapVoxels::TGridCube &)) &mrpt::opengl::COctoMapVoxels::TGridCube::operator=, "C++: mrpt::opengl::COctoMapVoxels::TGridCube::operator=(const struct mrpt::opengl::COctoMapVoxels::TGridCube &) --> struct mrpt::opengl::COctoMapVoxels::TGridCube &", pybind11::return_value_policy::automatic, pybind11::arg("")); } - { // mrpt::opengl::COctoMapVoxels::TInfoPerVoxelSet file:mrpt/opengl/COctoMapVoxels.h line:123 + { // mrpt::opengl::COctoMapVoxels::TInfoPerVoxelSet file:mrpt/opengl/COctoMapVoxels.h line:125 auto & enclosing_class = cl; pybind11::class_> cl(enclosing_class, "TInfoPerVoxelSet", ""); cl.def( pybind11::init( [](){ return new mrpt::opengl::COctoMapVoxels::TInfoPerVoxelSet(); } ) ); diff --git a/python/src/mrpt/rtti/CObject.cpp b/python/src/mrpt/rtti/CObject.cpp index bb4134b5fc..0918d66668 100644 --- a/python/src/mrpt/rtti/CObject.cpp +++ b/python/src/mrpt/rtti/CObject.cpp @@ -13,7 +13,6 @@ #include #include #include -#include #include #include #include @@ -23,6 +22,8 @@ #include #include #include +#include +#include #include #include #include @@ -35,11 +36,8 @@ #include #include #include -#include #include #include -#include -#include #include #include #include @@ -48,7 +46,6 @@ #include #include #include -#include #include #include #include @@ -167,18 +164,18 @@ void bind_mrpt_rtti_CObject(std::function< pybind11::module &(std::string const cl.def_static("get", (const struct mrpt::rtti::TRuntimeClassId * (*)()) &mrpt::rtti::CLASS_ID_impl::get, "C++: mrpt::rtti::CLASS_ID_impl::get() --> const struct mrpt::rtti::TRuntimeClassId *", pybind11::return_value_policy::automatic); } { // mrpt::rtti::CLASS_ID_impl file:mrpt/rtti/CObject.h line:92 - pybind11::class_, std::shared_ptr>> cl(M("mrpt::rtti"), "CLASS_ID_impl_mrpt_obs_CObservation_t", ""); - cl.def( pybind11::init( [](){ return new mrpt::rtti::CLASS_ID_impl(); } ) ); - cl.def_static("get", (const struct mrpt::rtti::TRuntimeClassId * (*)()) &mrpt::rtti::CLASS_ID_impl::get, "C++: mrpt::rtti::CLASS_ID_impl::get() --> const struct mrpt::rtti::TRuntimeClassId *", pybind11::return_value_policy::automatic); + pybind11::class_, std::shared_ptr>> cl(M("mrpt::rtti"), "CLASS_ID_impl_mrpt_maps_CVoxelMap_t", ""); + cl.def( pybind11::init( [](){ return new mrpt::rtti::CLASS_ID_impl(); } ) ); + cl.def_static("get", (const struct mrpt::rtti::TRuntimeClassId * (*)()) &mrpt::rtti::CLASS_ID_impl::get, "C++: mrpt::rtti::CLASS_ID_impl::get() --> const struct mrpt::rtti::TRuntimeClassId *", pybind11::return_value_policy::automatic); } { // mrpt::rtti::CLASS_ID_impl file:mrpt/rtti/CObject.h line:92 - pybind11::class_, std::shared_ptr>> cl(M("mrpt::rtti"), "CLASS_ID_impl_mrpt_obs_CSensoryFrame_t", ""); - cl.def( pybind11::init( [](){ return new mrpt::rtti::CLASS_ID_impl(); } ) ); - cl.def_static("get", (const struct mrpt::rtti::TRuntimeClassId * (*)()) &mrpt::rtti::CLASS_ID_impl::get, "C++: mrpt::rtti::CLASS_ID_impl::get() --> const struct mrpt::rtti::TRuntimeClassId *", pybind11::return_value_policy::automatic); + pybind11::class_, std::shared_ptr>> cl(M("mrpt::rtti"), "CLASS_ID_impl_mrpt_maps_CVoxelMapRGB_t", ""); + cl.def( pybind11::init( [](){ return new mrpt::rtti::CLASS_ID_impl(); } ) ); + cl.def_static("get", (const struct mrpt::rtti::TRuntimeClassId * (*)()) &mrpt::rtti::CLASS_ID_impl::get, "C++: mrpt::rtti::CLASS_ID_impl::get() --> const struct mrpt::rtti::TRuntimeClassId *", pybind11::return_value_policy::automatic); } { // mrpt::rtti::CLASS_ID_impl file:mrpt/rtti/CObject.h line:92 - pybind11::class_, std::shared_ptr>> cl(M("mrpt::rtti"), "CLASS_ID_impl_mrpt_maps_CLandmarksMap_t", ""); - cl.def( pybind11::init( [](){ return new mrpt::rtti::CLASS_ID_impl(); } ) ); - cl.def_static("get", (const struct mrpt::rtti::TRuntimeClassId * (*)()) &mrpt::rtti::CLASS_ID_impl::get, "C++: mrpt::rtti::CLASS_ID_impl::get() --> const struct mrpt::rtti::TRuntimeClassId *", pybind11::return_value_policy::automatic); + pybind11::class_, std::shared_ptr>> cl(M("mrpt::rtti"), "CLASS_ID_impl_mrpt_obs_CObservation_t", ""); + cl.def( pybind11::init( [](){ return new mrpt::rtti::CLASS_ID_impl(); } ) ); + cl.def_static("get", (const struct mrpt::rtti::TRuntimeClassId * (*)()) &mrpt::rtti::CLASS_ID_impl::get, "C++: mrpt::rtti::CLASS_ID_impl::get() --> const struct mrpt::rtti::TRuntimeClassId *", pybind11::return_value_policy::automatic); } } diff --git a/python/src/mrpt/rtti/CObject_1.cpp b/python/src/mrpt/rtti/CObject_1.cpp index 72875d7710..7aa14546e0 100644 --- a/python/src/mrpt/rtti/CObject_1.cpp +++ b/python/src/mrpt/rtti/CObject_1.cpp @@ -1,5 +1,29 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include +#include +#include +#include +#include #include // __str__ +#include +#include #include #include @@ -16,12 +40,14 @@ void bind_mrpt_rtti_CObject_1(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::rtti::internal::CopyCtor file:mrpt/rtti/CObject.h line:124 - pybind11::class_, std::shared_ptr>> cl(M("mrpt::rtti::internal"), "CopyCtor_true_t", ""); - cl.def( pybind11::init( [](){ return new mrpt::rtti::internal::CopyCtor(); } ) ); + { // mrpt::rtti::CLASS_ID_impl file:mrpt/rtti/CObject.h line:92 + pybind11::class_, std::shared_ptr>> cl(M("mrpt::rtti"), "CLASS_ID_impl_mrpt_obs_CSensoryFrame_t", ""); + cl.def( pybind11::init( [](){ return new mrpt::rtti::CLASS_ID_impl(); } ) ); + cl.def_static("get", (const struct mrpt::rtti::TRuntimeClassId * (*)()) &mrpt::rtti::CLASS_ID_impl::get, "C++: mrpt::rtti::CLASS_ID_impl::get() --> const struct mrpt::rtti::TRuntimeClassId *", pybind11::return_value_policy::automatic); } - { // mrpt::rtti::internal::CopyCtor file:mrpt/rtti/CObject.h line:133 - pybind11::class_, std::shared_ptr>> cl(M("mrpt::rtti::internal"), "CopyCtor_false_t", ""); - cl.def( pybind11::init( [](){ return new mrpt::rtti::internal::CopyCtor(); } ) ); + { // mrpt::rtti::CLASS_ID_impl file:mrpt/rtti/CObject.h line:92 + pybind11::class_, std::shared_ptr>> cl(M("mrpt::rtti"), "CLASS_ID_impl_mrpt_maps_CLandmarksMap_t", ""); + cl.def( pybind11::init( [](){ return new mrpt::rtti::CLASS_ID_impl(); } ) ); + cl.def_static("get", (const struct mrpt::rtti::TRuntimeClassId * (*)()) &mrpt::rtti::CLASS_ID_impl::get, "C++: mrpt::rtti::CLASS_ID_impl::get() --> const struct mrpt::rtti::TRuntimeClassId *", pybind11::return_value_policy::automatic); } } diff --git a/python/src/mrpt/rtti/CObject_2.cpp b/python/src/mrpt/rtti/CObject_2.cpp index 9d28273803..a50c75ffe4 100644 --- a/python/src/mrpt/rtti/CObject_2.cpp +++ b/python/src/mrpt/rtti/CObject_2.cpp @@ -1,8 +1,5 @@ -#include -#include #include #include // __str__ -#include #include #include @@ -17,54 +14,14 @@ PYBIND11_MAKE_OPAQUE(std::shared_ptr) #endif -// mrpt::rtti::CObject file:mrpt/rtti/CObject.h line:178 -struct PyCallBack_mrpt_rtti_CObject : public mrpt::rtti::CObject { - using mrpt::rtti::CObject::CObject; - - const struct mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override { - pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "GetRuntimeClass"); - if (overload) { - auto o = overload.operator()(); - if (pybind11::detail::cast_is_temporary_value_reference::value) { - static pybind11::detail::override_caster_t caster; - return pybind11::detail::cast_ref(std::move(o), caster); - } - else return pybind11::detail::cast_safe(std::move(o)); - } - return CObject::GetRuntimeClass(); - } - class mrpt::rtti::CObject * clone() const override { - pybind11::gil_scoped_acquire gil; - pybind11::function overload = pybind11::get_overload(static_cast(this), "clone"); - if (overload) { - auto o = overload.operator()(); - if (pybind11::detail::cast_is_temporary_value_reference::value) { - static pybind11::detail::override_caster_t caster; - return pybind11::detail::cast_ref(std::move(o), caster); - } - else return pybind11::detail::cast_safe(std::move(o)); - } - pybind11::pybind11_fail("Tried to call pure virtual function \"CObject::clone\""); - } -}; - void bind_mrpt_rtti_CObject_2(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::rtti::CObject file:mrpt/rtti/CObject.h line:178 - pybind11::class_, PyCallBack_mrpt_rtti_CObject> cl(M("mrpt::rtti"), "CObject", "Virtual base to provide a compiler-independent RTTI system.\n\n Each class named `Foo` will have associated smart pointer types:\n - `Foo::Ptr` => `std::shared_ptr` (the most commonly-used one)\n - `Foo::ConstPtr` => `std::shared_ptr`\n - `Foo::UniquePtr` => `std::unique_ptr`\n - `Foo::ConstUniquePtr` => `std::unique_ptr`\n\n It is recommended to use MRPT-defined `std::make_shared<>` instead\n of `std::make_shared<>` to create objects, to avoid memory alignment\n problems caused by classes containing Eigen vectors or matrices. Example:\n \n\n\n\n Or using the shorter auxiliary static method `::Create()` for conciseness or\n to keep compatibility with MRPT 1.5.* code bases:\n \n\n\n\n If a special memory allocator is needed, use `Foo::CreateAlloc(alloc,...);`.\n \n\n mrpt::rtti::CObject\n \n\n\n "); - cl.def(pybind11::init()); - cl.def( pybind11::init( [](){ return new PyCallBack_mrpt_rtti_CObject(); } ) ); - cl.def_static("GetRuntimeClassIdStatic", (const struct mrpt::rtti::TRuntimeClassId & (*)()) &mrpt::rtti::CObject::GetRuntimeClassIdStatic, "C++: mrpt::rtti::CObject::GetRuntimeClassIdStatic() --> const struct mrpt::rtti::TRuntimeClassId &", pybind11::return_value_policy::automatic); - cl.def("GetRuntimeClass", (const struct mrpt::rtti::TRuntimeClassId * (mrpt::rtti::CObject::*)() const) &mrpt::rtti::CObject::GetRuntimeClass, "Returns information about the class of an object in runtime. \n\nC++: mrpt::rtti::CObject::GetRuntimeClass() const --> const struct mrpt::rtti::TRuntimeClassId *", pybind11::return_value_policy::automatic); - cl.def("duplicateGetSmartPtr", (class std::shared_ptr (mrpt::rtti::CObject::*)() const) &mrpt::rtti::CObject::duplicateGetSmartPtr, "Makes a deep copy of the object and returns a smart pointer to it \n\nC++: mrpt::rtti::CObject::duplicateGetSmartPtr() const --> class std::shared_ptr"); - cl.def("clone", (class mrpt::rtti::CObject * (mrpt::rtti::CObject::*)() const) &mrpt::rtti::CObject::clone, "Returns a deep copy (clone) of the object, indepently of its class. \n\nC++: mrpt::rtti::CObject::clone() const --> class mrpt::rtti::CObject *", pybind11::return_value_policy::automatic); - cl.def("assign", (class mrpt::rtti::CObject & (mrpt::rtti::CObject::*)(const class mrpt::rtti::CObject &)) &mrpt::rtti::CObject::operator=, "C++: mrpt::rtti::CObject::operator=(const class mrpt::rtti::CObject &) --> class mrpt::rtti::CObject &", pybind11::return_value_policy::automatic, pybind11::arg("")); + { // mrpt::rtti::internal::CopyCtor file:mrpt/rtti/CObject.h line:124 + pybind11::class_, std::shared_ptr>> cl(M("mrpt::rtti::internal"), "CopyCtor_true_t", ""); + cl.def( pybind11::init( [](){ return new mrpt::rtti::internal::CopyCtor(); } ) ); + } + { // mrpt::rtti::internal::CopyCtor file:mrpt/rtti/CObject.h line:133 + pybind11::class_, std::shared_ptr>> cl(M("mrpt::rtti::internal"), "CopyCtor_false_t", ""); + cl.def( pybind11::init( [](){ return new mrpt::rtti::internal::CopyCtor(); } ) ); } - // mrpt::rtti::registerAllPendingClasses() file:mrpt/rtti/CObject.h line:339 - M("mrpt::rtti").def("registerAllPendingClasses", (void (*)()) &mrpt::rtti::registerAllPendingClasses, "Register all pending classes - to be called just before\n de-serializing an object, for example. After calling this method,\n pending_class_registers_modified is set to false until\n pending_class_registers() is invoked.\n\nC++: mrpt::rtti::registerAllPendingClasses() --> void"); - - // mrpt::rtti::classFactory(const std::string &) file:mrpt/rtti/CObject.h line:343 - M("mrpt::rtti").def("classFactory", (class std::shared_ptr (*)(const std::string &)) &mrpt::rtti::classFactory, "Creates an object given by its registered name.\n \n\n findRegisteredClass(), registerClass() \n\nC++: mrpt::rtti::classFactory(const std::string &) --> class std::shared_ptr", pybind11::arg("className")); - } diff --git a/python/src/mrpt/rtti/CObject_3.cpp b/python/src/mrpt/rtti/CObject_3.cpp index c75249f536..c1c727b175 100644 --- a/python/src/mrpt/rtti/CObject_3.cpp +++ b/python/src/mrpt/rtti/CObject_3.cpp @@ -1,7 +1,8 @@ +#include #include #include -#include #include // __str__ +#include #include #include @@ -16,11 +17,54 @@ PYBIND11_MAKE_OPAQUE(std::shared_ptr) #endif +// mrpt::rtti::CObject file:mrpt/rtti/CObject.h line:178 +struct PyCallBack_mrpt_rtti_CObject : public mrpt::rtti::CObject { + using mrpt::rtti::CObject::CObject; + + const struct mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "GetRuntimeClass"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + return CObject::GetRuntimeClass(); + } + class mrpt::rtti::CObject * clone() const override { + pybind11::gil_scoped_acquire gil; + pybind11::function overload = pybind11::get_overload(static_cast(this), "clone"); + if (overload) { + auto o = overload.operator()(); + if (pybind11::detail::cast_is_temporary_value_reference::value) { + static pybind11::detail::override_caster_t caster; + return pybind11::detail::cast_ref(std::move(o), caster); + } + else return pybind11::detail::cast_safe(std::move(o)); + } + pybind11::pybind11_fail("Tried to call pure virtual function \"CObject::clone\""); + } +}; + void bind_mrpt_rtti_CObject_3(std::function< pybind11::module &(std::string const &namespace_) > &M) { - { // mrpt::ptr_cast file:mrpt/rtti/CObject.h line:354 - pybind11::class_, std::shared_ptr>> cl(M("mrpt"), "ptr_cast_mrpt_serialization_CSerializable_t", ""); - cl.def( pybind11::init( [](){ return new mrpt::ptr_cast(); } ) ); - cl.def_static("from", (class std::shared_ptr (*)(const class std::shared_ptr &)) &mrpt::ptr_cast::from>, "C++: mrpt::ptr_cast::from(const class std::shared_ptr &) --> class std::shared_ptr", pybind11::arg("ptr")); + { // mrpt::rtti::CObject file:mrpt/rtti/CObject.h line:178 + pybind11::class_, PyCallBack_mrpt_rtti_CObject> cl(M("mrpt::rtti"), "CObject", "Virtual base to provide a compiler-independent RTTI system.\n\n Each class named `Foo` will have associated smart pointer types:\n - `Foo::Ptr` => `std::shared_ptr` (the most commonly-used one)\n - `Foo::ConstPtr` => `std::shared_ptr`\n - `Foo::UniquePtr` => `std::unique_ptr`\n - `Foo::ConstUniquePtr` => `std::unique_ptr`\n\n It is recommended to use MRPT-defined `std::make_shared<>` instead\n of `std::make_shared<>` to create objects, to avoid memory alignment\n problems caused by classes containing Eigen vectors or matrices. Example:\n \n\n\n\n Or using the shorter auxiliary static method `::Create()` for conciseness or\n to keep compatibility with MRPT 1.5.* code bases:\n \n\n\n\n If a special memory allocator is needed, use `Foo::CreateAlloc(alloc,...);`.\n \n\n mrpt::rtti::CObject\n \n\n\n "); + cl.def(pybind11::init()); + cl.def( pybind11::init( [](){ return new PyCallBack_mrpt_rtti_CObject(); } ) ); + cl.def_static("GetRuntimeClassIdStatic", (const struct mrpt::rtti::TRuntimeClassId & (*)()) &mrpt::rtti::CObject::GetRuntimeClassIdStatic, "C++: mrpt::rtti::CObject::GetRuntimeClassIdStatic() --> const struct mrpt::rtti::TRuntimeClassId &", pybind11::return_value_policy::automatic); + cl.def("GetRuntimeClass", (const struct mrpt::rtti::TRuntimeClassId * (mrpt::rtti::CObject::*)() const) &mrpt::rtti::CObject::GetRuntimeClass, "Returns information about the class of an object in runtime. \n\nC++: mrpt::rtti::CObject::GetRuntimeClass() const --> const struct mrpt::rtti::TRuntimeClassId *", pybind11::return_value_policy::automatic); + cl.def("duplicateGetSmartPtr", (class std::shared_ptr (mrpt::rtti::CObject::*)() const) &mrpt::rtti::CObject::duplicateGetSmartPtr, "Makes a deep copy of the object and returns a smart pointer to it \n\nC++: mrpt::rtti::CObject::duplicateGetSmartPtr() const --> class std::shared_ptr"); + cl.def("clone", (class mrpt::rtti::CObject * (mrpt::rtti::CObject::*)() const) &mrpt::rtti::CObject::clone, "Returns a deep copy (clone) of the object, indepently of its class. \n\nC++: mrpt::rtti::CObject::clone() const --> class mrpt::rtti::CObject *", pybind11::return_value_policy::automatic); + cl.def("assign", (class mrpt::rtti::CObject & (mrpt::rtti::CObject::*)(const class mrpt::rtti::CObject &)) &mrpt::rtti::CObject::operator=, "C++: mrpt::rtti::CObject::operator=(const class mrpt::rtti::CObject &) --> class mrpt::rtti::CObject &", pybind11::return_value_policy::automatic, pybind11::arg("")); } + // mrpt::rtti::registerAllPendingClasses() file:mrpt/rtti/CObject.h line:339 + M("mrpt::rtti").def("registerAllPendingClasses", (void (*)()) &mrpt::rtti::registerAllPendingClasses, "Register all pending classes - to be called just before\n de-serializing an object, for example. After calling this method,\n pending_class_registers_modified is set to false until\n pending_class_registers() is invoked.\n\nC++: mrpt::rtti::registerAllPendingClasses() --> void"); + + // mrpt::rtti::classFactory(const std::string &) file:mrpt/rtti/CObject.h line:343 + M("mrpt::rtti").def("classFactory", (class std::shared_ptr (*)(const std::string &)) &mrpt::rtti::classFactory, "Creates an object given by its registered name.\n \n\n findRegisteredClass(), registerClass() \n\nC++: mrpt::rtti::classFactory(const std::string &) --> class std::shared_ptr", pybind11::arg("className")); + } diff --git a/python/src/mrpt/rtti/CObject_4.cpp b/python/src/mrpt/rtti/CObject_4.cpp new file mode 100644 index 0000000000..c019500fe6 --- /dev/null +++ b/python/src/mrpt/rtti/CObject_4.cpp @@ -0,0 +1,26 @@ +#include +#include +#include +#include // __str__ + +#include +#include +#include +#include + + +#ifndef BINDER_PYBIND11_TYPE_CASTER + #define BINDER_PYBIND11_TYPE_CASTER + PYBIND11_DECLARE_HOLDER_TYPE(T, std::shared_ptr) + PYBIND11_DECLARE_HOLDER_TYPE(T, T*) + PYBIND11_MAKE_OPAQUE(std::shared_ptr) +#endif + +void bind_mrpt_rtti_CObject_4(std::function< pybind11::module &(std::string const &namespace_) > &M) +{ + { // mrpt::ptr_cast file:mrpt/rtti/CObject.h line:354 + pybind11::class_, std::shared_ptr>> cl(M("mrpt"), "ptr_cast_mrpt_serialization_CSerializable_t", ""); + cl.def( pybind11::init( [](){ return new mrpt::ptr_cast(); } ) ); + cl.def_static("from", (class std::shared_ptr (*)(const class std::shared_ptr &)) &mrpt::ptr_cast::from>, "C++: mrpt::ptr_cast::from(const class std::shared_ptr &) --> class std::shared_ptr", pybind11::arg("ptr")); + } +} diff --git a/python/src/pymrpt.cpp b/python/src/pymrpt.cpp index eff598c148..e4a28636a0 100644 --- a/python/src/pymrpt.cpp +++ b/python/src/pymrpt.cpp @@ -41,6 +41,7 @@ void bind_mrpt_rtti_CObject(std::function< pybind11::module &(std::string const void bind_mrpt_rtti_CObject_1(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_rtti_CObject_2(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_rtti_CObject_3(std::function< pybind11::module &(std::string const &namespace_) > &M); +void bind_mrpt_rtti_CObject_4(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_serialization_CSerializable(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_std_stl_multimap(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_std_stl_map(std::function< pybind11::module &(std::string const &namespace_) > &M); @@ -266,6 +267,11 @@ void bind_mrpt_maps_CRandomFieldGridMap3D(std::function< pybind11::module &(std: void bind_mrpt_maps_CReflectivityGridMap2D(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_maps_CWeightedPointsMap(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_maps_CWirelessPowerGridMap2D(std::function< pybind11::module &(std::string const &namespace_) > &M); +void bind_mrpt_maps_CVoxelMapBase(std::function< pybind11::module &(std::string const &namespace_) > &M); +void bind_mrpt_maps_CVoxelMapOccupancyBase(std::function< pybind11::module &(std::string const &namespace_) > &M); +void bind_mrpt_maps_CVoxelMapOccupancyBase_1(std::function< pybind11::module &(std::string const &namespace_) > &M); +void bind_mrpt_maps_CVoxelMapOccupancyBase_2(std::function< pybind11::module &(std::string const &namespace_) > &M); +void bind_mrpt_maps_CVoxelMap(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_obs_CObservationPointCloud(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_opengl_CBox(std::function< pybind11::module &(std::string const &namespace_) > &M); void bind_mrpt_opengl_COctreePointRenderer(std::function< pybind11::module &(std::string const &namespace_) > &M); @@ -463,6 +469,7 @@ PYBIND11_MODULE(pymrpt, root_module) { bind_mrpt_rtti_CObject_1(M); bind_mrpt_rtti_CObject_2(M); bind_mrpt_rtti_CObject_3(M); + bind_mrpt_rtti_CObject_4(M); bind_mrpt_serialization_CSerializable(M); bind_std_stl_multimap(M); bind_std_stl_map(M); @@ -688,6 +695,11 @@ PYBIND11_MODULE(pymrpt, root_module) { bind_mrpt_maps_CReflectivityGridMap2D(M); bind_mrpt_maps_CWeightedPointsMap(M); bind_mrpt_maps_CWirelessPowerGridMap2D(M); + bind_mrpt_maps_CVoxelMapBase(M); + bind_mrpt_maps_CVoxelMapOccupancyBase(M); + bind_mrpt_maps_CVoxelMapOccupancyBase_1(M); + bind_mrpt_maps_CVoxelMapOccupancyBase_2(M); + bind_mrpt_maps_CVoxelMap(M); bind_mrpt_obs_CObservationPointCloud(M); bind_mrpt_opengl_CBox(M); bind_mrpt_opengl_COctreePointRenderer(M); diff --git a/python/src/pymrpt.sources b/python/src/pymrpt.sources index 178433eefa..ae6dd7aa12 100644 --- a/python/src/pymrpt.sources +++ b/python/src/pymrpt.sources @@ -31,6 +31,7 @@ mrpt/rtti/CObject.cpp mrpt/rtti/CObject_1.cpp mrpt/rtti/CObject_2.cpp mrpt/rtti/CObject_3.cpp +mrpt/rtti/CObject_4.cpp mrpt/serialization/CSerializable.cpp std/stl_multimap.cpp std/stl_map.cpp @@ -256,6 +257,11 @@ mrpt/maps/CRandomFieldGridMap3D.cpp mrpt/maps/CReflectivityGridMap2D.cpp mrpt/maps/CWeightedPointsMap.cpp mrpt/maps/CWirelessPowerGridMap2D.cpp +mrpt/maps/CVoxelMapBase.cpp +mrpt/maps/CVoxelMapOccupancyBase.cpp +mrpt/maps/CVoxelMapOccupancyBase_1.cpp +mrpt/maps/CVoxelMapOccupancyBase_2.cpp +mrpt/maps/CVoxelMap.cpp mrpt/obs/CObservationPointCloud.cpp mrpt/opengl/CBox.cpp mrpt/opengl/COctreePointRenderer.cpp diff --git a/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi b/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi index 609fd2b283..6145d80537 100644 --- a/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi +++ b/python/stubs-out/mrpt/pymrpt/mrpt/maps.pyi @@ -3553,6 +3553,7 @@ class CSimplePointsMap(CPointsMap): def __init__(self, arg0: CSimplePointsMap) -> None: ... @overload def __init__(self, arg0: CSimplePointsMap) -> None: ... + def Create(self, *args, **kwargs) -> Any: ... def CreateObject(self, *args, **kwargs) -> Any: ... def GetRuntimeClass(self) -> mrpt.pymrpt.mrpt.rtti.TRuntimeClassId: ... def GetRuntimeClassIdStatic(self, *args, **kwargs) -> Any: ... @@ -3576,6 +3577,414 @@ class CSimplePointsMap(CPointsMap): @overload def setSize(size_t) -> void: ... +class CVoxelMap(CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccupancy_signed_char_t): + class TMapDefinition(CVoxelMap.TMapDefinitionBase): + inner_bits: int + insertionOpts: TVoxelMap_InsertionOptions + leaf_bits: int + likelihoodOpts: TVoxelMap_LikelihoodOptions + resolution: float + @overload + def __init__(self) -> None: ... + @overload + def __init__(self, arg0: CVoxelMap.TMapDefinition) -> None: ... + @overload + def __init__(self, arg0: CVoxelMap.TMapDefinition) -> None: ... + + class TMapDefinitionBase: + def __init__(self, *args, **kwargs) -> None: ... + @overload + def __init__(self) -> None: ... + @overload + def __init__(self, arg0: float) -> None: ... + @overload + def __init__(self, arg0: float, arg1: int) -> None: ... + @overload + def __init__(self, resolution: float, inner_bits: int, leaf_bits: int) -> None: ... + @overload + def __init__(self, arg0: CVoxelMap) -> None: ... + @overload + def __init__(self, arg0: CVoxelMap) -> None: ... + def CreateObject(self, *args, **kwargs) -> Any: ... + def GetRuntimeClass(self) -> mrpt.pymrpt.mrpt.rtti.TRuntimeClassId: ... + def GetRuntimeClassIdStatic(self, *args, **kwargs) -> Any: ... + def assign(self) -> CVoxelMap: ... + def clone(self) -> mrpt.pymrpt.mrpt.rtti.CObject: ... + +class CVoxelMapBase_mrpt_maps_VoxelNodeOccRGB_t(CMetricMap): + genericMapParams: TMapGenericParams + def __init__(self, *args, **kwargs) -> None: ... + def GetRuntimeClass(self) -> mrpt.pymrpt.mrpt.rtti.TRuntimeClassId: ... + def GetRuntimeClassIdStatic(self, *args, **kwargs) -> Any: ... + def asString(self) -> str: ... + @overload + def assign(self, o: CVoxelMapBase_mrpt_maps_VoxelNodeOccRGB_t) -> CVoxelMapBase_mrpt_maps_VoxelNodeOccRGB_t: ... + @overload + def assign(self) -> CMetricMap: ... + @overload + def auxParticleFilterCleanUp(self) -> None: ... + @overload + def auxParticleFilterCleanUp() -> void: ... + @overload + def canComputeObservationLikelihood(self, obs: mrpt.pymrpt.mrpt.obs.CObservation) -> bool: ... + @overload + def canComputeObservationLikelihood(constclassmrpt) -> bool: ... + @overload + def canComputeObservationsLikelihood(self, sf: mrpt.pymrpt.mrpt.obs.CSensoryFrame) -> bool: ... + @overload + def canComputeObservationsLikelihood(constclassmrpt) -> bool: ... + @overload + def clear(self) -> None: ... + @overload + def clear() -> void: ... + def compute3DMatchingRatio(self, *args, **kwargs) -> Any: ... + def computeObservationLikelihood(self, obs: mrpt.pymrpt.mrpt.obs.CObservation, takenFrom: mrpt.pymrpt.mrpt.poses.CPose3D) -> float: ... + def computeObservationsLikelihood(self, sf: mrpt.pymrpt.mrpt.obs.CSensoryFrame, takenFrom: mrpt.pymrpt.mrpt.poses.CPose3D) -> float: ... + def determineMatching2D(self, *args, **kwargs) -> Any: ... + def determineMatching3D(self, *args, **kwargs) -> Any: ... + @overload + def getAsOctoMapVoxels(self, gl_obj: mrpt.pymrpt.mrpt.opengl.COctoMapVoxels) -> None: ... + @overload + def getAsOctoMapVoxels(classmrpt) -> void: ... + def getAsSimplePointsMap(self) -> CSimplePointsMap: ... + @overload + def getVisualizationInto(self, o: mrpt.pymrpt.mrpt.opengl.CSetOfObjects) -> None: ... + @overload + def getVisualizationInto(classmrpt) -> void: ... + @overload + def insertObs(self, obs: mrpt.pymrpt.mrpt.obs.CObservation) -> bool: ... + @overload + def insertObs(self, obs: mrpt.pymrpt.mrpt.obs.CObservation, robotPose: mrpt.pymrpt.mrpt.poses.CPose3D) -> bool: ... + @overload + def insertObs(self, sf: mrpt.pymrpt.mrpt.obs.CSensoryFrame) -> bool: ... + @overload + def insertObs(self, sf: mrpt.pymrpt.mrpt.obs.CSensoryFrame, robotPose: mrpt.pymrpt.mrpt.poses.CPose3D) -> bool: ... + @overload + def isEmpty(self) -> bool: ... + @overload + def isEmpty() -> bool: ... + @overload + def loadFromProbabilisticPosesAndObservations(self, Map: CSimpleMap) -> None: ... + @overload + def loadFromProbabilisticPosesAndObservations(constclassmrpt) -> void: ... + @overload + def loadFromSimpleMap(self, Map: CSimpleMap) -> None: ... + @overload + def loadFromSimpleMap(constclassmrpt) -> void: ... + @overload + def saveMetricMapRepresentationToFile(self, filNamePrefix: str) -> None: ... + @overload + def saveMetricMapRepresentationToFile(conststd) -> void: ... + @overload + def saveMetricMapRepresentationToFile(self, filNamePrefix: str) -> None: ... + @overload + def saveMetricMapRepresentationToFile(conststd) -> void: ... + def squareDistanceToClosestCorrespondence(self, x0: float, y0: float) -> float: ... + +class CVoxelMapBase_mrpt_maps_VoxelNodeOccupancy_t(CMetricMap): + genericMapParams: TMapGenericParams + def __init__(self, *args, **kwargs) -> None: ... + def GetRuntimeClass(self) -> mrpt.pymrpt.mrpt.rtti.TRuntimeClassId: ... + def GetRuntimeClassIdStatic(self, *args, **kwargs) -> Any: ... + def asString(self) -> str: ... + @overload + def assign(self, o: CVoxelMapBase_mrpt_maps_VoxelNodeOccupancy_t) -> CVoxelMapBase_mrpt_maps_VoxelNodeOccupancy_t: ... + @overload + def assign(self) -> CMetricMap: ... + @overload + def auxParticleFilterCleanUp(self) -> None: ... + @overload + def auxParticleFilterCleanUp() -> void: ... + @overload + def canComputeObservationLikelihood(self, obs: mrpt.pymrpt.mrpt.obs.CObservation) -> bool: ... + @overload + def canComputeObservationLikelihood(constclassmrpt) -> bool: ... + @overload + def canComputeObservationsLikelihood(self, sf: mrpt.pymrpt.mrpt.obs.CSensoryFrame) -> bool: ... + @overload + def canComputeObservationsLikelihood(constclassmrpt) -> bool: ... + @overload + def clear(self) -> None: ... + @overload + def clear() -> void: ... + def compute3DMatchingRatio(self, *args, **kwargs) -> Any: ... + def computeObservationLikelihood(self, obs: mrpt.pymrpt.mrpt.obs.CObservation, takenFrom: mrpt.pymrpt.mrpt.poses.CPose3D) -> float: ... + def computeObservationsLikelihood(self, sf: mrpt.pymrpt.mrpt.obs.CSensoryFrame, takenFrom: mrpt.pymrpt.mrpt.poses.CPose3D) -> float: ... + def determineMatching2D(self, *args, **kwargs) -> Any: ... + def determineMatching3D(self, *args, **kwargs) -> Any: ... + @overload + def getAsOctoMapVoxels(self, gl_obj: mrpt.pymrpt.mrpt.opengl.COctoMapVoxels) -> None: ... + @overload + def getAsOctoMapVoxels(classmrpt) -> void: ... + def getAsSimplePointsMap(self) -> CSimplePointsMap: ... + @overload + def getVisualizationInto(self, o: mrpt.pymrpt.mrpt.opengl.CSetOfObjects) -> None: ... + @overload + def getVisualizationInto(classmrpt) -> void: ... + @overload + def insertObs(self, obs: mrpt.pymrpt.mrpt.obs.CObservation) -> bool: ... + @overload + def insertObs(self, obs: mrpt.pymrpt.mrpt.obs.CObservation, robotPose: mrpt.pymrpt.mrpt.poses.CPose3D) -> bool: ... + @overload + def insertObs(self, sf: mrpt.pymrpt.mrpt.obs.CSensoryFrame) -> bool: ... + @overload + def insertObs(self, sf: mrpt.pymrpt.mrpt.obs.CSensoryFrame, robotPose: mrpt.pymrpt.mrpt.poses.CPose3D) -> bool: ... + @overload + def isEmpty(self) -> bool: ... + @overload + def isEmpty() -> bool: ... + @overload + def loadFromProbabilisticPosesAndObservations(self, Map: CSimpleMap) -> None: ... + @overload + def loadFromProbabilisticPosesAndObservations(constclassmrpt) -> void: ... + @overload + def loadFromSimpleMap(self, Map: CSimpleMap) -> None: ... + @overload + def loadFromSimpleMap(constclassmrpt) -> void: ... + @overload + def saveMetricMapRepresentationToFile(self, filNamePrefix: str) -> None: ... + @overload + def saveMetricMapRepresentationToFile(conststd) -> void: ... + @overload + def saveMetricMapRepresentationToFile(self, filNamePrefix: str) -> None: ... + @overload + def saveMetricMapRepresentationToFile(conststd) -> void: ... + def squareDistanceToClosestCorrespondence(self, x0: float, y0: float) -> float: ... + +class CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccRGB_signed_char_t(CVoxelMapBase_mrpt_maps_VoxelNodeOccRGB_t): + genericMapParams: TMapGenericParams + insertionOptions: TVoxelMap_InsertionOptions + likelihoodOptions: TVoxelMap_LikelihoodOptions + renderingOptions: TVoxelMap_RenderingOptions + def __init__(self, *args, **kwargs) -> None: ... + def GetRuntimeClass(self) -> mrpt.pymrpt.mrpt.rtti.TRuntimeClassId: ... + def GetRuntimeClassIdStatic(self, *args, **kwargs) -> Any: ... + def asString(self) -> str: ... + @overload + def assign(self) -> CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccRGB_signed_char_t: ... + @overload + def assign(self, o: CVoxelMapBase_mrpt_maps_VoxelNodeOccRGB_t) -> CVoxelMapBase_mrpt_maps_VoxelNodeOccRGB_t: ... + @overload + def assign(self) -> CMetricMap: ... + @overload + def auxParticleFilterCleanUp(self) -> None: ... + @overload + def auxParticleFilterCleanUp() -> void: ... + @overload + def canComputeObservationLikelihood(self, obs: mrpt.pymrpt.mrpt.obs.CObservation) -> bool: ... + @overload + def canComputeObservationLikelihood(constclassmrpt) -> bool: ... + @overload + def canComputeObservationsLikelihood(self, sf: mrpt.pymrpt.mrpt.obs.CSensoryFrame) -> bool: ... + @overload + def canComputeObservationsLikelihood(constclassmrpt) -> bool: ... + @overload + def clear(self) -> None: ... + @overload + def clear() -> void: ... + def compute3DMatchingRatio(self, *args, **kwargs) -> Any: ... + def computeObservationLikelihood(self, obs: mrpt.pymrpt.mrpt.obs.CObservation, takenFrom: mrpt.pymrpt.mrpt.poses.CPose3D) -> float: ... + def computeObservationsLikelihood(self, sf: mrpt.pymrpt.mrpt.obs.CSensoryFrame, takenFrom: mrpt.pymrpt.mrpt.poses.CPose3D) -> float: ... + def determineMatching2D(self, *args, **kwargs) -> Any: ... + def determineMatching3D(self, *args, **kwargs) -> Any: ... + @overload + def getAsOctoMapVoxels(self, gl_obj: mrpt.pymrpt.mrpt.opengl.COctoMapVoxels) -> None: ... + @overload + def getAsOctoMapVoxels(classmrpt) -> void: ... + @overload + def getAsOctoMapVoxels(self, gl_obj: mrpt.pymrpt.mrpt.opengl.COctoMapVoxels) -> None: ... + @overload + def getAsOctoMapVoxels(classmrpt) -> void: ... + def getAsSimplePointsMap(self) -> CSimplePointsMap: ... + def getOccupiedVoxels(self) -> CSimplePointsMap: ... + def getPointOccupancy(self, x: float, y: float, z: float, prob_occupancy: float) -> bool: ... + @overload + def getVisualizationInto(self, o: mrpt.pymrpt.mrpt.opengl.CSetOfObjects) -> None: ... + @overload + def getVisualizationInto(classmrpt) -> void: ... + def get_logodd_lut(self, *args, **kwargs) -> Any: ... + @overload + def insertObs(self, obs: mrpt.pymrpt.mrpt.obs.CObservation) -> bool: ... + @overload + def insertObs(self, obs: mrpt.pymrpt.mrpt.obs.CObservation, robotPose: mrpt.pymrpt.mrpt.poses.CPose3D) -> bool: ... + @overload + def insertObs(self, sf: mrpt.pymrpt.mrpt.obs.CSensoryFrame) -> bool: ... + @overload + def insertObs(self, sf: mrpt.pymrpt.mrpt.obs.CSensoryFrame, robotPose: mrpt.pymrpt.mrpt.poses.CPose3D) -> bool: ... + @overload + def insertPointCloudAsEndPoints(self, pts: CPointsMap, sensorPt) -> None: ... + @overload + def insertPointCloudAsEndPoints(constclassmrpt, conststructmrpt) -> void: ... + @overload + def insertPointCloudAsRays(self, pts: CPointsMap, sensorPt) -> None: ... + @overload + def insertPointCloudAsRays(constclassmrpt, conststructmrpt) -> void: ... + @overload + def isEmpty(self) -> bool: ... + @overload + def isEmpty() -> bool: ... + @overload + def isEmpty(self) -> bool: ... + @overload + def isEmpty() -> bool: ... + def l2p(self, *args, **kwargs) -> Any: ... + def l2p_255(self, *args, **kwargs) -> Any: ... + @overload + def loadFromProbabilisticPosesAndObservations(self, Map: CSimpleMap) -> None: ... + @overload + def loadFromProbabilisticPosesAndObservations(constclassmrpt) -> void: ... + @overload + def loadFromSimpleMap(self, Map: CSimpleMap) -> None: ... + @overload + def loadFromSimpleMap(constclassmrpt) -> void: ... + def p2l(self, *args, **kwargs) -> Any: ... + @overload + def saveMetricMapRepresentationToFile(self, filNamePrefix: str) -> None: ... + @overload + def saveMetricMapRepresentationToFile(conststd) -> void: ... + @overload + def saveMetricMapRepresentationToFile(self, filNamePrefix: str) -> None: ... + @overload + def saveMetricMapRepresentationToFile(conststd) -> void: ... + def squareDistanceToClosestCorrespondence(self, x0: float, y0: float) -> float: ... + def updateCell_fast_free(self, theCell, logodd_obs: int, thres: int) -> None: ... + def updateCell_fast_occupied(self, theCell, logodd_obs: int, thres: int) -> None: ... + def updateVoxel(self, x: float, y: float, z: float, occupied: bool) -> None: ... + +class CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccupancy_signed_char_t(CVoxelMapBase_mrpt_maps_VoxelNodeOccupancy_t): + genericMapParams: TMapGenericParams + insertionOptions: TVoxelMap_InsertionOptions + likelihoodOptions: TVoxelMap_LikelihoodOptions + renderingOptions: TVoxelMap_RenderingOptions + def __init__(self, *args, **kwargs) -> None: ... + def GetRuntimeClass(self) -> mrpt.pymrpt.mrpt.rtti.TRuntimeClassId: ... + def GetRuntimeClassIdStatic(self, *args, **kwargs) -> Any: ... + def asString(self) -> str: ... + @overload + def assign(self) -> CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccupancy_signed_char_t: ... + @overload + def assign(self, o: CVoxelMapBase_mrpt_maps_VoxelNodeOccupancy_t) -> CVoxelMapBase_mrpt_maps_VoxelNodeOccupancy_t: ... + @overload + def assign(self) -> CMetricMap: ... + @overload + def auxParticleFilterCleanUp(self) -> None: ... + @overload + def auxParticleFilterCleanUp() -> void: ... + @overload + def canComputeObservationLikelihood(self, obs: mrpt.pymrpt.mrpt.obs.CObservation) -> bool: ... + @overload + def canComputeObservationLikelihood(constclassmrpt) -> bool: ... + @overload + def canComputeObservationsLikelihood(self, sf: mrpt.pymrpt.mrpt.obs.CSensoryFrame) -> bool: ... + @overload + def canComputeObservationsLikelihood(constclassmrpt) -> bool: ... + @overload + def clear(self) -> None: ... + @overload + def clear() -> void: ... + def compute3DMatchingRatio(self, *args, **kwargs) -> Any: ... + def computeObservationLikelihood(self, obs: mrpt.pymrpt.mrpt.obs.CObservation, takenFrom: mrpt.pymrpt.mrpt.poses.CPose3D) -> float: ... + def computeObservationsLikelihood(self, sf: mrpt.pymrpt.mrpt.obs.CSensoryFrame, takenFrom: mrpt.pymrpt.mrpt.poses.CPose3D) -> float: ... + def determineMatching2D(self, *args, **kwargs) -> Any: ... + def determineMatching3D(self, *args, **kwargs) -> Any: ... + @overload + def getAsOctoMapVoxels(self, gl_obj: mrpt.pymrpt.mrpt.opengl.COctoMapVoxels) -> None: ... + @overload + def getAsOctoMapVoxels(classmrpt) -> void: ... + @overload + def getAsOctoMapVoxels(self, gl_obj: mrpt.pymrpt.mrpt.opengl.COctoMapVoxels) -> None: ... + @overload + def getAsOctoMapVoxels(classmrpt) -> void: ... + def getAsSimplePointsMap(self) -> CSimplePointsMap: ... + def getOccupiedVoxels(self) -> CSimplePointsMap: ... + def getPointOccupancy(self, x: float, y: float, z: float, prob_occupancy: float) -> bool: ... + @overload + def getVisualizationInto(self, o: mrpt.pymrpt.mrpt.opengl.CSetOfObjects) -> None: ... + @overload + def getVisualizationInto(classmrpt) -> void: ... + def get_logodd_lut(self, *args, **kwargs) -> Any: ... + @overload + def insertObs(self, obs: mrpt.pymrpt.mrpt.obs.CObservation) -> bool: ... + @overload + def insertObs(self, obs: mrpt.pymrpt.mrpt.obs.CObservation, robotPose: mrpt.pymrpt.mrpt.poses.CPose3D) -> bool: ... + @overload + def insertObs(self, sf: mrpt.pymrpt.mrpt.obs.CSensoryFrame) -> bool: ... + @overload + def insertObs(self, sf: mrpt.pymrpt.mrpt.obs.CSensoryFrame, robotPose: mrpt.pymrpt.mrpt.poses.CPose3D) -> bool: ... + @overload + def insertPointCloudAsEndPoints(self, pts: CPointsMap, sensorPt) -> None: ... + @overload + def insertPointCloudAsEndPoints(constclassmrpt, conststructmrpt) -> void: ... + @overload + def insertPointCloudAsRays(self, pts: CPointsMap, sensorPt) -> None: ... + @overload + def insertPointCloudAsRays(constclassmrpt, conststructmrpt) -> void: ... + @overload + def isEmpty(self) -> bool: ... + @overload + def isEmpty() -> bool: ... + @overload + def isEmpty(self) -> bool: ... + @overload + def isEmpty() -> bool: ... + def l2p(self, *args, **kwargs) -> Any: ... + def l2p_255(self, *args, **kwargs) -> Any: ... + @overload + def loadFromProbabilisticPosesAndObservations(self, Map: CSimpleMap) -> None: ... + @overload + def loadFromProbabilisticPosesAndObservations(constclassmrpt) -> void: ... + @overload + def loadFromSimpleMap(self, Map: CSimpleMap) -> None: ... + @overload + def loadFromSimpleMap(constclassmrpt) -> void: ... + def p2l(self, *args, **kwargs) -> Any: ... + @overload + def saveMetricMapRepresentationToFile(self, filNamePrefix: str) -> None: ... + @overload + def saveMetricMapRepresentationToFile(conststd) -> void: ... + @overload + def saveMetricMapRepresentationToFile(self, filNamePrefix: str) -> None: ... + @overload + def saveMetricMapRepresentationToFile(conststd) -> void: ... + def squareDistanceToClosestCorrespondence(self, x0: float, y0: float) -> float: ... + def updateCell_fast_free(self, theCell, logodd_obs: int, thres: int) -> None: ... + def updateCell_fast_occupied(self, theCell, logodd_obs: int, thres: int) -> None: ... + def updateVoxel(self, x: float, y: float, z: float, occupied: bool) -> None: ... + +class CVoxelMapRGB(CVoxelMapOccupancyBase_mrpt_maps_VoxelNodeOccRGB_signed_char_t): + class TMapDefinition(CVoxelMapRGB.TMapDefinitionBase): + inner_bits: int + insertionOpts: TVoxelMap_InsertionOptions + leaf_bits: int + likelihoodOpts: TVoxelMap_LikelihoodOptions + resolution: float + @overload + def __init__(self) -> None: ... + @overload + def __init__(self, arg0: CVoxelMapRGB.TMapDefinition) -> None: ... + @overload + def __init__(self, arg0: CVoxelMapRGB.TMapDefinition) -> None: ... + + class TMapDefinitionBase: + def __init__(self, *args, **kwargs) -> None: ... + @overload + def __init__(self) -> None: ... + @overload + def __init__(self, arg0: float) -> None: ... + @overload + def __init__(self, arg0: float, arg1: int) -> None: ... + @overload + def __init__(self, resolution: float, inner_bits: int, leaf_bits: int) -> None: ... + @overload + def __init__(self, arg0: CVoxelMapRGB) -> None: ... + @overload + def __init__(self, arg0: CVoxelMapRGB) -> None: ... + def CreateObject(self, *args, **kwargs) -> Any: ... + def GetRuntimeClass(self) -> mrpt.pymrpt.mrpt.rtti.TRuntimeClassId: ... + def GetRuntimeClassIdStatic(self, *args, **kwargs) -> Any: ... + def assign(self) -> CVoxelMapRGB: ... + def clone(self) -> mrpt.pymrpt.mrpt.rtti.CObject: ... + class CWeightedPointsMap(CPointsMap): class TMapDefinition(CWeightedPointsMap.TMapDefinitionBase): insertionOpts: CPointsMap.TInsertionOptions @@ -3792,6 +4201,103 @@ class TSetOfMetricMapInitializers(mrpt.pymrpt.mrpt.config.CLoadableOptions): @overload def size() -> size_t: ... +class TVoxelMap_InsertionOptions(mrpt.pymrpt.mrpt.config.CLoadableOptions): + clamp_max: float + clamp_min: float + decimation: int + max_range: float + prob_hit: float + prob_miss: float + ray_trace_free_space: bool + @overload + def __init__(self) -> None: ... + @overload + def __init__(self, arg0: TVoxelMap_InsertionOptions) -> None: ... + @overload + def __init__(self, arg0: TVoxelMap_InsertionOptions) -> None: ... + def assign(self) -> TVoxelMap_InsertionOptions: ... + @overload + def loadFromConfigFile(self, source: mrpt.pymrpt.mrpt.config.CConfigFileBase, section: str) -> None: ... + @overload + def loadFromConfigFile(constclassmrpt, conststd) -> void: ... + @overload + def readFromStream(self, in: mrpt.pymrpt.mrpt.serialization.CArchive) -> None: ... + @overload + def readFromStream(classmrpt) -> void: ... + @overload + def saveToConfigFile(self, c: mrpt.pymrpt.mrpt.config.CConfigFileBase, s: str) -> None: ... + @overload + def saveToConfigFile(classmrpt, conststd) -> void: ... + @overload + def writeToStream(self, out: mrpt.pymrpt.mrpt.serialization.CArchive) -> None: ... + @overload + def writeToStream(classmrpt) -> void: ... + +class TVoxelMap_LikelihoodOptions(mrpt.pymrpt.mrpt.config.CLoadableOptions): + decimation: int + occupiedThreshold: float + @overload + def __init__(self) -> None: ... + @overload + def __init__(self, arg0: TVoxelMap_LikelihoodOptions) -> None: ... + @overload + def __init__(self, arg0: TVoxelMap_LikelihoodOptions) -> None: ... + def assign(self) -> TVoxelMap_LikelihoodOptions: ... + @overload + def loadFromConfigFile(self, source: mrpt.pymrpt.mrpt.config.CConfigFileBase, section: str) -> None: ... + @overload + def loadFromConfigFile(constclassmrpt, conststd) -> void: ... + @overload + def readFromStream(self, in: mrpt.pymrpt.mrpt.serialization.CArchive) -> None: ... + @overload + def readFromStream(classmrpt) -> void: ... + @overload + def saveToConfigFile(self, c: mrpt.pymrpt.mrpt.config.CConfigFileBase, s: str) -> None: ... + @overload + def saveToConfigFile(classmrpt, conststd) -> void: ... + @overload + def writeToStream(self, out: mrpt.pymrpt.mrpt.serialization.CArchive) -> None: ... + @overload + def writeToStream(classmrpt) -> void: ... + +class TVoxelMap_RenderingOptions: + freeThreshold: float + generateFreeVoxels: bool + generateOccupiedVoxels: bool + occupiedThreshold: float + visibleFreeVoxels: bool + visibleOccupiedVoxels: bool + @overload + def __init__(self) -> None: ... + @overload + def __init__(self, arg0: TVoxelMap_RenderingOptions) -> None: ... + def assign(self) -> TVoxelMap_RenderingOptions: ... + @overload + def readFromStream(self, in: mrpt.pymrpt.mrpt.serialization.CArchive) -> None: ... + @overload + def readFromStream(classmrpt) -> void: ... + @overload + def writeToStream(self, out: mrpt.pymrpt.mrpt.serialization.CArchive) -> None: ... + @overload + def writeToStream(classmrpt) -> void: ... + +class VoxelNodeOccRGB: + class TColor: + B: int + G: int + R: int + def __init__(self) -> None: ... + color: Any + numColObs: int + occupancy: int + def __init__(self) -> None: ... + def occupancyRef(self) -> int: ... + +class VoxelNodeOccupancy: + occupancy: int + def __init__(self) -> None: ... + def occupancyRef(self) -> int: ... + class mrptEventMetricMapClear(mrpt.pymrpt.mrpt.system.mrptEvent): def __init__(self, smap) -> None: ... def assign(self) -> mrptEventMetricMapClear: ... diff --git a/python/stubs-out/mrpt/pymrpt/mrpt/opengl/__init__.pyi b/python/stubs-out/mrpt/pymrpt/mrpt/opengl/__init__.pyi index d29cf054a1..17a6c797a4 100644 --- a/python/stubs-out/mrpt/pymrpt/mrpt/opengl/__init__.pyi +++ b/python/stubs-out/mrpt/pymrpt/mrpt/opengl/__init__.pyi @@ -1513,6 +1513,7 @@ class COctoMapVoxels(CRenderizableShaderTriangles, CRenderizableShaderWireFrame, __members__: ClassVar[dict] = ... # read-only COLOR_FROM_HEIGHT: ClassVar[COctoMapVoxels.visualization_mode_t] = ... COLOR_FROM_OCCUPANCY: ClassVar[COctoMapVoxels.visualization_mode_t] = ... + COLOR_FROM_RGB_DATA: ClassVar[COctoMapVoxels.visualization_mode_t] = ... FIXED: ClassVar[COctoMapVoxels.visualization_mode_t] = ... MIXED: ClassVar[COctoMapVoxels.visualization_mode_t] = ... TRANSPARENCY_FROM_OCCUPANCY: ClassVar[COctoMapVoxels.visualization_mode_t] = ... @@ -1543,6 +1544,7 @@ class COctoMapVoxels(CRenderizableShaderTriangles, CRenderizableShaderWireFrame, def value(self) -> int: ... COLOR_FROM_HEIGHT: ClassVar[COctoMapVoxels.visualization_mode_t] = ... COLOR_FROM_OCCUPANCY: ClassVar[COctoMapVoxels.visualization_mode_t] = ... + COLOR_FROM_RGB_DATA: ClassVar[COctoMapVoxels.visualization_mode_t] = ... FIXED: ClassVar[COctoMapVoxels.visualization_mode_t] = ... MIXED: ClassVar[COctoMapVoxels.visualization_mode_t] = ... TRANSPARENCY_FROM_OCCUPANCY: ClassVar[COctoMapVoxels.visualization_mode_t] = ... diff --git a/python/stubs-out/mrpt/pymrpt/mrpt/rtti/__init__.pyi b/python/stubs-out/mrpt/pymrpt/mrpt/rtti/__init__.pyi index 07186eda7b..26e0cb8ef8 100644 --- a/python/stubs-out/mrpt/pymrpt/mrpt/rtti/__init__.pyi +++ b/python/stubs-out/mrpt/pymrpt/mrpt/rtti/__init__.pyi @@ -55,6 +55,14 @@ class CLASS_ID_impl_mrpt_maps_CSimplePointsMap_t: def __init__(self) -> None: ... def get(self, *args, **kwargs) -> Any: ... +class CLASS_ID_impl_mrpt_maps_CVoxelMapRGB_t: + def __init__(self) -> None: ... + def get(self, *args, **kwargs) -> Any: ... + +class CLASS_ID_impl_mrpt_maps_CVoxelMap_t: + def __init__(self) -> None: ... + def get(self, *args, **kwargs) -> Any: ... + class CLASS_ID_impl_mrpt_maps_CWeightedPointsMap_t: def __init__(self) -> None: ... def get(self, *args, **kwargs) -> Any: ... diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt index 3ae80c14b2..ee4b9a8275 100644 --- a/samples/CMakeLists.txt +++ b/samples/CMakeLists.txt @@ -287,6 +287,8 @@ if(MRPT_BUILD_EXAMPLES) maps_gridmap3D_simple maps_observer_pattern_example maps_octomap_simple + maps_voxelmap_simple + maps_voxelmap_from_tum_dataset maps_gmrf_map_example maps_ransac_data_association ) diff --git a/samples/maps_voxelmap_from_tum_dataset/CMakeLists.txt b/samples/maps_voxelmap_from_tum_dataset/CMakeLists.txt new file mode 100644 index 0000000000..5691e926c9 --- /dev/null +++ b/samples/maps_voxelmap_from_tum_dataset/CMakeLists.txt @@ -0,0 +1,62 @@ +#----------------------------------------------------------------------------------------------- +# CMake file for the MRPT example: /maps_voxelmap_from_tum_dataset +# +# Run with "ccmake ." at the root directory, or use it as a template for +# starting your own programs +#----------------------------------------------------------------------------------------------- +set(sampleName maps_voxelmap_from_tum_dataset) +project(EXAMPLE_${sampleName}) + +cmake_minimum_required(VERSION 3.1) + +# --------------------------------------------------------------------------- +# Set the output directory of each example to its corresponding subdirectory +# in the binary tree: +# --------------------------------------------------------------------------- +set(EXECUTABLE_OUTPUT_PATH ".") + +# The list of "libs" which can be included can be found in: +# https://www.mrpt.org/Libraries +# Add the top-level dependencies only. +# -------------------------------------------------------------------------- +foreach(dep maps;gui) + # if not building from inside MRPT source tree, find it as a cmake + # imported project: + if (NOT TARGET mrpt::${dep}) + find_package(mrpt-${dep} REQUIRED) + endif() +endforeach() + +# Define the executable target: +add_executable(${sampleName} test.cpp ) + +if(TARGET examples) + add_dependencies(examples ${sampleName}) +endif() + +set_target_properties( + ${sampleName} + PROPERTIES + PROJECT_LABEL "(EXAMPLE) ${sampleName}") + +# Add special defines needed by this example, if any: +set(MY_DEFS ) +if(MY_DEFS) # If not empty + add_definitions("-D${MY_DEFS}") +endif() + +# Add the required libraries for linking: +foreach(dep maps;gui) + target_link_libraries(${sampleName} mrpt::${dep}) +endforeach() + +# Set optimized building: +if((${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang" OR CMAKE_COMPILER_IS_GNUCXX) AND NOT CMAKE_BUILD_TYPE MATCHES "Debug") + add_compile_options(-O3) +endif() + +# This part can be removed if you are compiling this program outside of +# the MRPT tree: +if(DEFINED MRPT_LIBS_ROOT) # Fails if build outside of MRPT project. + DeclareAppDependencies(${sampleName} mrpt::maps;mrpt::gui) # Dependencies +endif() diff --git a/samples/maps_voxelmap_from_tum_dataset/test.cpp b/samples/maps_voxelmap_from_tum_dataset/test.cpp new file mode 100644 index 0000000000..128d55b244 --- /dev/null +++ b/samples/maps_voxelmap_from_tum_dataset/test.cpp @@ -0,0 +1,257 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +// ------------------------------------------------------ +// TestVoxelMapFromTUM +// ------------------------------------------------------ +void TestVoxelMapFromTUM( + const std::string& datasetRawlogFile, const std::string& groundTruthFile, + double VOXELMAP_RESOLUTION, double VOXELMAP_MAX_RANGE) +{ + // To find out external image files: + mrpt::io::setLazyLoadPathBase( + mrpt::obs::CRawlog::detectImagesDirectory(datasetRawlogFile)); + + std::cout << "Loading dataset: " << datasetRawlogFile << "..." << std::endl; + + mrpt::obs::CRawlog dataset; + dataset.loadFromRawLogFile(datasetRawlogFile); + + std::cout << "Done! " << dataset.size() << " entries." << std::endl; + + std::cout << "Loading GT path from: " << groundTruthFile << "..." + << std::endl; + + mrpt::math::CMatrixDouble gtData; + gtData.loadFromTextFile(groundTruthFile); + + std::cout << "Done! " << gtData.rows() << " rows." << std::endl; + + // # timestamp tx ty tz qx qy qz qw + mrpt::poses::CPose3DInterpolator gt; + for (int i = 0; i < gtData.rows(); i++) + { + gt.insert( + mrpt::Clock::fromDouble(gtData(i, 0)), + mrpt::poses::CPose3D::FromQuaternionAndTranslation( + mrpt::math::CQuaternionDouble( + gtData(i, 7), gtData(i, 4), gtData(i, 5), gtData(i, 6)), + mrpt::math::TPoint3D( + gtData(i, 1), gtData(i, 2), gtData(i, 3)))); + } + + // ---------------------- + // Voxel map + // ---------------------- + mrpt::maps::CVoxelMapRGB map(VOXELMAP_RESOLUTION); + + map.insertionOptions.max_range = VOXELMAP_MAX_RANGE; // [m] + map.insertionOptions.ray_trace_free_space = false; // only occupied + + // gui and demo app: + mrpt::gui::CDisplayWindow3D win("VoxelMap demo", 640, 480); + + auto glVoxels = mrpt::opengl::COctoMapVoxels::Create(); + + // *IMPORTANT*: Required to see RGB color in the opengl visualization: + glVoxels->setVisualizationMode( + mrpt::opengl::COctoMapVoxels::COLOR_FROM_RGB_DATA); + + // create GL visual objects: + auto glCamGroup = mrpt::opengl::CSetOfObjects::Create(); + glCamGroup->insert(mrpt::opengl::stock_objects::CornerXYZSimple(0.3)); + auto glObsPts = mrpt::opengl::CPointCloudColoured::Create(); + glCamGroup->insert(glObsPts); + bool glCamFrustrumDone = false; + + mrpt::opengl::Viewport::Ptr glViewRGB; + + { + mrpt::opengl::Scene::Ptr& scene = win.get3DSceneAndLock(); + + // Set a large near plane so we can "see thru walls" easily when + // approaching a point: + scene->getViewport()->setViewportClipDistances(2.0, 200.0); + + { + auto gl_grid = + mrpt::opengl::CGridPlaneXY::Create(-20, 20, -20, 20, 0, 1); + gl_grid->setColor_u8(mrpt::img::TColor(0x80, 0x80, 0x80)); + scene->insert(gl_grid); + } + scene->insert(mrpt::opengl::stock_objects::CornerXYZSimple()); + + scene->insert(glCamGroup); + + // View occupied points: + { + auto mapPts = map.getOccupiedVoxels(); + mapPts->renderOptions.point_size = 5.0; + scene->insert(mapPts->getVisualization()); + } + + scene->insert(glVoxels); + + glViewRGB = scene->createViewport("rgb_view"); + glViewRGB->setViewportPosition(0, 0.7, 0.3, 0.25); + glViewRGB->setTransparent(true); + + win.unlockAccess3DScene(); + } + + std::cout << "Close the window to exit" << std::endl; + + size_t rawlogIndex = 0; + bool paused = false; + + mrpt::Clock::time_point lastObsTim; + + while (win.isOpen()) + { + win.get3DSceneAndLock(); + + // Get and process one observation: + if (rawlogIndex < dataset.size() && !paused) + { + mrpt::obs::CObservation3DRangeScan::Ptr obs; + + if (dataset.getType(rawlogIndex) == + mrpt::obs::CRawlog::etObservation && + (obs = std::dynamic_pointer_cast< + mrpt::obs::CObservation3DRangeScan>( + dataset.getAsObservation(rawlogIndex)))) + { + bool poseOk = false; + mrpt::poses::CPose3D camPose; + lastObsTim = obs->getTimeStamp(); + gt.interpolate(lastObsTim, camPose, poseOk); + + if (poseOk) + { + // set viz camera pose: + glCamGroup->setPose(camPose); + + using namespace mrpt::literals; + obs->sensorPose = mrpt::poses::CPose3D::FromYawPitchRoll( + 0.0_deg, -90.0_deg, 90.0_deg); + + // draw observation raw data: + mrpt::maps::CColouredPointsMap colPts; + + mrpt::obs::T3DPointsProjectionParams pp; + pp.takeIntoAccountSensorPoseOnRobot = true; + obs->unprojectInto(colPts, pp); + + glObsPts->loadFromPointsMap(&colPts); + + if (!glCamFrustrumDone) + { + glCamFrustrumDone = true; + auto glFrustrum = mrpt::opengl::CFrustum::Create( + obs->cameraParamsIntensity, + 1e-3 /*focalDistScale*/); + glFrustrum->setPose(obs->sensorPose); + glCamGroup->insert(glFrustrum); + } + + // update the voxel map: + map.insertObservation(*obs, camPose); + + // Update the voxel map visualization: + static int decimUpdateViz = 0; + if (decimUpdateViz++ > 20) + { + decimUpdateViz = 0; + map.renderingOptions.generateFreeVoxels = false; + map.getAsOctoMapVoxels(*glVoxels); + } + + // RGB view: + if (obs->hasIntensityImage) + { + glViewRGB->setImageView(obs->intensityImage); + } + } + } + rawlogIndex++; + } + + win.unlockAccess3DScene(); + + if (win.keyHit()) + { + const unsigned int k = win.getPushedKey(); + + switch (k) + { + case ' ': paused = !paused; break; + }; + } + + win.addTextMessage( + 5, 5, + mrpt::format( + "Timestamp: %s RawlogIndex: %zu ActiveVoxelCells: %zu", + mrpt::system::dateTimeLocalToString(lastObsTim).c_str(), + rawlogIndex, map.grid().activeCellsCount()), + 1 /*id*/); + + win.repaint(); + + using namespace std::chrono_literals; + std::this_thread::sleep_for(10ms); + }; +} + +int main(int argc, char** argv) +{ + try + { + if (argc != 3 && argc != 4) + throw std::invalid_argument( + "Usage: PROGRAM " + " []"); + + const std::string gtFile = mrpt::system::pathJoin( + {mrpt::system::extractFileDirectory(argv[1]), "groundtruth.txt"}); + + double VOXELMAP_MAX_RANGE = 5.0; + if (argc == 4) { VOXELMAP_MAX_RANGE = std::stod(argv[3]); } + + TestVoxelMapFromTUM( + argv[1], gtFile, std::stod(argv[2]), VOXELMAP_MAX_RANGE); + + return 0; + } + catch (const std::exception& e) + { + std::cout << "Exception: " << e.what() << std::endl; + return -1; + } +} diff --git a/samples/maps_voxelmap_simple/CMakeLists.txt b/samples/maps_voxelmap_simple/CMakeLists.txt new file mode 100644 index 0000000000..a0590736cf --- /dev/null +++ b/samples/maps_voxelmap_simple/CMakeLists.txt @@ -0,0 +1,62 @@ +#----------------------------------------------------------------------------------------------- +# CMake file for the MRPT example: /maps_voxelmap_simple +# +# Run with "ccmake ." at the root directory, or use it as a template for +# starting your own programs +#----------------------------------------------------------------------------------------------- +set(sampleName maps_voxelmap_simple) +project(EXAMPLE_${sampleName}) + +cmake_minimum_required(VERSION 3.1) + +# --------------------------------------------------------------------------- +# Set the output directory of each example to its corresponding subdirectory +# in the binary tree: +# --------------------------------------------------------------------------- +set(EXECUTABLE_OUTPUT_PATH ".") + +# The list of "libs" which can be included can be found in: +# https://www.mrpt.org/Libraries +# Add the top-level dependencies only. +# -------------------------------------------------------------------------- +foreach(dep maps;gui) + # if not building from inside MRPT source tree, find it as a cmake + # imported project: + if (NOT TARGET mrpt::${dep}) + find_package(mrpt-${dep} REQUIRED) + endif() +endforeach() + +# Define the executable target: +add_executable(${sampleName} test.cpp ) + +if(TARGET examples) + add_dependencies(examples ${sampleName}) +endif() + +set_target_properties( + ${sampleName} + PROPERTIES + PROJECT_LABEL "(EXAMPLE) ${sampleName}") + +# Add special defines needed by this example, if any: +set(MY_DEFS ) +if(MY_DEFS) # If not empty + add_definitions("-D${MY_DEFS}") +endif() + +# Add the required libraries for linking: +foreach(dep maps;gui) + target_link_libraries(${sampleName} mrpt::${dep}) +endforeach() + +# Set optimized building: +if((${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang" OR CMAKE_COMPILER_IS_GNUCXX) AND NOT CMAKE_BUILD_TYPE MATCHES "Debug") + add_compile_options(-O3) +endif() + +# This part can be removed if you are compiling this program outside of +# the MRPT tree: +if(DEFINED MRPT_LIBS_ROOT) # Fails if build outside of MRPT project. + DeclareAppDependencies(${sampleName} mrpt::maps;mrpt::gui) # Dependencies +endif() diff --git a/samples/maps_voxelmap_simple/test.cpp b/samples/maps_voxelmap_simple/test.cpp new file mode 100644 index 0000000000..8cfc9819f3 --- /dev/null +++ b/samples/maps_voxelmap_simple/test.cpp @@ -0,0 +1,175 @@ +/* +------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | https://www.mrpt.org/ | + | | + | Copyright (c) 2005-2023, Individual contributors, see AUTHORS file | + | See: https://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See: https://www.mrpt.org/License | + +------------------------------------------------------------------------+ */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +// ------------------------------------------------------ +// TestVoxelMap +// ------------------------------------------------------ +void TestVoxelMap() +{ + mrpt::maps::CVoxelMap map(0.1); + + // map.insertionOptions.max_range = 5.0; // [m] + + // Manually update voxels: + if (false) + { + map.updateVoxel(1, 1, 1, true); // integrate 'occupied' measurement + + map.updateVoxel(1.5, 1, 1, true); // integrate 'occupied' measurement + map.updateVoxel(1.5, 1, 1, true); // integrate 'occupied' measurement + map.updateVoxel(1.5, 1, 1, true); // integrate 'occupied' measurement + + map.updateVoxel(-1, -1, 1, false); // integrate 'free' measurement + + double occup; + bool is_mapped; + mrpt::math::TPoint3D pt; + + pt = mrpt::math::TPoint3D(1, 1, 1); + is_mapped = map.getPointOccupancy(pt.x, pt.y, pt.z, occup); + std::cout << "pt: " << pt + << " is mapped?: " << (is_mapped ? "YES" : "NO") + << " occupancy: " << occup << std::endl; + + pt = mrpt::math::TPoint3D(-1, -1, 1); + is_mapped = map.getPointOccupancy(pt.x, pt.y, pt.z, occup); + std::cout << "pt: " << pt + << " is mapped?: " << (is_mapped ? "YES" : "NO") + << " occupancy: " << occup << std::endl; + } + + // Insert 2D scan: + if (true) + { + mrpt::obs::CObservation2DRangeScan scan1; + mrpt::obs::stock_observations::example2DRangeScan(scan1); + map.insertObservation(scan1); + } + + mrpt::gui::CDisplayWindow3D win("VoxelMap demo", 640, 480); + + auto gl_map = mrpt::opengl::COctoMapVoxels::Create(); + + { + mrpt::opengl::Scene::Ptr& scene = win.get3DSceneAndLock(); + + { + auto gl_grid = + mrpt::opengl::CGridPlaneXY::Create(-20, 20, -20, 20, 0, 1); + gl_grid->setColor_u8(mrpt::img::TColor(0x80, 0x80, 0x80)); + scene->insert(gl_grid); + } + scene->insert(mrpt::opengl::stock_objects::CornerXYZSimple()); + + map.getAsOctoMapVoxels(*gl_map); + + // View occupied points: + { + auto mapPts = map.getOccupiedVoxels(); + mapPts->renderOptions.point_size = 5.0; + scene->insert(mapPts->getVisualization()); + } + + gl_map->showGridLines(false); + gl_map->showVoxels(mrpt::opengl::VOXEL_SET_OCCUPIED, true); + gl_map->showVoxels(mrpt::opengl::VOXEL_SET_FREESPACE, true); + scene->insert(gl_map); + + win.unlockAccess3DScene(); + } + + std::cout << "Close the window to exit" << std::endl; + + bool update_msg = true; + + while (win.isOpen()) + { + if (win.keyHit()) + { + const unsigned int k = win.getPushedKey(); + + switch (k) + { + case 'g': + case 'G': + gl_map->showGridLines(!gl_map->areGridLinesVisible()); + break; + case 'f': + case 'F': + gl_map->showVoxels( + mrpt::opengl::VOXEL_SET_FREESPACE, + !gl_map->areVoxelsVisible( + mrpt::opengl::VOXEL_SET_FREESPACE)); + break; + case 'o': + case 'O': + gl_map->showVoxels( + mrpt::opengl::VOXEL_SET_OCCUPIED, + !gl_map->areVoxelsVisible( + mrpt::opengl::VOXEL_SET_OCCUPIED)); + break; + case 'l': + case 'L': + gl_map->enableLights(!gl_map->areLightsEnabled()); + break; + }; + update_msg = true; + } + + if (update_msg) + { + update_msg = false; + + win.addTextMessage( + 5, 5, + mrpt::format( + "Commands: 'f' (freespace=%s) | 'o' (occupied=%s) | 'l' " + "(lights=%s)", + gl_map->areVoxelsVisible(mrpt::opengl::VOXEL_SET_FREESPACE) + ? "YES" + : "NO", + gl_map->areVoxelsVisible(mrpt::opengl::VOXEL_SET_OCCUPIED) + ? "YES" + : "NO", + gl_map->areLightsEnabled() ? "YES" : "NO")); + + win.repaint(); + } + + using namespace std::chrono_literals; + std::this_thread::sleep_for(10ms); + }; +} + +int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) +{ + try + { + TestVoxelMap(); + return 0; + } + catch (const std::exception& e) + { + std::cout << "Exception: " << e.what() << std::endl; + return -1; + } +} diff --git a/share/mrpt/config_files/icp-slam/icp-slam_demo_voxelmap.ini b/share/mrpt/config_files/icp-slam/icp-slam_demo_voxelmap.ini new file mode 100644 index 0000000000..ceb318e8b1 --- /dev/null +++ b/share/mrpt/config_files/icp-slam/icp-slam_demo_voxelmap.ini @@ -0,0 +1,99 @@ +#------------------------------------------------------------ +# Config file for the "ICP-SLAM" application +# See: https://www.mrpt.org/list-of-mrpt-apps/application-icp-slam/ +#------------------------------------------------------------ + +#======================================================= +# Section: [ICP] +# Parameters of ICP inside the ICP-based SLAM class +#======================================================= +[ICP] +maxIterations = 80 // The maximum number of iterations to execute if convergence is not achieved before +minAbsStep_trans = 1e-6 // If the correction in all translation coordinates (X,Y,Z) is below this threshold (in meters), iterations are terminated: +minAbsStep_rot = 1e-6 // If the correction in all rotation coordinates (yaw,pitch,roll) is below this threshold (in radians), iterations are terminated: + +thresholdDist = 0.3 // Initial maximum distance for matching a pair of points +thresholdAng_DEG = 5 // An angular factor (in degrees) to increase the matching distance for distant points. + +ALFA = 0.8 // After convergence, the thresholds are multiplied by this constant and ICP keep running (provides finer matching) + +smallestThresholdDist=0.05 // This is the smallest the distance threshold can become after stopping ICP and accepting the result. +onlyClosestCorrespondences=true // 1: Use the closest points only, 0: Use all the correspondences within the threshold (more robust sometimes, but slower) +onlyUniqueRobust = true // Force unique correspondences in both directions when pairing two point clouds + +# 0: icpClassic +# 1: icpLevenbergMarquardt +ICP_algorithm = icpClassic + +# decimation to apply to the point cloud being registered against the map +# Reduce to "1" to obtain the best accuracy +corresponding_points_decimation = 5 + +#======================================================= +# Section: [MappingApplication] +# Use: Here comes global parameters for the app. +#======================================================= +[MappingApplication] +# The source file (RAW-LOG) with action/observation pairs +rawlog_file="../../datasets/2006-01ENE-21-SENA_Telecom Faculty_one_loop_only.rawlog" +rawlog_offset=0 + +# The directory where the log files will be saved (left in blank if no log is required) +logOutput_dir=LOG_ICP-SLAM +LOG_FREQUENCY=50 // The frequency of log files generation: +SAVE_3D_SCENE=1 +SAVE_POSE_LOG=0 +CAMERA_3DSCENE_FOLLOWS_ROBOT=1 +SHOW_PROGRESS_3D_REAL_TIME=1 + +SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS=5 +SHOW_LASER_SCANS_3D = true + +localizationLinDistance = 0.10 // The distance threshold for correcting odometry with ICP (meters) +localizationAngDistance = 5 // The distance threshold for correcting odometry with ICP (degrees) + +insertionLinDistance = 1.0 // The distance threshold for inserting observations in the map (meters) +insertionAngDistance = 15.0 // The distance threshold for inserting observations in the map (degrees) + +minICPgoodnessToAccept = 0.20 // Minimum ICP quality to accept correction [0,1]. + +matchAgainstTheGrid = false + +# ======================================================== +# MULTIMETRIC MAP CONFIGURATION +# See docs for (Google for) mrpt::maps::CMultiMetricMap +# ======================================================== +# Creation of maps: +voxelMap_count = 1 +pointsMap_count = 1 + +# ==================================================== +# MULTIMETRIC MAP: PointsMap #00 +# ==================================================== +# Creation Options for PointsMap 00: +[MappingApplication_pointsMap_00_insertOpts] +minDistBetweenLaserPoints = 0.04 +fuseWithExisting = false +isPlanarMap = true + +# ==================================================== +# MULTIMETRIC MAP: VoxelMap #00 +# ==================================================== +# Creation Options for VoxelMap #00: +[MappingApplication_voxelMap_00_creationOpts] +resolution = 0.05 // (meters) + +# Insertion Options for VoxelMap #00: +[MappingApplication_voxelMap_00_insertOpts] +max_range = -1 // (Default=-1, no limits) +prob_hit = 0.7 // sets the probablility for a "hit" (will be converted to logodds) - sensor model +prob_miss = 0.4 // sets the probablility for a "miss" (will be converted to logodds) - sensor model +clamp_min = 0.05 // sets the minimum threshold for occupancy clamping (sensor model) +clamp_max = 0.95 // sets the maximum threshold for occupancy clamping (sensor model) +ray_trace_free_space = false +decimation = 1 + +# Likelihood Options for VoxelMap #00: +[MappingApplication_voxelMap_00_likelihoodOpts] +# None +