$search
00001 /****************************************************************************** 00002 * \file 00003 * 00004 * $Id:$ 00005 * 00006 * Copyright (C) Brno University of Technology 00007 * 00008 * This file is part of software developed by dcgm-robotics@FIT group. 00009 * 00010 * Author: Vit Stancl (stancl@fit.vutbr.cz) 00011 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz) 00012 * Date: 25/1/2012 00013 * 00014 * This file is free software: you can redistribute it and/or modify 00015 * it under the terms of the GNU Lesser General Public License as published by 00016 * the Free Software Foundation, either version 3 of the License, or 00017 * (at your option) any later version. 00018 * 00019 * This file is distributed in the hope that it will be useful, 00020 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00021 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00022 * GNU Lesser General Public License for more details. 00023 * 00024 * You should have received a copy of the GNU Lesser General Public License 00025 * along with this file. If not, see <http://www.gnu.org/licenses/>. 00026 */ 00027 00028 #pragma once 00029 #ifndef pcl_registration_module_H_included 00030 #define pcl_registration_module_H_included 00031 00032 #include <srs_env_model/but_server/server_tools.h> 00033 #include <srs_env_model/OctomapUpdates.h> 00034 #include <pcl/registration/icp.h> 00035 #include <pcl/registration/ia_ransac.h> 00036 #include <pcl/registration/icp_nl.h> 00037 00038 00039 namespace srs_env_model 00040 { 00041 enum EPclRegistrationMode 00042 { 00043 PCL_REGISTRATION_MODE_NONE = 0, 00044 PCL_REGISTRATION_MODE_ICP = 1, 00045 PCL_REGISTRATION_MODE_ICPNL = 2, 00046 PCL_REGISTRATION_MODE_SCA = 3 00047 }; 00048 00049 template <typename PointSource, typename PointTarget, typename Scalar = float> 00050 class CPclRegistration 00051 { 00052 public: 00053 typedef typename pcl::Registration<PointSource, PointTarget> tRegistration; 00054 typedef typename pcl::Registration<PointSource, PointTarget>::PointCloudSource PointCloudSource; 00055 typedef typename PointCloudSource::Ptr PointSourcePtr; 00056 typedef typename PointCloudSource::ConstPtr PointSourceConstPtr; 00057 00058 typedef typename pcl::Registration<PointSource, PointTarget>::PointCloudTarget PointCloudTarget; 00059 typedef typename PointCloudTarget::Ptr PointTargetPtr; 00060 typedef typename PointCloudTarget::ConstPtr PointTargetConstPtr; 00061 00062 // String mode names 00063 static const std::string m_mode_names[]; 00064 00065 public: 00067 CPclRegistration() : m_mode(PCL_REGISTRATION_MODE_NONE), m_registrationPtr(0) { } 00068 00070 void setMode( EPclRegistrationMode mode ); 00071 00073 EPclRegistrationMode getMode() { return m_mode; } 00074 00076 bool isRegistering() { return m_mode != PCL_REGISTRATION_MODE_NONE; } 00077 00079 Eigen::Matrix4f getTransform() 00080 { 00081 if( m_registrationPtr != 0 ) 00082 return m_registrationPtr->getFinalTransformation(); 00083 00084 return Eigen::Matrix4f(); 00085 } 00086 00091 bool process( PointSourcePtr & source, PointTargetPtr & target, PointSourcePtr & output ); 00092 00094 template< class tpRegistration > 00095 tpRegistration * getRegistrationAlgorithmPtr() 00096 { 00097 return m_registrationPtr; 00098 } 00099 00102 void init( ros::NodeHandle & node_handle ); 00103 00105 std::string getStrMode() { return m_mode_names[ m_mode ]; } 00106 00108 void resetParameters(); 00109 00110 protected: 00113 EPclRegistrationMode modeFromString( const std::string & name ); 00114 00116 void setRegistrationParameters(); 00117 00119 void setSCAParameters(); 00120 00121 bool inSensorCone(const cv::Point2d& uv); 00122 00123 protected: 00125 EPclRegistrationMode m_mode; 00126 00128 pcl::IterativeClosestPoint< PointSource, PointTarget > m_algIcp; 00129 00131 pcl::IterativeClosestPointNonLinear< PointSource, PointTarget > m_algIcpNl; 00132 00134 pcl::SampleConsensusInitialAlignment< PointSource, PointTarget, pcl::FPFHSignature33 > m_algSCA; 00135 00137 tRegistration * m_registrationPtr; 00138 00139 //--------------------------- 00140 // Common parameters 00141 00143 int m_maxIterations; 00144 00146 double m_RANSACOutlierRejectionThreshold; 00147 00149 double m_maxCorrespondenceDistance; 00150 00152 double m_transformationEpsilon ; 00153 00154 //-------------------------- 00155 // SCA features 00156 00158 double m_scaMinSampleDistance; 00159 00161 int m_scaNumOfSamples; 00162 00164 int m_scaCorrespondenceRamdomness; 00165 00166 public: 00167 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00168 00169 }; // CPclRegistration 00170 00171 00172 } // namespace srs_env_model 00173 00174 #include "pcl_registration_module.hpp" 00175 00176 #endif // pcl_registration_module_H_included 00177