CWeightedPointsMap.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                       http://www.mrpt.org/                                |
   |                                                                           |
   |   Copyright (C) 2005-2011  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Machine Perception and Intelligent    |
   |      Robotics Lab, University of Malaga (Spain).                          |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT is free software: you can redistribute it and/or modify          |
   |     it under the terms of the GNU General Public License as published by  |
   |     the Free Software Foundation, either version 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT is distributed in the hope that it will be useful,                 |
   |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
   |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
   |     GNU General Public License for more details.                          |
   |                                                                           |
   |     You should have received a copy of the GNU General Public License     |
   |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */
#ifndef CWeightedPointsMap_H
#define CWeightedPointsMap_H

#include <mrpt/slam/CPointsMap.h>
#include <mrpt/slam/CObservation2DRangeScan.h>
#include <mrpt/slam/CObservation3DRangeScan.h>
#include <mrpt/utils/CSerializable.h>
#include <mrpt/math/CMatrix.h>

#include <mrpt/maps/link_pragmas.h>

namespace mrpt
{
        namespace slam
        {

                DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CWeightedPointsMap , CPointsMap, MAPS_IMPEXP )

                /** A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
                 *    This class stores the coordinates (x,y,z) and a "weight", or counter of how many times that point has been seen, used only if points fusion is enabled in the options structure.
                 * \sa CMetricMap, CPoint, mrpt::utils::CSerializable, CSimplePointsMap
                 * \ingroup mrpt_maps_grp
                 */
                class MAPS_IMPEXP CWeightedPointsMap : public CPointsMap
                {
                        // This must be added to any CSerializable derived class:
                        DEFINE_SERIALIZABLE( CWeightedPointsMap )

                 public:
                         CWeightedPointsMap();          //!< Default constructor
                         virtual ~CWeightedPointsMap(); //!< Destructor

                        // --------------------------------------------
                        /** @name Pure virtual interfaces to be implemented by any class derived from CPointsMap
                                @{ */

                        /** Reserves memory for a given number of points: the size of the map does not change, it only reserves the memory.
                          *  This is useful for situations where it is approximately known the final size of the map. This method is more
                          *  efficient than constantly increasing the size of the buffers. Refer to the STL C++ library's "reserve" methods.
                          */
                        virtual void reserve(size_t newLength);

                        /** Resizes all point buffers so they can hold the given number of points: newly created points are set to default values,
                          *  and old contents are not changed.
                          * \sa reserve, setPoint, setPointFast, setSize
                          */
                        virtual void resize(size_t newLength);

                        /** Resizes all point buffers so they can hold the given number of points, *erasing* all previous contents
                          *  and leaving all points to default values.
                          * \sa reserve, setPoint, setPointFast, setSize
                          */
                        virtual void setSize(size_t newLength);

                        /** Changes the coordinates of the given point (0-based index), *without* checking for out-of-bounds and *without* calling mark_as_modified() \sa setPoint */
                        virtual void  setPointFast(size_t index,float x, float y, float z);

                        /** The virtual method for \a insertPoint() *without* calling mark_as_modified()   */
                        virtual void  insertPointFast( float x, float y, float z = 0 );

                         /** Virtual assignment operator, to be implemented in derived classes.
                           */
                         virtual void  copyFrom(const CPointsMap &obj);

                        /** Get all the data fields for one point as a vector: [X Y Z WEIGHT]
                          *  Unlike getPointAllFields(), this method does not check for index out of bounds
                          * \sa getPointAllFields, setPointAllFields, setPointAllFieldsFast
                          */
                        virtual void  getPointAllFieldsFast( const size_t index, std::vector<float> & point_data ) const {
                                point_data.resize(4);
                                point_data[0] = x[index];
                                point_data[1] = y[index];
                                point_data[2] = z[index];
                                point_data[3] = pointWeight[index];
                        }

                        /** Set all the data fields for one point as a vector: [X Y Z WEIGHT]
                          *  Unlike setPointAllFields(), this method does not check for index out of bounds
                          * \sa setPointAllFields, getPointAllFields, getPointAllFieldsFast
                          */
                        virtual void  setPointAllFieldsFast( const size_t index, const std::vector<float> & point_data ) {
                                ASSERTDEB_(point_data.size()==4)
                                x[index] = point_data[0];
                                y[index] = point_data[1];
                                z[index] = point_data[2];
                                pointWeight[index] = point_data[3];
                        }

                        /** See CPointsMap::loadFromRangeScan() */
                        virtual void  loadFromRangeScan(
                                        const CObservation2DRangeScan &rangeScan,
                                        const CPose3D                             *robotPose = NULL );

                        /** See CPointsMap::loadFromRangeScan() */
                        virtual void  loadFromRangeScan(
                                        const CObservation3DRangeScan &rangeScan,
                                        const CPose3D                             *robotPose = NULL );

                protected:

                        /** Auxiliary method called from within \a addFrom() automatically, to finish the copying of class-specific data  */
                        virtual void  addFrom_classSpecific(const CPointsMap &anotherMap, const size_t nPreviousPoints);

                        // Friend methods:
                        template <class Derived> friend struct detail::loadFromRangeImpl;
                        template <class Derived> friend struct detail::pointmap_traits;

                public:

                        /** @} */
                        // --------------------------------------------

                        /// Sets the point weight, which is ignored in all classes but those which actually store that field (Note: No checks are done for out-of-bounds index). \sa getPointWeight
                        virtual void setPointWeight(size_t index,unsigned long w) { pointWeight[index]=w; }
                        /// Gets the point weight, which is ignored in all classes (defaults to 1) but in those which actually store that field (Note: No checks are done for out-of-bounds index).  \sa setPointWeight
                        virtual unsigned int getPointWeight(size_t index) const { return pointWeight[index]; }

                protected:
                        std::vector<uint32_t>  pointWeight;  //!< The points weights

                        /** Clear the map, erasing all the points.
                         */
                        virtual void  internal_clear();

                protected:
                        /** @name PLY Import virtual methods to implement in base classes
                            @{ */
                        /** In a base class, reserve memory to prepare subsequent calls to PLY_import_set_vertex */
                        virtual void PLY_import_set_vertex_count(const size_t N);
                        /** @} */

                }; // End of class def.

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

#endif