ppf_registration_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: C++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2016, 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 
00036 #define BOOST_PARAMETER_MAX_ARITY 7
00037 
00038 #include <pcl/features/ppf.h>
00039 #include <pcl/features/normal_3d.h>
00040 #include <pcl/registration/ppf_registration.h>
00041 #include "jsk_pcl_ros/ppf_registration.h"
00042 
00043 namespace jsk_pcl_ros
00044 {
00045   void PPFRegistration::onInit()
00046   {
00047     DiagnosticNodelet::onInit();
00048     srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00049     dynamic_reconfigure::Server<Config>::CallbackType f =
00050       boost::bind (&PPFRegistration::configCallback, this, _1, _2);
00051     srv_->setCallback (f);
00052 
00053     pub_points_array_ = advertise<jsk_recognition_msgs::PointsArray>(*pnh_, "output/points_array", 1);
00054     pub_pose_array_ = advertise<geometry_msgs::PoseArray>(*pnh_, "output/pose_array", 1);
00055     pub_pose_stamped_ = advertise<geometry_msgs::PoseStamped>(*pnh_, "output/pose_stamped", 1);
00056     pub_cloud_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output/cloud", 1);
00057     onInitPostProcess();
00058   }
00059 
00060   void PPFRegistration::subscribe()
00061   {
00062     sub_input_.subscribe(*pnh_, "input/cloud", 1);
00063     sub_reference_array_.subscribe(*pnh_, "input/reference_array", 1);
00064     sub_reference_cloud_.subscribe(*pnh_, "input/reference_cloud", 1);
00065     if (use_array_)
00066     {
00067       if (approximate_sync_)
00068       {
00069         array_async_ = boost::make_shared<message_filters::Synchronizer<ArrayApproximateSyncPolicy> >(queue_size_);
00070         array_async_->connectInput(sub_input_, sub_reference_array_);
00071         array_async_->registerCallback(boost::bind(&PPFRegistration::ArrayRegistration, this, _1, _2));
00072       }
00073       else
00074       {
00075         array_sync_ = boost::make_shared<message_filters::Synchronizer<ArraySyncPolicy> >(queue_size_);
00076         array_sync_->connectInput(sub_input_, sub_reference_array_);
00077         array_sync_->registerCallback(boost::bind(&PPFRegistration::ArrayRegistration, this, _1, _2));
00078       }
00079     }
00080     else
00081     {
00082       if (approximate_sync_)
00083       {
00084         cloud_async_ = boost::make_shared<message_filters::Synchronizer<CloudApproximateSyncPolicy> >(queue_size_);
00085         cloud_async_->connectInput(sub_input_, sub_reference_cloud_);
00086         cloud_async_->registerCallback(boost::bind(&PPFRegistration::CloudRegistration, this, _1, _2));
00087       }
00088       else
00089       {
00090         cloud_sync_ = boost::make_shared<message_filters::Synchronizer<CloudSyncPolicy> >(queue_size_);
00091         cloud_sync_->connectInput(sub_input_, sub_reference_cloud_);
00092         cloud_sync_->registerCallback(boost::bind(&PPFRegistration::CloudRegistration, this, _1, _2));
00093       }
00094     }
00095   }
00096 
00097   void PPFRegistration::unsubscribe()
00098   {
00099     sub_input_.unsubscribe();
00100     sub_reference_array_.unsubscribe();
00101     sub_reference_cloud_.unsubscribe();
00102   }
00103 
00104   pcl::PointCloud<pcl::PointNormal>::Ptr PPFRegistration::calculateNormals (pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
00105   {
00106     float normal_estimation_search_radius = float(search_radius_);
00107     pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal> ());
00108     pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimation_filter;
00109     pcl::search::KdTree<pcl::PointXYZ>::Ptr search_tree (new pcl::search::KdTree<pcl::PointXYZ> ());
00110     pcl::PointCloud<pcl::PointNormal>::Ptr cloud_calculated (new pcl::PointCloud<pcl::PointNormal> ());
00111     normal_estimation_filter.setInputCloud (cloud);
00112     normal_estimation_filter.setSearchMethod (search_tree);
00113     normal_estimation_filter.setRadiusSearch (normal_estimation_search_radius);
00114     normal_estimation_filter.compute (*cloud_normals);
00115     pcl::concatenateFields (*cloud, *cloud_normals, *cloud_calculated);
00116 
00117     // DEBUG
00118     NODELET_INFO_STREAM("cloud with normals size:" << cloud_calculated->points.size());
00119     return cloud_calculated;
00120   }
00121 
00122   void PPFRegistration::configCallback(Config &config, uint32_t level)
00123   {
00124     boost::mutex::scoped_lock lock(mutex_);
00125     use_array_ = config.use_array;
00126     queue_size_ = config.queue_size;
00127     approximate_sync_ = config.approximate_sync;
00128     search_radius_ = config.search_radius;
00129     sampling_rate_ = config.sampling_rate;
00130   }
00131 
00132   void PPFRegistration::CloudRegistration
00133   (const sensor_msgs::PointCloud2::ConstPtr& input_cloud,
00134    const sensor_msgs::PointCloud2::ConstPtr& input_reference_cloud)
00135   {
00136     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
00137     pcl::fromROSMsg(*input_cloud, *cloud);
00138 
00139     //calculate normals for target
00140     cloud_with_normals = calculateNormals (cloud);
00141 
00142     // training from reference
00143     // convert from ROSMsg -> pcl PointCloud
00144     pcl::PointCloud<pcl::PointXYZ>::Ptr reference_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
00145     pcl::fromROSMsg(*input_reference_cloud, *reference_cloud);
00146 
00147     // calculate normal for reference
00148     reference_cloud_with_normals = calculateNormals (reference_cloud);
00149 
00150     // ppf estimation registration
00151     pcl::PointCloud<pcl::PPFSignature>::Ptr reference_cloud_ppf (new pcl::PointCloud<pcl::PPFSignature> ());
00152     ppf_estimator.setInputCloud (reference_cloud_with_normals);
00153     ppf_estimator.setInputNormals (reference_cloud_with_normals);
00154     ppf_estimator.compute (*reference_cloud_ppf);
00155 
00156     // hashmap search
00157     pcl::PPFHashMapSearch::Ptr hashmap_search (new pcl::PPFHashMapSearch (12.0f / 180.0f * float (M_PI), 0.05f));
00158     hashmap_search->setInputFeatureCloud (reference_cloud_ppf);
00159 
00160     // register references to target
00161     // set parameters for the PPF registration procedure
00162     ppf_registration.setSceneReferencePointSamplingRate (sampling_rate_);
00163     ppf_registration.setPositionClusteringThreshold (float(position_clustering_threshold_));
00164     ppf_registration.setRotationClusteringThreshold (float(rotation_clustering_threshold_) / 180.0f * float (M_PI));
00165     ppf_registration.setSearchMethod (hashmap_search);
00166     ppf_registration.setInputSource (reference_cloud_with_normals);
00167     ppf_registration.setInputTarget (cloud_with_normals);
00168     pcl::PointCloud<pcl::PointNormal> cloud_output_subsampled;
00169     ppf_registration.align (cloud_output_subsampled);
00170 
00171     // get ppf transformation
00172     Eigen::Matrix4f mat = ppf_registration.getFinalTransformation ();
00173     Eigen::Affine3f pose (mat);
00174     // DEBUG
00175     Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
00176     NODELET_INFO_STREAM( "Matrix:\n" << mat.format(CleanFmt));
00177 
00178     // transform reference
00179     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_output (new pcl::PointCloud<pcl::PointXYZ> ());
00180     pcl::transformPointCloud (*reference_cloud, *cloud_output, pose);
00181 
00182     //convert pcl to ROSMsg
00183     sensor_msgs::PointCloud2 cloud_msg;
00184     toROSMsg(*cloud_output, cloud_msg);
00185     geometry_msgs::Pose pose_msg_;
00186     geometry_msgs::PoseStamped pose_stamped_msg;
00187     tf::poseEigenToMsg(pose, pose_msg_);
00188     pose_stamped_msg.pose = pose_msg_;
00189 
00190     // publish
00191     pose_stamped_msg.header = input_cloud->header;
00192     cloud_msg.header = input_cloud->header;
00193     pub_pose_stamped_.publish(pose_stamped_msg);
00194     pub_cloud_.publish(cloud_msg);
00195   }
00196 
00197   void PPFRegistration::ArrayRegistration
00198   (const sensor_msgs::PointCloud2::ConstPtr& input_cloud,
00199    const jsk_recognition_msgs::PointsArray::ConstPtr& input_reference_points_array)
00200   {
00201     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
00202     pcl::fromROSMsg(*input_cloud, *cloud);
00203 
00204     //calculate normals for target
00205     cloud_with_normals = calculateNormals (cloud);
00206 
00207     jsk_recognition_msgs::PointsArray::Ptr points_array_msg (new jsk_recognition_msgs::PointsArray ());
00208     geometry_msgs::PoseArray::Ptr pose_array_msg (new geometry_msgs::PoseArray ());
00209     std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr > reference_cloud_vector;
00210     std::vector<pcl::PointCloud<pcl::PointNormal>::Ptr > reference_cloud_with_normals_vector;
00211     std::vector<pcl::PPFHashMapSearch::Ptr > hashmap_search_vector;
00212 
00213     // training from reference
00214     for (size_t reference_i=0; reference_i < input_reference_points_array->cloud_list.size(); reference_i++)
00215     {
00216       sensor_msgs::PointCloud2 input_reference_cloud = input_reference_points_array->cloud_list[reference_i];
00217 
00218       // convert from ROSMsg -> pcl PointCloud
00219       pcl::PointCloud<pcl::PointXYZ>::Ptr reference_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
00220       pcl::fromROSMsg(input_reference_cloud, *reference_cloud);
00221       reference_cloud_vector.push_back(reference_cloud);
00222 
00223       // calculate normal for reference
00224       reference_cloud_with_normals = calculateNormals (reference_cloud);
00225       reference_cloud_with_normals_vector.push_back(reference_cloud_with_normals);
00226 
00227       // ppf estimation registration
00228       pcl::PointCloud<pcl::PPFSignature>::Ptr reference_cloud_ppf (new pcl::PointCloud<pcl::PPFSignature> ());
00229       ppf_estimator.setInputCloud (reference_cloud_with_normals);
00230       ppf_estimator.setInputNormals (reference_cloud_with_normals);
00231       ppf_estimator.compute (*reference_cloud_ppf);
00232 
00233       // hashmap search
00234       pcl::PPFHashMapSearch::Ptr hashmap_search (new pcl::PPFHashMapSearch (12.0f / 180.0f * float (M_PI), 0.05f));
00235       hashmap_search->setInputFeatureCloud (reference_cloud_ppf);
00236       hashmap_search_vector.push_back(hashmap_search);
00237     }
00238 
00239     // register references to target
00240     for (size_t reference_i=0; reference_i < input_reference_points_array->cloud_list.size(); reference_i++)
00241     {
00242       // set parameters for the PPF registration procedure
00243       ppf_registration.setSceneReferencePointSamplingRate (sampling_rate_);
00244       ppf_registration.setPositionClusteringThreshold (float(position_clustering_threshold_));
00245       ppf_registration.setRotationClusteringThreshold (float(rotation_clustering_threshold_) / 180.0f * float (M_PI));
00246       ppf_registration.setSearchMethod (hashmap_search_vector[reference_i]);
00247       ppf_registration.setInputSource (reference_cloud_with_normals_vector[reference_i]);
00248       ppf_registration.setInputTarget (cloud_with_normals);
00249       pcl::PointCloud<pcl::PointNormal> cloud_output_subsampled;
00250       ppf_registration.align (cloud_output_subsampled);
00251 
00252       // get ppf transformation
00253       Eigen::Matrix4f mat = ppf_registration.getFinalTransformation ();
00254       Eigen::Affine3f pose (mat);
00255       // DEBUG
00256       Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
00257       NODELET_INFO_STREAM( "Matrix:\n" << mat.format(CleanFmt));
00258 
00259       // transform reference
00260       pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_output (new pcl::PointCloud<pcl::PointXYZ> ());
00261       pcl::transformPointCloud (*reference_cloud_vector[reference_i], *cloud_output, pose);
00262 
00263       //convert pcl to ROSMsg
00264       sensor_msgs::PointCloud2 cloud_msg_;
00265       toROSMsg(*cloud_output, cloud_msg_);
00266       geometry_msgs::Pose pose_msg_;
00267       tf::poseEigenToMsg(pose, pose_msg_);
00268       points_array_msg->cloud_list.push_back(cloud_msg_);
00269       pose_array_msg->poses.push_back(pose_msg_);
00270     }
00271     // publish
00272     pose_array_msg->header = input_cloud->header;
00273     points_array_msg->header = input_cloud->header;
00274     pub_pose_array_.publish(pose_array_msg);
00275     pub_points_array_.publish(points_array_msg);
00276   }
00277 }
00278 
00279 #include <pluginlib/class_list_macros.h>
00280 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::PPFRegistration, nodelet::Nodelet);


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