diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index b8fec54d7d..7f7f3f751c 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -1,6 +1,11 @@ \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 - 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-maps_voxelmap_simple.md b/doc/source/doxygen-docs/example-maps_voxelmap_simple.md new file mode 100644 index 0000000000..7212dc74dc --- /dev/null +++ b/doc/source/doxygen-docs/example-maps_voxelmap_simple.md @@ -0,0 +1,5 @@ +\page maps_octomap_simple Example: maps_octomap_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..ed0bf29eec 100644 --- a/doc/source/examples.rst +++ b/doc/source/examples.rst @@ -78,6 +78,7 @@ 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_math_csparse_example.rst page_math_kmeans_example.rst page_math_leastsquares_example.rst 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/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 824bc6059e..ddb34a1f08 100644 --- a/libs/maps/include/mrpt/maps/COctoMap.h +++ b/libs/maps/include/mrpt/maps/COctoMap.h @@ -51,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/CVoxelMap.h b/libs/maps/include/mrpt/maps/CVoxelMap.h new file mode 100644 index 0000000000..213fe4033c --- /dev/null +++ b/libs/maps/include/mrpt/maps/CVoxelMap.h @@ -0,0 +1,249 @@ +/* +------------------------------------------------------------------------+ + | 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 + +namespace mrpt::maps +{ +/** + * Log-odds occupancy sparse voxel map. + * + * \ingroup mrpt_maps_grp + */ +class CVoxelMap : public CVoxelMapBase, + public detail::logoddscell_traits +{ + // This must be added to any CSerializable derived class: + DEFINE_SERIALIZABLE(CVoxelMap, mrpt::maps) + + protected: + using traits_t = detail::logoddscell_traits; + + public: + CVoxelMap( + double resolution = 0.05, uint8_t inner_bits = 2, uint8_t leaf_bits = 3) + : CVoxelMapBase(resolution, inner_bits, leaf_bits) + { + } + ~CVoxelMap(); + + bool isEmpty() const override; + 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); + + struct TInsertionOptions : public mrpt::config::CLoadableOptions + { + TInsertionOptions() = 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 = false; + + // See base docs + void loadFromConfigFile( + const mrpt::config::CConfigFileBase& source, + const std::string& section) override; + + void writeToStream(mrpt::serialization::CArchive& out) const; + void readFromStream(mrpt::serialization::CArchive& in); + }; + + /// The options used when inserting observations in the map: + TInsertionOptions insertionOptions; + + /** Options used when evaluating "computeObservationLikelihood" + * \sa CObservation::computeObservationLikelihood + */ + struct TLikelihoodOptions : public mrpt::config::CLoadableOptions + { + TLikelihoodOptions() = default; + ~TLikelihoodOptions() override = default; + + // See base docs + void loadFromConfigFile( + const mrpt::config::CConfigFileBase& source, + const std::string& section) 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; + }; + + TLikelihoodOptions likelihoodOptions; + + /** Options for the conversion of a mrpt::maps::COctoMap into a + * mrpt::opengl::COctoMapVoxels */ + struct TRenderingOptions + { + TRenderingOptions() = default; + + bool generateOccupiedVoxels = true; + bool visibleOccupiedVoxels = true; + + bool generateFreeVoxels = true; + bool visibleFreeVoxels = true; + + /** Binary dump to stream */ + void writeToStream(mrpt::serialization::CArchive& out) const; + /** Binary dump to stream */ + void readFromStream(mrpt::serialization::CArchive& in); + }; + + TRenderingOptions renderingOptions; + + MAP_DEFINITION_START(CVoxelMap) + double resolution = 0.10; + uint8_t inner_bits = 2; + uint8_t leaf_bits = 3; + mrpt::maps::CVoxelMap::TInsertionOptions insertionOpts; + mrpt::maps::CVoxelMap::TLikelihoodOptions likelihoodOpts; + MAP_DEFINITION_END(CVoxelMap) + + /** 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 voxel_node_t logodd_obs, + const voxel_node_t thres) + { + if (theCell == nullptr) return; + if (*theCell > thres) *theCell -= logodd_obs; + else + *theCell = 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 voxel_node_t logodd_obs, + const voxel_node_t thres) + { + if (voxel_node_t* cell = 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 voxel_node_t logodd_obs, + const voxel_node_t thres) + { + if (theCell == nullptr) return; + if (*theCell < thres) *theCell += logodd_obs; + else + *theCell = 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 voxel_node_t logodd_obs, + const voxel_node_t thres) + { + if (voxel_node_t* cell = 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(); + + /** 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 voxel_node_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 voxel_node_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 voxel_node_t p2l(const float p) + { + return get_logodd_lut().p2l(p); + } + + protected: + void internal_clear() override; + 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 \ No newline at end of file diff --git a/libs/maps/include/mrpt/maps/CVoxelMapBase.h b/libs/maps/include/mrpt/maps/CVoxelMapBase.h new file mode 100644 index 0000000000..bdb174ea22 --- /dev/null +++ b/libs/maps/include/mrpt/maps/CVoxelMapBase.h @@ -0,0 +1,128 @@ +/* +------------------------------------------------------------------------+ + | 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()) + { + } + Bonxai::VoxelGrid grid; + mutable typename Bonxai::VoxelGrid::Accessor accessor; + }; + std::unique_ptr m_impl; +}; + +} // namespace mrpt::maps \ No newline at end of file diff --git a/libs/maps/include/mrpt/maps/CVoxelMapRGB.h b/libs/maps/include/mrpt/maps/CVoxelMapRGB.h new file mode 100644 index 0000000000..2ee0e206ed --- /dev/null +++ b/libs/maps/include/mrpt/maps/CVoxelMapRGB.h @@ -0,0 +1,16 @@ +/* +------------------------------------------------------------------------+ + | 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 +{ +} \ No newline at end of file diff --git a/libs/maps/include/mrpt/maps/bonxai/bonxai.hpp b/libs/maps/include/mrpt/maps/bonxai/bonxai.hpp index b5c5970611..63a117e164 100644 --- a/libs/maps/include/mrpt/maps/bonxai/bonxai.hpp +++ b/libs/maps/include/mrpt/maps/bonxai/bonxai.hpp @@ -806,7 +806,7 @@ inline size_t VoxelGrid::activeCellsCount() const { const int32_t inner_index = *inner_it; auto& leaf_grid = inner_grid.cell(inner_index); - total_size += leaf_grid->mask.countOn(); + total_size += leaf_grid->mask().countOn(); } } return total_size; diff --git a/libs/maps/src/maps/CVoxelMap.cpp b/libs/maps/src/maps/CVoxelMap.cpp new file mode 100644 index 0000000000..0a758e10c1 --- /dev/null +++ b/libs/maps/src/maps/CVoxelMap.cpp @@ -0,0 +1,410 @@ +/* +------------------------------------------------------------------------+ + | 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 + +using namespace mrpt::maps; +using namespace std::string_literals; // "..."s + +// =========== Begin of Map definition ============ +MAP_DEFINITION_REGISTER("mrpt::maps::CVoxelMap", 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) + +// Static lookup tables for log-odds +static CLogOddsGridMapLUT logodd_lut; + +CLogOddsGridMapLUT& CVoxelMap::get_logodd_lut() +{ + return logodd_lut; +} + +/*--------------------------------------------------------------- + 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; + + THROW_EXCEPTION("TODO"); + // const_cast(&m_impl->m_octomap)->writeBinary(ss); +} + +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(); + + THROW_EXCEPTION("TODO"); + // m_impl->m_octomap.readBinary(ss); + } + 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); + return true; +} + +double CVoxelMap::internal_computeObservationLikelihood( + const mrpt::obs::CObservation& obs, + const mrpt::poses::CPose3D& takenFrom) const +{ + THROW_EXCEPTION("TODO"); + return 0; +} + +bool CVoxelMap::isEmpty() const +{ + THROW_EXCEPTION("TODO"); + return false; +} + +void CVoxelMap::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 = 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&>(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); + const auto pt = Bonxai::CoordToPos(coord, grid.resolution); + bbox.updateWithPoint({pt.x, pt.y, pt.z}); + + if ((occ >= 0.5 && renderingOptions.generateOccupiedVoxels) || + (occ < 0.5 && 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; + + default: THROW_EXCEPTION("Unknown coloring scheme!"); + } + + const size_t vx_set = + (occ > 0.5) ? 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); +} + +void CVoxelMap::TInsertionOptions::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); +} + +void CVoxelMap::TInsertionOptions::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; +} + +void CVoxelMap::TInsertionOptions::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; + break; + default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + } +} + +void CVoxelMap::TRenderingOptions::writeToStream( + mrpt::serialization::CArchive& out) const +{ + const uint8_t version = 0; + out << version; + + out << generateOccupiedVoxels << visibleOccupiedVoxels; + out << generateFreeVoxels << visibleFreeVoxels; +} + +void CVoxelMap::TRenderingOptions::readFromStream( + mrpt::serialization::CArchive& in) +{ + const uint8_t version = in.ReadAs(); + switch (version) + { + case 0: + in >> generateOccupiedVoxels >> visibleOccupiedVoxels; + in >> generateFreeVoxels >> visibleFreeVoxels; + break; + default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); + } +} + +void CVoxelMap::TLikelihoodOptions::loadFromConfigFile( + const mrpt::config::CConfigFileBase& c, const std::string& s) +{ + MRPT_LOAD_CONFIG_VAR(decimation, int, c, s); +} + +void CVoxelMap::TLikelihoodOptions::writeToStream( + mrpt::serialization::CArchive& out) const +{ + out << decimation; +} + +void CVoxelMap::TLikelihoodOptions::readFromStream( + mrpt::serialization::CArchive& in) +{ + in >> decimation; +} + +void CVoxelMap::internal_clear() +{ + // Is this enough? + m_impl->grid.root_map.clear(); +} + +void CVoxelMap::updateVoxel( + const double x, const double y, const double z, bool occupied) +{ + voxel_node_t* cell = m_impl->accessor.value( + Bonxai::PosToCoord({x, y, z}, m_impl->grid.inv_resolution), + true /*create*/); + if (!cell) return; // should never happen? + + if (occupied) + { + const voxel_node_t logodd_observation_occupied = + std::max(1, p2l(insertionOptions.prob_hit)); + const voxel_node_t logodd_thres_occupied = + p2l(1.0 - insertionOptions.clamp_max); + + updateCell_fast_occupied( + cell, logodd_observation_occupied, logodd_thres_occupied); + } + else + { + const voxel_node_t logodd_observation_free = + std::max(1, p2l(insertionOptions.prob_miss)); + const voxel_node_t logodd_thres_free = + p2l(1.0 - insertionOptions.clamp_min); + + updateCell_fast_free(cell, logodd_observation_free, logodd_thres_free); + } +} + +bool CVoxelMap::getPointOccupancy( + const double x, const double y, const double z, + double& prob_occupancy) const +{ + voxel_node_t* cell = m_impl->accessor.value( + Bonxai::PosToCoord({x, y, z}, m_impl->grid.inv_resolution), + false /*create*/); + + if (!cell) return false; + + prob_occupancy = 1.0 - l2p(*cell); + return true; +} + +void CVoxelMap::insertPointCloudAsRays( + const mrpt::maps::CPointsMap& pts, const mrpt::math::TPoint3D& sensorPt) +{ + THROW_EXCEPTION("TODO"); +} + +void CVoxelMap::insertPointCloudAsEndPoints(const mrpt::maps::CPointsMap& pts) +{ + const voxel_node_t logodd_observation_occupied = + std::max(1, p2l(insertionOptions.prob_hit)); + const voxel_node_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(); + + for (size_t i = 0; i < xs.size(); i++) + { + voxel_node_t* cell = m_impl->accessor.value( + Bonxai::PosToCoord( + {xs[i], ys[i], zs[i]}, m_impl->grid.inv_resolution), + true /*create*/); + if (!cell) continue; // should never happen? + + updateCell_fast_occupied( + cell, logodd_observation_occupied, logodd_thres_occupied); + } +} 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..bcace7a487 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/samples/CMakeLists.txt b/samples/CMakeLists.txt index 3ae80c14b2..680a1a4c15 100644 --- a/samples/CMakeLists.txt +++ b/samples/CMakeLists.txt @@ -287,6 +287,7 @@ if(MRPT_BUILD_EXAMPLES) maps_gridmap3D_simple maps_observer_pattern_example maps_octomap_simple + maps_voxelmap_simple maps_gmrf_map_example maps_ransac_data_association ) 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..f74c7aeca8 --- /dev/null +++ b/samples/maps_voxelmap_simple/test.cpp @@ -0,0 +1,166 @@ +/* +------------------------------------------------------------------------+ + | 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); + + // 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 (1) + { + 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); + + 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; + } +}