pcl_registration_module.h

Go to the documentation of this file.

/******************************************************************************
 * \file
 *
 * $Id:$
 *
 * Copyright (C) Brno University of Technology
 *
 * This file is part of software developed by dcgm-robotics@FIT group.
 *
 * Author: Vit Stancl (stancl@fit.vutbr.cz)
 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz)
 * Date: 25/1/2012
 *
 * This file is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This file 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 Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this file.  If not, see <http://www.gnu.org/licenses/>.
 */

#pragma once
#ifndef pcl_registration_module_H_included
#define pcl_registration_module_H_included

#include <srs_env_model/but_server/server_tools.h>
#include <srs_env_model/OctomapUpdates.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/ia_ransac.h>
#include <pcl/registration/icp_nl.h>


namespace srs_env_model
{
enum EPclRegistrationMode
{
        PCL_REGISTRATION_MODE_NONE = 0,
        PCL_REGISTRATION_MODE_ICP = 1,
        PCL_REGISTRATION_MODE_ICPNL = 2,
        PCL_REGISTRATION_MODE_SCA = 3
};

template <typename PointSource, typename PointTarget, typename Scalar = float>
class CPclRegistration
{
public:
        typedef typename pcl::Registration<PointSource, PointTarget> tRegistration;
        typedef typename pcl::Registration<PointSource, PointTarget>::PointCloudSource PointCloudSource;
        typedef typename PointCloudSource::Ptr  PointSourcePtr;
        typedef typename PointCloudSource::ConstPtr     PointSourceConstPtr;

        typedef typename pcl::Registration<PointSource, PointTarget>::PointCloudTarget PointCloudTarget;
        typedef typename PointCloudTarget::Ptr  PointTargetPtr;
        typedef typename PointCloudTarget::ConstPtr     PointTargetConstPtr;

        // String mode names
        static const std::string m_mode_names[];

public:
        CPclRegistration() : m_mode(PCL_REGISTRATION_MODE_NONE), m_registrationPtr(0) { }

        void setMode( EPclRegistrationMode mode );

        EPclRegistrationMode getMode() { return m_mode; }

        bool isRegistering() { return m_mode != PCL_REGISTRATION_MODE_NONE; }

        Eigen::Matrix4f getTransform()
        {
                if( m_registrationPtr != 0 )
                        return m_registrationPtr->getFinalTransformation();

                return Eigen::Matrix4f();
        }

        bool process( PointSourcePtr & source, PointTargetPtr & target, PointSourcePtr & output );

        template< class tpRegistration >
        tpRegistration * getRegistrationAlgorithmPtr()
        {
                return m_registrationPtr;
        }

        void init( ros::NodeHandle & node_handle );

        std::string getStrMode() { return m_mode_names[ m_mode ]; }

        void resetParameters();

protected:
        EPclRegistrationMode modeFromString( const std::string & name );

        void setRegistrationParameters();

        void setSCAParameters();

        bool inSensorCone(const cv::Point2d& uv);

protected:
        EPclRegistrationMode m_mode;

        pcl::IterativeClosestPoint< PointSource, PointTarget > m_algIcp;

        pcl::IterativeClosestPointNonLinear< PointSource, PointTarget > m_algIcpNl;

        pcl::SampleConsensusInitialAlignment< PointSource, PointTarget, pcl::FPFHSignature33 > m_algSCA;

        tRegistration * m_registrationPtr;

        //---------------------------
        // Common parameters

        int m_maxIterations;

        double m_RANSACOutlierRejectionThreshold;

        double m_maxCorrespondenceDistance;

        double m_transformationEpsilon ;

        //--------------------------
        // SCA features

        double m_scaMinSampleDistance;

        int m_scaNumOfSamples;

        int m_scaCorrespondenceRamdomness;

public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

}; // CPclRegistration


} // namespace srs_env_model

#include "pcl_registration_module.hpp"

#endif //  pcl_registration_module_H_included