maps/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/opengl/COctoMapVoxels.h>
#include <mrpt/opengl/COpenGLScene.h>
#include <mrpt/obs/obs_frwds.h>
#include <mrpt/utils/safe_pointers.h>
#include <mrpt/utils/pimpl.h>

#include <mrpt/maps/link_pragmas.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_t,class octree_node_t>
                class COctoMapBase : public mrpt::maps::CMetricMap
                {
                public:
                        typedef COctoMapBase<octree_t, octree_node_t> myself_t;       //!< The type of this MRPT class

                        /** Constructor, defines the resolution of the octomap (length of each voxel side) */
                        COctoMapBase() : insertionOptions(*this) { }
                        virtual ~COctoMapBase() { }

                        /** Get a reference to the internal octomap object. Example:
                           * \code
                           *  mrpt::maps::COctoMap  map;
                           *  octomap::OcTree &om = map.getOctomap<octomap::OcTree>();
                           * \endcode
                           */
                        template <class OCTOMAP_CLASS>
                        inline OCTOMAP_CLASS & getOctomap() { return *m_octomap.ptr.get(); }

                        /** 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 );

                                TInsertionOptions(); //!< Especial constructor, not attached to a real COctoMap object: used only in limited situations, since get*() methods don't work, etc.
                                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()!=NULL;
                                        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) MRPT_OVERRIDE; // See base docs
                                void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs

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

                                /// (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->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->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->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->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->setClampingThresMax(thresProb); }

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

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

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

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

                        /** 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) MRPT_OVERRIDE; // See base docs
                                void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs

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

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

                        TLikelihoodOptions  likelihoodOptions;

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

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

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

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

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

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

                        TRenderingOptions renderingOptions;

                        /** Returns a 3D object representing the map.
                                * \sa renderingOptions
                                */
                        virtual void  getAs3DObject( mrpt::opengl::CSetOfObjectsPtr     &outObj ) const MRPT_OVERRIDE
                        {
                                mrpt::opengl::COctoMapVoxelsPtr gl_obj = mrpt::opengl::COctoMapVoxels::Create();
                                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;

                        /** 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;

                        /** 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);

                        /** 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;

                        virtual void setOccupancyThres(double prob) = 0;
                        virtual void setProbHit(double prob) = 0;
                        virtual void setProbMiss(double prob) = 0;
                        virtual void setClampingThresMin(double thresProb) = 0;
                        virtual void setClampingThresMax(double thresProb) = 0;
                        virtual double getOccupancyThres() const = 0;
                        virtual float getOccupancyThresLog() const = 0;
                        virtual double getProbHit() const = 0;
                        virtual float getProbHitLog() const = 0;
                        virtual double getProbMiss() const = 0;
                        virtual float getProbMissLog() const = 0;
                        virtual double getClampingThresMin() const = 0;
                        virtual float getClampingThresMinLog() const = 0;
                        virtual double getClampingThresMax() const = 0;
                        virtual float getClampingThresMaxLog() const = 0;

                protected:
                        /**  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.
                          */
                        template <class octomap_point3d, class octomap_pointcloud>
                        bool internal_build_PointCloud_for_observation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose, octomap_point3d &sensorPt, octomap_pointcloud &scan) const;

                        PIMPL_DECLARE_TYPE(octree_t, m_octomap); //!< The actual octo-map object.

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

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

#endif