feature_registration_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2015, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 #define BOOST_PARAMETER_MAX_ARITY 7
00036 #include "jsk_pcl_ros/feature_registration.h"
00037 #include <pcl/registration/sample_consensus_prerejective.h>
00038 #include <pcl/segmentation/sac_segmentation.h>
00039 #include <pcl/features/fpfh_omp.h>
00040 #include <eigen_conversions/eigen_msg.h>
00041 #include <pcl_conversions/pcl_conversions.h>
00042 #include "jsk_recognition_utils/pcl_conversion_util.h"
00043 
00044 namespace jsk_pcl_ros
00045 {
00046   void FeatureRegistration::onInit()
00047   {
00048     DiagnosticNodelet::onInit();
00049 
00050     srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00051     typename dynamic_reconfigure::Server<Config>::CallbackType f =
00052       boost::bind (&FeatureRegistration::configCallback, this, _1, _2);
00053     srv_->setCallback (f);
00054 
00055     
00056     // subscribe reference always
00057     reference_sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00058     sub_input_reference_.subscribe(*pnh_, "input/reference/cloud", 1);
00059     sub_input_reference_feature_.subscribe(*pnh_, "input/reference/feature", 1);
00060     reference_sync_->connectInput(
00061       sub_input_reference_, sub_input_reference_feature_);
00062     reference_sync_->registerCallback(boost::bind(&FeatureRegistration::referenceCallback,
00063                                                   this, _1, _2));
00064     pub_pose_ = advertise<geometry_msgs::PoseStamped>(*pnh_, "output", 1);
00065     pub_cloud_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output/cloud", 1);
00066     onInitPostProcess();
00067   }
00068 
00069   void FeatureRegistration::subscribe()
00070   {
00071     sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00072     sub_input_.subscribe(*pnh_, "input", 1);
00073     sub_input_feature_.subscribe(*pnh_, "input/feature", 1);
00074     sync_->connectInput(
00075       sub_input_, sub_input_feature_);
00076     sync_->registerCallback(boost::bind(&FeatureRegistration::estimate,
00077                                         this, _1, _2));
00078   }
00079 
00080   void FeatureRegistration::unsubscribe()
00081   {
00082     sub_input_.unsubscribe();
00083     sub_input_feature_.unsubscribe();
00084   }
00085 
00086   void FeatureRegistration::referenceCallback(
00087     const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
00088     const sensor_msgs::PointCloud2::ConstPtr& feature_msg)
00089   {
00090     boost::mutex::scoped_lock lock(mutex_);
00091     NODELET_DEBUG("update reference");
00092     reference_cloud_.reset(new pcl::PointCloud<pcl::PointNormal>);
00093     reference_feature_.reset(new pcl::PointCloud<pcl::FPFHSignature33>);
00094     pcl::fromROSMsg(*cloud_msg, *reference_cloud_);
00095     pcl::fromROSMsg(*feature_msg, *reference_feature_);
00096   }
00097 
00098   void FeatureRegistration::configCallback(
00099     Config &config, uint32_t level)
00100   {
00101     boost::mutex::scoped_lock lock(mutex_);
00102     max_iterations_ = config.max_iterations;
00103     correspondence_randomness_ = config.correspondence_randomness;
00104     similarity_threshold_ = config.similarity_threshold;
00105     max_correspondence_distance_ = config.max_correspondence_distance;
00106     inlier_fraction_ = config.inlier_fraction;
00107   }
00108   
00109   void FeatureRegistration::estimate(
00110     const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
00111     const sensor_msgs::PointCloud2::ConstPtr& feature_msg)
00112   {
00113     boost::mutex::scoped_lock lock(mutex_);
00114     if (!reference_cloud_ || !reference_feature_) {
00115       NODELET_ERROR("Not yet reference data is available");
00116       return;
00117     }
00118 
00119     pcl::PointCloud<pcl::PointNormal>::Ptr
00120       cloud (new pcl::PointCloud<pcl::PointNormal>);
00121     pcl::PointCloud<pcl::PointNormal>::Ptr
00122       object_aligned (new pcl::PointCloud<pcl::PointNormal>);
00123     pcl::PointCloud<pcl::FPFHSignature33>::Ptr
00124       feature (new pcl::PointCloud<pcl::FPFHSignature33>);
00125     pcl::fromROSMsg(*cloud_msg, *cloud);
00126     pcl::fromROSMsg(*feature_msg, *feature);
00127 
00128     pcl::SampleConsensusPrerejective<pcl::PointNormal,
00129                                      pcl::PointNormal,
00130                                      pcl::FPFHSignature33> align;
00131     
00132     align.setInputSource(reference_cloud_);
00133     align.setSourceFeatures(reference_feature_);
00134 
00135     align.setInputTarget(cloud);
00136     align.setTargetFeatures(feature);
00137 
00138     align.setMaximumIterations(max_iterations_); // Number of RANSAC iterations
00139     align.setNumberOfSamples(3); // Number of points to sample for generating/prerejecting a pose
00140     align.setCorrespondenceRandomness(correspondence_randomness_); // Number of nearest features to use
00141     align.setSimilarityThreshold(similarity_threshold_); // Polygonal edge length similarity threshold
00142     align.setMaxCorrespondenceDistance(max_correspondence_distance_); // Inlier threshold
00143     align.setInlierFraction(inlier_fraction_); // Required inlier fraction for accepting a pose hypothesis
00144     align.align (*object_aligned);
00145   
00146     if (align.hasConverged ())
00147     {
00148       // Print results
00149       printf ("\n");
00150       Eigen::Affine3f transformation(align.getFinalTransformation());
00151       geometry_msgs::PoseStamped ros_pose;
00152       tf::poseEigenToMsg(transformation, ros_pose.pose);
00153       ros_pose.header = cloud_msg->header;
00154       pub_pose_.publish(ros_pose);
00155 
00156       pcl::PointCloud<pcl::PointNormal>::Ptr
00157         result_cloud (new pcl::PointCloud<pcl::PointNormal>);
00158       pcl::transformPointCloud(
00159         *reference_cloud_, *result_cloud, transformation);
00160       sensor_msgs::PointCloud2 ros_cloud;
00161       pcl::toROSMsg(*object_aligned, ros_cloud);
00162       ros_cloud.header = cloud_msg->header;
00163       pub_cloud_.publish(ros_cloud);
00164       
00165       //pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), object->size ());
00166     
00167     }
00168     else {
00169       NODELET_WARN("failed to align pointcloud");
00170     }
00171 
00172   }
00173 }
00174 
00175 #include <pluginlib/class_list_macros.h>
00176 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::FeatureRegistration, nodelet::Nodelet);


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Tue Jul 2 2019 19:41:43