CRangeScanOps.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 CRANGESCANOPS_H
#define CRANGESCANOPS_H

#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/obs/CObservation3DRangeScan.h>
#include <mrpt/utils/CLoadableOptions.h>
#include <mrpt/utils/CConfigFileBase.h>
#include <mrpt/utils/CStream.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/maps/CPointsMap.h>
#include <mrpt/poses/CPosePDF.h>
#include <mrpt/poses/CPose3DPDF.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/slam/CICP.h>
#include <mrpt/system/os.h>

#include <iostream>
#include <vector>

namespace mrpt
{
namespace graphslam
{
namespace deciders
{
/**\brief Class for keeping together all the RangeScanner-related functions.
 *
 * ## Description
 *
 * Deciders that make use of either 2DRangeScans (laser generated
 * observations) or 3DRangeScans (RGBD-cameras) can inherit from
 * this class in case they want to use the underlying methods
 *
 * ### .ini Configuration Parameters
 *
 * \htmlinclude graphslam-engine_config_params_preamble.txt
 *
 * - \b conversion_sensor_label
 *   + \a Default value : "KINECT_TO_2D_SCAN"
 *   + \a Required      : FALSE
 *   + \a Description   : Applicable only when dealing with 3DRangeScans.
 *   Used for converting 3DRangeScan to 2DRangesScan so that they are
 *   visualized on the 2D surface
 *
 * - \b conversion_angle_sup
 *   + \a Default value : 10
 *   + \a Required      : FALSE
 *   + \a Description   : Applicable only when dealing with 3DRangeScans.
 *   Used for converting 3DRangeScan to 2DRangesScan so that they are
 *   visualized on the 2D surface
 *
 * - \b conversion_angle_inf
 *   + \a Default value : 10
 *   + \a Required      : FALSE
 *   + \a Description   : Applicable only when dealing with 3DRangeScans.
 *   Used for converting 3DRangeScan to 2DRangesScan so that they are
 *   visualized on the 2D surface
 *
 * - \b conversion_oversampling_ratio
 *   + \a Default value : 1.1
 *   + \a Required      : FALSE
 *   + \a Description   : Applicable only when dealing with 3DRangeScans.
 *   Used for converting 3DRangeScan to 2DRangesScan so that they are
 *   visualized on the 2D surface
 *
 * \note Class contains an instance of the mrpt::slam::CICP class and it parses
 * the configuration parameters of the latter from the
 * "ICP" section. Refer to
 * mrpt::slam::CICP documentation for its list of
 * configuration parameters
 *
 * \ingroup mrpt_graphslam_grp
 */
template <class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
class CRangeScanOps
{
        /**\brief Handy typedefs */
        /**\{*/
        typedef typename GRAPH_T::constraint_t constraint_t;
        typedef CRangeScanOps<GRAPH_T> self_t;
        /**\}*/

   protected:
        // Protected methods
        // ////////////////////////////////////////////////////////////

        /**\brief Align the 2D range scans provided and fill the potential edge that
         * can transform the one into the other.
         *
         * User can optionally ask that additional information be returned in a
         * TReturnInfo struct
         */
        void getICPEdge(
                const mrpt::obs::CObservation2DRangeScan& from,
                const mrpt::obs::CObservation2DRangeScan& to, constraint_t* rel_edge,
                const mrpt::poses::CPose2D* initial_pose = nullptr,
                mrpt::slam::CICP::TReturnInfo* icp_info = nullptr);
        /**\brief Align the 3D range scans provided and find the potential edge that
         * can transform the one into the other.
         *
         * Fills the 2D part (rel_edge) of the 3D constraint between the scans,
         * since
         * we are interested in computing the 2D alignment. User can optionally ask
         * that additional information be returned in a TReturnInfo struct
         */
        void getICPEdge(
                const mrpt::obs::CObservation3DRangeScan& from,
                const mrpt::obs::CObservation3DRangeScan& to, constraint_t* rel_edge,
                const mrpt::poses::CPose2D* initial_pose = nullptr,
                mrpt::slam::CICP::TReturnInfo* icp_info = nullptr);
        /**\brief Reduce the size of the given CPointsMap by keeping one out of
         * "keep_point_every" points.
         *
         * \note If low_lim is set then the PointsMap will contain at least low_lim
         * measurements, regardless of keep_point_every value. Set low_lim to 0 if
         * no
         * lower limit is to be specified
         */
        void decimatePointsMap(
                mrpt::maps::CPointsMap* m, size_t keep_point_every = 4,
                size_t low_lim = 0);
        /**\brief Wrapper around the CObservation3DRangeScan::convertTo2DScan
         * corresponding method
         *
         * \return True if operation was successful, false otherwise
         */
        bool convert3DTo2DRangeScan(
                /*from = */ mrpt::obs::CObservation3DRangeScan::Ptr& scan3D_in,
                /*to   = */ mrpt::obs::CObservation2DRangeScan::Ptr* scan2D_out =
                        nullptr);

        struct TParams : public mrpt::utils::CLoadableOptions
        {
           public:
                TParams();
                ~TParams();

                void loadFromConfigFile(
                        const mrpt::utils::CConfigFileBase& source,
                        const std::string& section);
                void dumpToTextStream(mrpt::utils::CStream& out) const;

                mrpt::slam::CICP icp;

                /**\brief Struct holding the parameters of 3D to the corresponding 2D
                 * range scan conversion.
                 */
                mrpt::obs::T3DPointsTo2DScanParams conversion_params;

                bool has_read_config;
        };
        TParams params;
};
}
}
}  // end of namespaces

#include "CRangeScanOps_impl.h"
#endif /* end of include guard: CRANGESCANOPS_H */