diff --git a/libs/maps/include/mrpt/maps/CVoxelMap.h b/libs/maps/include/mrpt/maps/CVoxelMap.h index fdd5aa4697..8f434e542e 100644 --- a/libs/maps/include/mrpt/maps/CVoxelMap.h +++ b/libs/maps/include/mrpt/maps/CVoxelMap.h @@ -9,255 +9,49 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include +#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 occupancy sparse voxel map. + * Log-odds sparse voxel map for cells containing only the *occupancy* of each + * voxel. * * \ingroup mrpt_maps_grp */ -class CVoxelMap : public CVoxelMapBase, - public detail::logoddscell_traits +class CVoxelMap : public CVoxelMapOccupancyBase { // 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) + : CVoxelMapOccupancyBase(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); - - /** 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; - - 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 = 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); - }; - - /// 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 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; - }; - - TLikelihoodOptions likelihoodOptions; - - /** Options for the conversion of a mrpt::maps::COctoMap into a - * mrpt::opengl::COctoMapVoxels */ - struct TRenderingOptions - { - TRenderingOptions() = 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); - }; - - 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; + mrpt::maps::TVoxelMap_InsertionOptions insertionOpts; + mrpt::maps::TVoxelMap_LikelihoodOptions 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 = @@ -265,11 +59,6 @@ class CVoxelMap : public CVoxelMapBase, double internal_computeObservationLikelihood( const mrpt::obs::CObservation& obs, const mrpt::poses::CPose3D& takenFrom) const override; - - void invalidateOccupiedCache() { m_cachedOccupied.reset(); } - - void updateOccupiedPointsCache() const; - mutable mrpt::maps::CSimplePointsMap::Ptr m_cachedOccupied; }; } // 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..68767855fb --- /dev/null +++ b/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h @@ -0,0 +1,629 @@ +/* +------------------------------------------------------------------------+ + | 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 = data.color; + } + 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 index 2ee0e206ed..7aa8e021a2 100644 --- a/libs/maps/include/mrpt/maps/CVoxelMapRGB.h +++ b/libs/maps/include/mrpt/maps/CVoxelMapRGB.h @@ -9,8 +9,68 @@ #pragma once -#include +#include +#include namespace mrpt::maps { -} \ No newline at end of file +/** Voxel contents for CVoxelMapRGB + */ +struct VoxelNodeOccRGB +{ + int8_t occupancy = 0; + mrpt::img::TColor color; + + // ---- 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/src/maps/CVoxelMap.cpp b/libs/maps/src/maps/CVoxelMap.cpp index 1fed856f5d..aabf373149 100644 --- a/libs/maps/src/maps/CVoxelMap.cpp +++ b/libs/maps/src/maps/CVoxelMap.cpp @@ -56,14 +56,6 @@ mrpt::maps::CMetricMap* CVoxelMap::internal_CreateFromMapDefinition( 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 ---------------------------------------------------------------*/ @@ -132,7 +124,7 @@ bool CVoxelMap::internal_insertObservation( if (insertionOptions.ray_trace_free_space) insertPointCloudAsRays(pts, sensorPt); else - insertPointCloudAsEndPoints(pts); + insertPointCloudAsEndPoints(pts, sensorPt); return true; } @@ -143,415 +135,3 @@ double CVoxelMap::internal_computeObservationLikelihood( THROW_EXCEPTION("TODO"); return 0; } - -bool CVoxelMap::isEmpty() const { return m_impl->grid.activeCellsCount() == 0; } - -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 >= 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; - - 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); -} - -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); - MRPT_LOAD_CONFIG_VAR(decimation, uint64_t, c, s); -} -void CVoxelMap::TInsertionOptions::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 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 << decimation; -} - -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 >> decimation; - 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; - out << occupiedThreshold << freeThreshold; -} - -void CVoxelMap::TRenderingOptions::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 CVoxelMap::TLikelihoodOptions::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 CVoxelMap::TLikelihoodOptions::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 CVoxelMap::TLikelihoodOptions::writeToStream( - mrpt::serialization::CArchive& out) const -{ - out << decimation << occupiedThreshold; -} - -void CVoxelMap::TLikelihoodOptions::readFromStream( - mrpt::serialization::CArchive& in) -{ - in >> decimation >> occupiedThreshold; -} - -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) -{ - invalidateOccupiedCache(); - - 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) -{ - invalidateOccupiedCache(); - - 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(); - - 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}, m_impl->grid.inv_resolution); - - // Use fixed comma for the ray tracing direction: - constexpr unsigned int FRBITS = 9; - - 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); - - // 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]}, 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 = 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 = - m_impl->accessor.value(endCoord, true /*create*/); - cell) - { - updateCell_fast_occupied( - cell, logodd_observation_occupied, logodd_thres_occupied); - } - } // for each point/ray -} - -void CVoxelMap::insertPointCloudAsEndPoints(const mrpt::maps::CPointsMap& pts) -{ - invalidateOccupiedCache(); - - 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(); - - 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]).sqrNorm() > maxSqrDist) - continue; // skip - - 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); - } -} - -void CVoxelMap::updateOccupiedPointsCache() const -{ - if (m_cachedOccupied) return; // done - - m_cachedOccupied = mrpt::maps::CSimplePointsMap::Create(); - - // forEachCell() has no const version - auto& grid = const_cast&>(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); - 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); -} - -mrpt::maps::CSimplePointsMap::Ptr CVoxelMap::getOccupiedVoxels() const -{ - updateOccupiedPointsCache(); - return m_cachedOccupied; -} 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..2afd64e9ff --- /dev/null +++ b/libs/maps/src/maps/CVoxelMapRGB.cpp @@ -0,0 +1,223 @@ +/* +------------------------------------------------------------------------+ + | 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 + +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; + + THROW_EXCEPTION("TODO"); + // const_cast(&m_impl->m_octomap)->writeBinary(ss); +} + +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(); + + THROW_EXCEPTION("TODO"); + // m_impl->m_octomap.readBinary(ss); + } + 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 copy color: + mrpt::img::TColorf colF; + colPts.getPointColor(i, colF.R, colF.G, colF.B); + cell->color = colF.asTColor(); + } + + 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/registerAllClasses.cpp b/libs/maps/src/registerAllClasses.cpp index bcace7a487..44f9b776cf 100644 --- a/libs/maps/src/registerAllClasses.cpp +++ b/libs/maps/src/registerAllClasses.cpp @@ -47,7 +47,7 @@ MRPT_INITIALIZER(registerAllClasses_mrpt_maps) registerClass(CLASS_ID(CColouredOctoMap)); registerClass(CLASS_ID(CVoxelMap)); - // registerClass(CLASS_ID(CVoxelMapRGB)); + registerClass(CLASS_ID(CVoxelMapRGB)); registerClass(CLASS_ID(CAngularObservationMesh)); registerClass(CLASS_ID(CPlanarLaserScan)); 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/samples/maps_voxelmap_from_tum_dataset/test.cpp b/samples/maps_voxelmap_from_tum_dataset/test.cpp index 459e760d46..53158d793a 100644 --- a/samples/maps_voxelmap_from_tum_dataset/test.cpp +++ b/samples/maps_voxelmap_from_tum_dataset/test.cpp @@ -10,7 +10,7 @@ #include #include #include -#include +#include #include #include #include @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -31,7 +32,7 @@ // ------------------------------------------------------ void TestVoxelMapFromTUM( const std::string& datasetRawlogFile, const std::string& groundTruthFile, - double VOXELMAP_RESOLUTION) + double VOXELMAP_RESOLUTION, double VOXELMAP_MAX_RANGE) { // To find out external image files: mrpt::io::setLazyLoadPathBase( @@ -68,10 +69,9 @@ void TestVoxelMapFromTUM( // ---------------------- // Voxel map // ---------------------- + mrpt::maps::CVoxelMapRGB map(VOXELMAP_RESOLUTION); - mrpt::maps::CVoxelMap map(VOXELMAP_RESOLUTION); - - map.insertionOptions.max_range = 5.0; // [m] + map.insertionOptions.max_range = VOXELMAP_MAX_RANGE; // [m] map.insertionOptions.ray_trace_free_space = false; // only occupied // gui and demo app: @@ -79,15 +79,26 @@ void TestVoxelMapFromTUM( 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); @@ -107,6 +118,9 @@ void TestVoxelMapFromTUM( scene->insert(glVoxels); + glViewRGB = scene->createViewport("rgb_view"); + glViewRGB->setViewportPosition(0, 0.7, 0.4, 0.3); + win.unlockAccess3DScene(); } @@ -165,8 +179,7 @@ void TestVoxelMapFromTUM( } // update the voxel map: - colPts.changeCoordinatesReference(camPose); - map.insertPointCloudAsEndPoints(colPts); + map.insertObservation(*obs, camPose); // Update the voxel map visualization: static int decimUpdateViz = 0; @@ -176,6 +189,12 @@ void TestVoxelMapFromTUM( map.renderingOptions.generateFreeVoxels = false; map.getAsOctoMapVoxels(*glVoxels); } + + // RGB view: + if (obs->hasIntensityImage) + { + glViewRGB->setImageView(obs->intensityImage); + } } } rawlogIndex++; @@ -212,12 +231,20 @@ int main(int argc, char** argv) { try { - if (argc != 4) + 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); - TestVoxelMapFromTUM(argv[1], argv[2], std::stod(argv[3])); return 0; } catch (const std::exception& e)