maps/CMultiMetricMap.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2015, 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 CMultiMetricMap_H
#define CMultiMetricMap_H

#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/maps/COctoMap.h>
#include <mrpt/maps/CColouredOctoMap.h>
#include <mrpt/maps/CGasConcentrationGridMap2D.h>
#include <mrpt/maps/CWirelessPowerGridMap2D.h>
#include <mrpt/maps/CHeightGridMap2D.h>
#include <mrpt/maps/CReflectivityGridMap2D.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/maps/CColouredPointsMap.h>
#include <mrpt/maps/CWeightedPointsMap.h>
#include <mrpt/maps/CLandmarksMap.h>
#include <mrpt/maps/CBeaconMap.h>
#include <mrpt/maps/CMetricMap.h>
#include <mrpt/utils/CSerializable.h>
#include <mrpt/utils/CLoadableOptions.h>
#include <mrpt/utils/TEnumType.h>
#include <mrpt/obs/obs_frwds.h>

#include <mrpt/slam/link_pragmas.h>

namespace mrpt
{
namespace maps
{
        class TSetOfMetricMapInitializers;

        DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CMultiMetricMap , CMetricMap, SLAM_IMPEXP )

        /** This class stores any customizable set of metric maps.
         * The internal metric maps can be accessed directly by the user as smart pointers with CMultiMetricMap::getMapByIndex() or via `iterator`s.
         * The utility of this container is to operate on several maps simultaneously: update them by inserting observations, 
         * evaluate the likelihood of one observation by fusing (multiplying) the likelihoods over the different maps, etc.
         *
         *  <b>These kinds of metric maps can be kept inside</b> (list may be incomplete, refer to classes derived from mrpt::maps::CMetricMap):
         *              - mrpt::maps::CSimplePointsMap: For 2D or 3D range scans, ...
         *              - mrpt::maps::COccupancyGridMap2D: 2D, <b>horizontal</b>  laser range scans, at different altitudes.
         *              - mrpt::maps::COctoMap: For 3D occupancy grids of variable resolution, with octrees (based on the library `octomap`).
         *              - mrpt::maps::CColouredOctoMap: The same than above, but nodes can store RGB data appart from occupancy.
         *              - mrpt::maps::CLandmarksMap: For visual landmarks,etc...
         *              - mrpt::maps::CGasConcentrationGridMap2D: For gas concentration maps.
         *              - mrpt::maps::CWirelessPowerGridMap2D: For wifi power maps.
         *              - mrpt::maps::CBeaconMap: For range-only SLAM.
         *              - mrpt::maps::CHeightGridMap2D: For maps of height for each (x,y) location.
         *              - mrpt::maps::CReflectivityGridMap2D: For maps of "reflectivity" for each (x,y) location.
         *              - mrpt::maps::CColouredPointsMap: For point map with color.
         *              - mrpt::maps::CWeightedPointsMap: For point map with weights (capable of "fusing").
         *
         * See CMultiMetricMap::setListOfMaps() for the method for initializing this class programatically.
         * See also TSetOfMetricMapInitializers::loadFromConfigFile for a template of ".ini"-like configuration
         * file that can be used to define which maps to create and all their parameters.
         * Alternatively, the list of maps is public so it can be directly manipulated/accessed in CMultiMetricMap::maps
         *
         *  Configuring the list of maps: Alternatives
         * --------------------------------------------
         * 
         *  **Method #1: Using map definition structures**
         * \code
         * mrpt::maps::TSetOfMetricMapInitializers map_inits;
         * {
         *   mrpt::maps::COccupancyGridMap2D::TMapDefinition def;
         *   def.resolution = 0.05;
         *   def.insertionOpts.maxOccupancyUpdateCertainty = 0.8;
         *   def.insertionOpts.maxDistanceInsertion = 30;
         *   map_inits.push_back(def);
         * }
         * {
         *   mrpt::maps::CSimplePointsMap::TMapDefinition def;
         *   map_inits.push_back(def);
         * }
         * mrpt::maps::CMultiMetricMap theMap;
         * theMap.setListOfMaps(map_inits);
         * \endcode
         * 
         *  **Method #2: Using a configuration file**
         * See TSetOfMetricMapInitializers::loadFromConfigFile() for details on expected file format.
         *
         * \code
         * mrpt::utils::CConfigFile cfgFile("file.cfg");
         * mrpt::maps::TSetOfMetricMapInitializers map_inits;
         * map_inits.loadFromConfigFile(cfgFile, "MapDefinition");
         *
         * mrpt::maps::CMultiMetricMap theMap;
         * theMap.setListOfMaps(map_inits);
         * \endcode
         *
         *  **Method #3: Manual manipulation**
         *
         * \code
         * mrpt::maps::CMultiMetricMap theMap;
         * {
         *  mrpt::maps::CSimplePointsMapPtr ptMap = mrpt::maps::CSimplePointsMap::Create();
         *  theMap.maps.push_back(ptMap);
         * }
         * \endcode
         *
         * \note [New in MRPT 1.3.0]: `likelihoodMapSelection`, which selected the map to be used when 
         *  computing the likelihood of an observation, has been removed. Use the `enableObservationLikelihood` 
         *  property of each individual map declaration. 
         * 
         * \note [New in MRPT 1.3.0]: `enableInsertion_{pointsMap,...}` have been also removed. 
         *  Use the `enableObservationInsertion` property of each map declaration.
         *
         * \note [New in MRPT 1.3.0]: Plain list of maps is exposed in `maps` member. Proxies named `m_pointsMaps`,`m_gridMaps`, etc. 
         *  are provided for backwards-compatibility and for their utility.
         *
         * \note This class belongs to [mrpt-slam] instead of [mrpt-maps] due to the dependency on map classes in mrpt-vision.
         * \sa CMetricMap  \ingroup mrpt_slam_grp 
         */
        class SLAM_IMPEXP CMultiMetricMap : public mrpt::maps::CMetricMap
        {
                // This must be added to any CSerializable derived class:
                DEFINE_SERIALIZABLE( CMultiMetricMap )
        protected:
                void  deleteAllMaps(); //!< Deletes all maps and clears the internal lists of maps (with clear_unique(), so user copies remain alive)
                virtual void  internal_clear(); //!< Clear all elements of the map.
                // See base class docs
                virtual bool  internal_insertObservation( const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose = NULL ) MRPT_OVERRIDE;
                /** Returns true if any of the inner maps is able to compute a sensible likelihood function for this observation.
                 * \param obs The observation.
                 * \sa computeObservationLikelihood
                 */
                bool internal_canComputeObservationLikelihood( const mrpt::obs::CObservation *obs );
                // See docs in base class
                double   internal_computeObservationLikelihood( const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom );

        public:
                /** @name Access to internal list of maps: direct list, iterators, utility methods and proxies
                    @{ */
                typedef std::deque<mrpt::maps::CMetricMapPtr> TListMaps;

                /** The list of MRPT metric maps in this object. Use dynamic_cast or smart pointer-based downcast to access maps by their actual type.
                  * You can directly manipulate this list. Helper methods to initialize it are described in the docs of CMultiMetricMap 
                  */
                TListMaps maps;

                typedef TListMaps::iterator iterator;
                typedef TListMaps::const_iterator const_iterator;
                iterator begin() { return maps.begin(); }
                const_iterator begin() const { return maps.begin(); }
                iterator end() { return maps.end(); }
                const_iterator end() const { return maps.end(); }

                /** Gets the i-th map \exception std::runtime_error On out-of-bounds */
                mrpt::maps::CMetricMapPtr getMapByIndex(size_t idx) const;

                /** Returns the i'th observation of a given class (or of a descendant class), or NULL if there is no such observation in the array.
                  *  Example:
                  * \code
                  *  CObservationImagePtr obs = m_SF->getObservationByClass<CObservationImage>();
                  * \endcode
                  * By default (ith=0), the first observation is returned.
                  */
                template <typename T>
                typename T::SmartPtr getMapByClass( const size_t &ith = 0 ) const
                {
                        size_t  foundCount = 0;
                        const mrpt::utils::TRuntimeClassId*     class_ID = T::classinfo;
                        for (const_iterator it = begin();it!=end();++it)
                                if ( (*it)->GetRuntimeClass()->derivedFrom( class_ID ) )
                                        if (foundCount++ == ith)
                                                return typename T::SmartPtr(*it);
                        return typename T::SmartPtr();  // Not found: return empty smart pointer
                }

                /** Takes a const ref of a STL non-associative container of smart pointers at construction and exposes an interface 
                  * mildly similar to that of another STL container containing only those elements 
                  * in the original container that can be `dynamic_cast`ed to `SELECTED_CLASS_PTR` */
                template <class SELECTED_CLASS_PTR, class CONTAINER>
                struct ProxyFilterContainerByClass
                {
                        typedef typename SELECTED_CLASS_PTR::value_type* ptr_t;
                        typedef const typename SELECTED_CLASS_PTR::value_type* const_ptr_t;
                        ProxyFilterContainerByClass(CONTAINER &source) : m_source(source) {}

                        bool empty() const { return size()==0; }
                        size_t size() const {
                                size_t cnt=0;
                                for(typename CONTAINER::const_iterator it=m_source.begin();it!=m_source.end();++it)
                                        if ( dynamic_cast<const_ptr_t>(it->pointer()) )
                                                cnt++;
                                return cnt;
                        }
                        SELECTED_CLASS_PTR operator [](size_t index) const {
                                size_t cnt=0;
                                for(typename CONTAINER::const_iterator it=m_source.begin();it!=m_source.end();++it)
                                        if ( dynamic_cast<const_ptr_t>(it->pointer()) ) 
                                                if (cnt++ == index) { return SELECTED_CLASS_PTR(*it); }
                                throw std::out_of_range("Index is out of range");
                        }
                private:
                        CONTAINER & m_source;
                }; // end ProxyFilterContainerByClass

                /** A proxy like ProxyFilterContainerByClass, but it directly appears as if 
                  * it was a single smart pointer (empty if no matching object is found in the container) */
                template <class SELECTED_CLASS_PTR, class CONTAINER>
                struct ProxySelectorContainerByClass
                {
                        typedef typename SELECTED_CLASS_PTR::value_type pointee_t;
                        typedef typename SELECTED_CLASS_PTR::value_type* ptr_t;
                        typedef const typename SELECTED_CLASS_PTR::value_type* const_ptr_t;
                        ProxySelectorContainerByClass(CONTAINER &source) : m_source(source) {}
                        operator const SELECTED_CLASS_PTR & () const { internal_update_ref(); return m_ret; }
                        operator bool() const { internal_update_ref(); return m_ret.present(); }
                        bool present() const { internal_update_ref(); return m_ret.present(); }
                        ptr_t pointer(){ internal_update_ref(); return m_ret.pointer(); }
                        ptr_t operator ->() const {
                                internal_update_ref();
                                if (m_ret.present()) return m_ret.pointer();
                                else throw std::runtime_error("Tried to derefer NULL pointer");
                        }
                        pointee_t & operator *() const {
                                internal_update_ref();
                                if (m_ret.present()) return *m_ret.pointer();
                                else throw std::runtime_error("Tried to derefer NULL pointer");
                        }
                private:
                        CONTAINER & m_source;
                        mutable SELECTED_CLASS_PTR m_ret;
                        void internal_update_ref() const {
                                for(typename CONTAINER::const_iterator it=m_source.begin();it!=m_source.end();++it) {
                                        if ( dynamic_cast<const_ptr_t>(it->pointer()) ) {
                                                m_ret=SELECTED_CLASS_PTR(*it);
                                                return;
                                        }
                                }
                                m_ret=SELECTED_CLASS_PTR(); // Not found
                        }

                }; // end ProxySelectorContainerByClass

                ProxyFilterContainerByClass<mrpt::maps::CSimplePointsMapPtr,TListMaps>           m_pointsMaps; //!< STL-like proxy to access this kind of maps in \ref maps
                ProxyFilterContainerByClass<mrpt::maps::COccupancyGridMap2DPtr,TListMaps>        m_gridMaps;   //!< STL-like proxy to access this kind of maps in \ref maps
                ProxyFilterContainerByClass<mrpt::maps::COctoMapPtr,TListMaps>                   m_octoMaps;   //!< STL-like proxy to access this kind of maps in \ref maps
                ProxyFilterContainerByClass<mrpt::maps::CColouredOctoMapPtr,TListMaps>           m_colourOctoMaps;   //!< STL-like proxy to access this kind of maps in \ref maps
                ProxyFilterContainerByClass<mrpt::maps::CGasConcentrationGridMap2DPtr,TListMaps> m_gasGridMaps;   //!< STL-like proxy to access this kind of maps in \ref maps
                ProxyFilterContainerByClass<mrpt::maps::CWirelessPowerGridMap2DPtr,TListMaps>    m_wifiGridMaps;   //!< STL-like proxy to access this kind of maps in \ref maps
                ProxyFilterContainerByClass<mrpt::maps::CHeightGridMap2DPtr,TListMaps>           m_heightMaps;   //!< STL-like proxy to access this kind of maps in \ref maps
                ProxyFilterContainerByClass<mrpt::maps::CReflectivityGridMap2DPtr,TListMaps>     m_reflectivityMaps;   //!< STL-like proxy to access this kind of maps in \ref maps
                ProxySelectorContainerByClass<mrpt::maps::CColouredPointsMapPtr,TListMaps>       m_colourPointsMap; //!< Proxy that looks like a smart pointer to the first matching object in \ref maps
                ProxySelectorContainerByClass<mrpt::maps::CWeightedPointsMapPtr,TListMaps>       m_weightedPointsMap; //!< Proxy that looks like a smart pointer to the first matching object in \ref maps
                ProxySelectorContainerByClass<mrpt::maps::CLandmarksMapPtr,TListMaps>            m_landmarksMap; //!< Proxy that looks like a smart pointer to the first matching object in \ref maps
                ProxySelectorContainerByClass<mrpt::maps::CBeaconMapPtr,TListMaps>               m_beaconMap; //!< Proxy that looks like a smart pointer to the first matching object in \ref maps

                /** @} */

                /** Constructor.
                 * \param initializers One internal map will be created for each entry in this "TSetOfMetricMapInitializers" struct.
                 *  If initializers is NULL, no internal map will be created.
                 */
                CMultiMetricMap(const mrpt::maps::TSetOfMetricMapInitializers   *initializers = NULL);
                CMultiMetricMap(const mrpt::maps::CMultiMetricMap &other );  //!< Copy constructor
                mrpt::maps::CMultiMetricMap &operator = ( const mrpt::maps::CMultiMetricMap &other ); //!< Copy operator from "other" object.
                virtual ~CMultiMetricMap( ); //!< Destructor.

                /** Sets the list of internal map according to the passed list of map initializers (Current maps' content will be deleted!) */
                void  setListOfMaps( const mrpt::maps::TSetOfMetricMapInitializers      *initializers );
                /** \overload */
                void  setListOfMaps( const mrpt::maps::TSetOfMetricMapInitializers      &initializers ) { this->setListOfMaps(&initializers); }

                bool  isEmpty() const MRPT_OVERRIDE; //!< Returns true if all maps returns true to their isEmpty() method, which is map-dependent. Read the docs of each map class

                // See docs in base class.
                virtual void  determineMatching2D(
                        const mrpt::maps::CMetricMap      * otherMap,
                        const mrpt::poses::CPose2D         & otherMapPose,
                        mrpt::utils::TMatchingPairList     & correspondences,
                        const mrpt::maps::TMatchingParams & params,
                        mrpt::maps::TMatchingExtraResults & extraResults ) const;

                /** See the definition in the base class: Calls in this class become a call to every single map in this set. */
                float  compute3DMatchingRatio(
                                const mrpt::maps::CMetricMap                                            *otherMap,
                                const mrpt::poses::CPose3D                                                      &otherMapPose,
                                float                                                                   maxDistForCorr = 0.10f,
                                float                                                                   maxMahaDistForCorr = 2.0f
                                ) const;

                /** The implementation in this class just calls all the corresponding method of the contained metric maps.
                  */
                void  saveMetricMapRepresentationToFile(
                        const std::string       &filNamePrefix
                        ) const;

                /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation".
                  *  This method should normally do nothing, but in some cases can be used to free auxiliary cached variables.
                  */
                void  auxParticleFilterCleanUp();

                /** Returns a 3D object representing the map.
                  */
                void  getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr    &outObj ) const;

                /** If the map is a simple point map or it's a multi-metric map that contains EXACTLY one simple point map, return it.
                        * Otherwise, return NULL
                        */
                virtual const mrpt::maps::CSimplePointsMap * getAsSimplePointsMap() const;
                virtual       mrpt::maps::CSimplePointsMap * getAsSimplePointsMap();

                /** An auxiliary variable that can be used freely by the users (this will be copied to other maps using the copy constructor, copy operator, streaming,etc) The default value is 0.
                  */
                unsigned int    m_ID;

        }; // End of class def.
        DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE( CMultiMetricMap , mrpt::maps::CMetricMap, SLAM_IMPEXP )


        } // End of namespace
} // End of namespace

#endif