COctoMapBase.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

#ifndef MRPT_COctoMapBase_H
#define MRPT_COctoMapBase_H

#include <mrpt/maps/CMetricMap.h>
#include <mrpt/utils/CLoadableOptions.h>
#include <mrpt/utils/safe_pointers.h>
#include <octomap/octomap.h>
#include <mrpt/opengl/COctoMapVoxels.h>
#include <mrpt/opengl/COpenGLScene.h>
#include <mrpt/obs/obs_frwds.h>

namespace mrpt
{
namespace maps
{
/** A three-dimensional probabilistic occupancy grid, implemented as an
 * octo-tree with the "octomap" C++ library.
 *  This base class represents a 3D map where each voxel may contain an
 * "occupancy" property, RGBD data, etc. depending on the derived class.
 *
 * As with any other mrpt::maps::CMetricMap, you can obtain a 3D representation
 * of the map calling getAs3DObject() or getAsOctoMapVoxels()
 *
 * To use octomap's iterators to go through the voxels, use
 * COctoMap::getOctomap()
 *
 * The octomap library was presented in:
 *  - K. M. Wurm, A. Hornung, M. Bennewitz, C. Stachniss, and W. Burgard,
 *     <i>"OctoMap: A Probabilistic, Flexible, and Compact 3D Map Representation
 * for Robotic Systems"</i>
 *     in Proc. of the ICRA 2010 Workshop on Best Practice in 3D Perception and
 * Modeling for Mobile Manipulation, 2010. Software available at
 * http://octomap.sf.net/.
 *
 * \sa CMetricMap, the example in "MRPT/samples/octomap_simple"
 * \ingroup mrpt_maps_grp
 */
template <class OCTREE, class OCTREE_NODE>
class COctoMapBase : public mrpt::maps::CMetricMap
{
   public:
        /** The type of this MRPT class */
        typedef COctoMapBase<OCTREE, OCTREE_NODE> myself_t;
        /** The type of the octree class in the "octomap" library. */
        typedef OCTREE octree_t;
        /** The type of nodes in the octree, in the "octomap" library. */
        typedef OCTREE_NODE octree_node_t;

        /** Constructor, defines the resolution of the octomap (length of each voxel
         * side) */
        COctoMapBase(const double resolution = 0.10)
                : insertionOptions(*this), m_octomap(resolution)
        {
        }
        virtual ~COctoMapBase() {}
        /** Get a reference to the internal octomap object. Example:
           * \code
           *  mrpt::maps::COctoMap  map;
           *  ...
           *  octomap::OcTree &om = map.getOctomap();
           *
           *
           * \endcode
           */
        inline OCTREE& getOctomap() { return m_octomap; }
        /** With this struct options are provided to the observation insertion
        * process.
        * \sa CObservation::insertObservationInto()
        */
        struct TInsertionOptions : public utils::CLoadableOptions
        {
                /** Initilization of default parameters */
                TInsertionOptions(myself_t& parent);

                /** Especial constructor, not attached to a real COctoMap object: used
                 * only in limited situations, since get*() methods don't work, etc. */
                TInsertionOptions();
                TInsertionOptions& operator=(const TInsertionOptions& o)
                {
                        // Copy all but the m_parent pointer!
                        maxrange = o.maxrange;
                        pruning = o.pruning;
                        const bool o_has_parent = o.m_parent.get() != nullptr;
                        setOccupancyThres(
                                o_has_parent ? o.getOccupancyThres() : o.occupancyThres);
                        setProbHit(o_has_parent ? o.getProbHit() : o.probHit);
                        setProbMiss(o_has_parent ? o.getProbMiss() : o.probMiss);
                        setClampingThresMin(
                                o_has_parent ? o.getClampingThresMin() : o.clampingThresMin);
                        setClampingThresMax(
                                o_has_parent ? o.getClampingThresMax() : o.clampingThresMax);
                        return *this;
                }

                void loadFromConfigFile(
                        const mrpt::utils::CConfigFileBase& source,
                        const std::string& section) override;  // See base docs
                void dumpToTextStream(
                        mrpt::utils::CStream& out) const override;  // See base docs

                /** maximum range for how long individual beams are inserted (default
                 * -1: complete beam) */
                double maxrange;
                /** whether the tree is (losslessly) pruned after insertion (default:
                 * true) */
                bool pruning;

                /// (key name in .ini files: "occupancyThres") sets the threshold for
                /// occupancy (sensor model) (Default=0.5)
                void setOccupancyThres(double prob)
                {
                        if (m_parent.get()) m_parent->m_octomap.setOccupancyThres(prob);
                }
                /// (key name in .ini files: "probHit")sets the probablility for a "hit"
                /// (will be converted to logodds) - sensor model (Default=0.7)
                void setProbHit(double prob)
                {
                        if (m_parent.get()) m_parent->m_octomap.setProbHit(prob);
                }
                /// (key name in .ini files: "probMiss")sets the probablility for a
                /// "miss" (will be converted to logodds) - sensor model (Default=0.4)
                void setProbMiss(double prob)
                {
                        if (m_parent.get()) m_parent->m_octomap.setProbMiss(prob);
                }
                /// (key name in .ini files: "clampingThresMin")sets the minimum
                /// threshold for occupancy clamping (sensor model) (Default=0.1192, -2
                /// in log odds)
                void setClampingThresMin(double thresProb)
                {
                        if (m_parent.get())
                                m_parent->m_octomap.setClampingThresMin(thresProb);
                }
                /// (key name in .ini files: "clampingThresMax")sets the maximum
                /// threshold for occupancy clamping (sensor model) (Default=0.971, 3.5
                /// in log odds)
                void setClampingThresMax(double thresProb)
                {
                        if (m_parent.get())
                                m_parent->m_octomap.setClampingThresMax(thresProb);
                }

                /// @return threshold (probability) for occupancy - sensor model
                double getOccupancyThres() const
                {
                        if (m_parent.get())
                                return m_parent->m_octomap.getOccupancyThres();
                        else
                                return this->occupancyThres;
                }
                /// @return threshold (logodds) for occupancy - sensor model
                float getOccupancyThresLog() const
                {
                        return m_parent->m_octomap.getOccupancyThresLog();
                }

                /// @return probablility for a "hit" in the sensor model (probability)
                double getProbHit() const
                {
                        if (m_parent.get())
                                return m_parent->m_octomap.getProbHit();
                        else
                                return this->probHit;
                }
                /// @return probablility for a "hit" in the sensor model (logodds)
                float getProbHitLog() const
                {
                        return m_parent->m_octomap.getProbHitLog();
                }
                /// @return probablility for a "miss"  in the sensor model (probability)
                double getProbMiss() const
                {
                        if (m_parent.get())
                                return m_parent->m_octomap.getProbMiss();
                        else
                                return this->probMiss;
                }
                /// @return probablility for a "miss"  in the sensor model (logodds)
                float getProbMissLog() const
                {
                        return m_parent->m_octomap.getProbMissLog();
                }

                /// @return minimum threshold for occupancy clamping in the sensor model
                /// (probability)
                double getClampingThresMin() const
                {
                        if (m_parent.get())
                                return m_parent->m_octomap.getClampingThresMin();
                        else
                                return this->clampingThresMin;
                }
                /// @return minimum threshold for occupancy clamping in the sensor model
                /// (logodds)
                float getClampingThresMinLog() const
                {
                        return m_parent->m_octomap.getClampingThresMinLog();
                }
                /// @return maximum threshold for occupancy clamping in the sensor model
                /// (probability)
                double getClampingThresMax() const
                {
                        if (m_parent.get())
                                return m_parent->m_octomap.getClampingThresMax();
                        else
                                return this->clampingThresMax;
                }
                /// @return maximum threshold for occupancy clamping in the sensor model
                /// (logodds)
                float getClampingThresMaxLog() const
                {
                        return m_parent->m_octomap.getClampingThresMaxLog();
                }

           private:
                mrpt::utils::ignored_copy_ptr<myself_t> m_parent;

                double occupancyThres;  // sets the threshold for occupancy (sensor
                // model) (Default=0.5)
                double probHit;  // sets the probablility for a "hit" (will be converted
                // to logodds) - sensor model (Default=0.7)
                double probMiss;  // sets the probablility for a "miss" (will be
                // converted to logodds) - sensor model (Default=0.4)
                double clampingThresMin;  // sets the minimum threshold for occupancy
                // clamping (sensor model) (Default=0.1192, -2
                // in log odds)
                double clampingThresMax;  // sets the maximum threshold for occupancy
                // clamping (sensor model) (Default=0.971, 3.5
                // in log odds)
        };

        /** The options used when inserting observations in the map */
        TInsertionOptions insertionOptions;

        /** Options used when evaluating "computeObservationLikelihood"
        * \sa CObservation::computeObservationLikelihood
        */
        struct TLikelihoodOptions : public utils::CLoadableOptions
        {
                /** Initilization of default parameters
                        */
                TLikelihoodOptions();
                virtual ~TLikelihoodOptions() {}
                void loadFromConfigFile(
                        const mrpt::utils::CConfigFileBase& source,
                        const std::string& section) override;  // See base docs
                void dumpToTextStream(
                        mrpt::utils::CStream& out) const override;  // See base docs

                /** Binary dump to stream */
                void writeToStream(mrpt::utils::CStream& out) const;
                /** Binary dump to stream */
                void readFromStream(mrpt::utils::CStream& in);

                /** Speed up the likelihood computation by considering only one out of N
                 * rays (default=1) */
                uint32_t decimation;
        };

        TLikelihoodOptions likelihoodOptions;

        /** Returns true if the map is empty/no observation has been inserted */
        virtual bool isEmpty() const override;

        virtual void saveMetricMapRepresentationToFile(
                const std::string& filNamePrefix) const override;

        /** Options for the conversion of a mrpt::maps::COctoMap into a
         * mrpt::opengl::COctoMapVoxels */
        struct TRenderingOptions
        {
                /** Generate grid lines for all octree nodes, useful to draw the
                 * "structure" of the octree, but computationally costly (Default:
                 * false) */
                bool generateGridLines;

                /** Generate voxels for the occupied volumes  (Default=true) */
                bool generateOccupiedVoxels;
                /** Set occupied voxels visible (requires generateOccupiedVoxels=true)
                 * (Default=true) */
                bool visibleOccupiedVoxels;

                /** Generate voxels for the freespace (Default=true) */
                bool generateFreeVoxels;
                /** Set free voxels visible (requires generateFreeVoxels=true)
                 * (Default=true) */
                bool visibleFreeVoxels;

                TRenderingOptions()
                        : generateGridLines(false),
                          generateOccupiedVoxels(true),
                          visibleOccupiedVoxels(true),
                          generateFreeVoxels(true),
                          visibleFreeVoxels(true)
                {
                }

                /** Binary dump to stream */
                void writeToStream(mrpt::utils::CStream& out) const;
                /** Binary dump to stream */
                void readFromStream(mrpt::utils::CStream& in);
        };

        TRenderingOptions renderingOptions;

        /** Returns a 3D object representing the map.
                * \sa renderingOptions
                */
        virtual void getAs3DObject(
                mrpt::opengl::CSetOfObjects::Ptr& outObj) const override
        {
                mrpt::opengl::COctoMapVoxels::Ptr gl_obj =
                        mrpt::make_aligned_shared<mrpt::opengl::COctoMapVoxels>();
                this->getAsOctoMapVoxels(*gl_obj);
                outObj->insert(gl_obj);
        }

        /** Builds a renderizable representation of the octomap as a
         * mrpt::opengl::COctoMapVoxels object.
                * \sa renderingOptions
                */
        virtual void getAsOctoMapVoxels(
                mrpt::opengl::COctoMapVoxels& gl_obj) const = 0;

        /** Check whether the given point lies within the volume covered by the
         * octomap (that is, whether it is "mapped") */
        bool isPointWithinOctoMap(const float x, const float y, const float z) const
        {
                octomap::OcTreeKey key;
                return m_octomap.coordToKeyChecked(octomap::point3d(x, y, z), key);
        }

        /** 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 float x, const float y, const float z,
                double& prob_occupancy) const;

        /** 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)
        {
                m_octomap.updateNode(x, y, z, occupied);
        }

        /** Update the octomap with a 2D or 3D scan, given directly as a point cloud
         * and the 3D location of the sensor (the origin of the rays) in this map's
         * frame of reference.
          * Insertion parameters can be found in \a insertionOptions.
          * \sa The generic observation insertion method
         * CMetricMap::insertObservation()
          */
        void insertPointCloud(
                const CPointsMap& ptMap, const float sensor_x, const float sensor_y,
                const float sensor_z);

        /** Just like insertPointCloud but with a single ray. */
        void insertRay(
                const float end_x, const float end_y, const float end_z,
                const float sensor_x, const float sensor_y, const float sensor_z)
        {
                m_octomap.insertRay(
                        octomap::point3d(sensor_x, sensor_y, sensor_z),
                        octomap::point3d(end_x, end_y, end_z), insertionOptions.maxrange,
                        insertionOptions.pruning);
        }

        /** Performs raycasting in 3d, similar to computeRay().
         *
         * A ray is cast from origin with a given direction, the first occupied
         * cell is returned (as center coordinate). If the starting coordinate is
         * already
         * occupied in the tree, this coordinate will be returned as a hit.
         *
         * @param origin starting coordinate of ray
         * @param direction A vector pointing in the direction of the raycast. Does
         * not need to be normalized.
         * @param end returns the center of the cell that was hit by the ray, if
         * successful
         * @param ignoreUnknownCells whether unknown cells are ignored. If false
         * (default), the raycast aborts when an unkown cell is hit.
         * @param maxRange Maximum range after which the raycast is aborted (<= 0:
         * no limit, default)
         * @return whether or not an occupied cell was hit
         */
        bool castRay(
                const mrpt::math::TPoint3D& origin,
                const mrpt::math::TPoint3D& direction, mrpt::math::TPoint3D& end,
                bool ignoreUnknownCells = false, double maxRange = -1.0) const;

        /** @name Direct access to octomap library methods
                @{ */

        double getResolution() const { return m_octomap.getResolution(); }
        unsigned int getTreeDepth() const { return m_octomap.getTreeDepth(); }
        /// \return The number of nodes in the tree
        size_t size() const { return m_octomap.size(); }
        /// \return Memory usage of the complete octree in bytes (may vary between
        /// architectures)
        size_t memoryUsage() const { return m_octomap.memoryUsage(); }
        /// \return Memory usage of the a single octree node
        size_t memoryUsageNode() const { return m_octomap.memoryUsageNode(); }
        /// \return Memory usage of a full grid of the same size as the OcTree in
        /// bytes (for comparison)
        size_t memoryFullGrid() const { return m_octomap.memoryFullGrid(); }
        double volume() { return m_octomap.volume(); }
        /// Size of OcTree (all known space) in meters for x, y and z dimension
        void getMetricSize(double& x, double& y, double& z)
        {
                return m_octomap.getMetricSize(x, y, z);
        }
        /// Size of OcTree (all known space) in meters for x, y and z dimension
        void getMetricSize(double& x, double& y, double& z) const
        {
                return m_octomap.getMetricSize(x, y, z);
        }
        /// minimum value of the bounding box of all known space in x, y, z
        void getMetricMin(double& x, double& y, double& z)
        {
                return m_octomap.getMetricMin(x, y, z);
        }
        /// minimum value of the bounding box of all known space in x, y, z
        void getMetricMin(double& x, double& y, double& z) const
        {
                return m_octomap.getMetricMin(x, y, z);
        }
        /// maximum value of the bounding box of all known space in x, y, z
        void getMetricMax(double& x, double& y, double& z)
        {
                return m_octomap.getMetricMax(x, y, z);
        }
        /// maximum value of the bounding box of all known space in x, y, z
        void getMetricMax(double& x, double& y, double& z) const
        {
                return m_octomap.getMetricMax(x, y, z);
        }

        /// Traverses the tree to calculate the total number of nodes
        size_t calcNumNodes() const { return m_octomap.calcNumNodes(); }
        /// Traverses the tree to calculate the total number of leaf nodes
        size_t getNumLeafNodes() const { return m_octomap.getNumLeafNodes(); }
        /** @} */

   protected:
        virtual void internal_clear() override { m_octomap.clear(); }
        /**  Builds the list of 3D points in global coordinates for a generic
         * observation. Used for both, insertObservation() and computeLikelihood().
          * \param[out] point3d_sensorPt Is a pointer to a "point3D".
          * \param[out] ptr_scan Is in fact a pointer to "octomap::Pointcloud". Not
         * declared as such to avoid headers dependencies in user code.
          * \return false if the observation kind is not applicable.
          */
        bool internal_build_PointCloud_for_observation(
                const mrpt::obs::CObservation* obs,
                const mrpt::poses::CPose3D* robotPose,
                octomap::point3d& point3d_sensorPt,
                octomap::Pointcloud& ptr_scan) const;

        /** The actual octo-map object. */
        OCTREE m_octomap;

   private:
        // See docs in base class
        virtual double internal_computeObservationLikelihood(
                const mrpt::obs::CObservation* obs,
                const mrpt::poses::CPose3D& takenFrom) override;

};  // End of class def.
}  // End of namespace
}  // End of namespace

#include "COctoMapBase_impl.h"

#endif