pointcloud_registration_node.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2010, Hozefa Indorewala <indorewala@ias.in.tum.de>
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /*
00031  * Author: Hozefa Indorewala
00032  * This node subscribes to a PointCloud2 topic, receives point clouds and then registers them and publishes
00033  * the result on /merged_pointcloud (PointCloud2)
00034  */
00035 
00036 #include <pcl/point_types.h>
00037 #include <pcl/features/feature.h>
00038 #include <pcl/ros/conversions.h>
00039 #include <ros/ros.h>
00040 #include <sensor_msgs/PointCloud2.h>
00041 #include <sensor_msgs/PointCloud.h>
00042 #include <sensor_msgs/point_cloud_conversion.h>
00043 #include <pointcloud_registration/pointcloud_registration_point_types.h>
00044 #include <pcl/io/pcd_io.h>
00045 #include <Eigen/SVD>
00046 
00047 #include "pcl/filters/statistical_outlier_removal.h" // to filter outliers
00048 
00049 #include "pcl/filters/voxel_grid.h" //for downsampling the point cloud
00050 
00051 #include "pcl/kdtree/kdtree_flann.h" //for the kdtree
00052 
00053 #include "pcl/registration/transforms.h" //for the transformation function
00054 
00055 #include <pcl/features/normal_3d_omp.h>
00056 
00057 #include <pointcloud_registration/icp/icp_correspondences_check.h> //for icp
00058 #include <algorithm> //for the sort and unique functions
00059 
00060 #include <ctime>
00061 
00062 const float PI = 3.14159265;
00063 
00064 typedef pcl::PointNormal  PointT;
00065 
00067 bool pclSort (pcl::PointNormal i, pcl::PointNormal j)
00068 {
00069   return (i.x < j.x);
00070 }
00071 
00073 bool pclUnique (pcl::PointNormal i, pcl::PointNormal j)
00074 {
00075   double x_diff = fabs(i.x - j.x);
00076   double y_diff = fabs(i.y - j.y);
00077   double z_diff = fabs(i.z - j.z);
00078   if(x_diff < 0.0001 && y_diff < 0.0001 && z_diff < 0.0001 )
00079     return 1;
00080   else
00081     return 0;
00082 }
00084 
00085 class PointCloudRegistration
00086 {
00087   public:
00088     PointCloudRegistration();
00089     ~PointCloudRegistration();
00090     void pointcloudRegistrationCallBack(const sensor_msgs::PointCloud2& msg);
00091     Eigen::Matrix4f getOverlapTransformation();
00092     void publishPointCloud(pcl::PointCloud<pcl::PointNormal> &pointcloud);
00093     pcl::PointCloud<pcl::PointNormal> convertFromMsgToPointCloud(const sensor_msgs::PointCloud2& pointcloud_msg);
00094     void spin (int argc, char** argv);
00095 
00096   private:
00097     ros::NodeHandle nh_;
00098   std::string merged_pointcloud_topic_, subscribe_pointcloud_topic_, frame_id_, field_;
00099     int max_number_of_iterations_icp_, max_nn_icp_, max_nn_overlap_;
00100     double downsample_leafsize_, epsilon_z_, epsilon_curvature_, epsilon_transformation_, radius_icp_, radius_overlap_;
00101     bool downsample_pointcloud_before_, downsample_pointcloud_after_, filter_outliers_, curvature_check_;
00102     int scan_index_, counter_;
00103     time_t start, end;
00104     Eigen::Matrix4f final_transformation_;
00105     ros::Subscriber pointcloud_subscriber_;
00106     ros::Publisher pointcloud_merged_publisher_;
00107 
00108     pcl::IterativeClosestPointCorrespondencesCheck<pcl::PointNormal, pcl::PointNormal> icp_; // for icp
00109     pcl::KdTreeFLANN<pcl::PointNormal> kdtree_;  // for kdtree
00110     bool firstCloudReceived_, secondCloudReceived_;
00111     pcl::PointCloud<pcl::PointNormal> pointcloud2_current_, pointcloud2_merged_, pointcloud2_transformed_;
00112 };
00113 
00115 
00116 Eigen::Matrix4f PointCloudRegistration::getOverlapTransformation()
00117 {
00118   // In this function we extract the overlapped region of the two points cloud and compute
00119   // the transformation and return it.
00120 
00121   if(firstCloudReceived_ == false && secondCloudReceived_ == false )
00122   {
00123     ROS_ERROR("[PointCloudRegistration:] getOverlapTransformation called before receiving atleast 2 point clouds");
00124     exit(1);
00125   }
00126   else
00127   {
00128     //searching for overlapped points in the point cloud
00129     // Allocate enough space to hold the results
00130     std::vector<int> nn_indices (max_nn_overlap_);
00131     std::vector<float> nn_dists (max_nn_overlap_);
00132 
00133     pcl::PointCloud<pcl::PointNormal> overlap_model, overlap_current;
00134     Eigen::Matrix4f transformation;
00135     ROS_INFO("[PointCloudRegistration:] finding overlapping points");
00136     start = time(NULL);
00137     std::vector<pcl:: PointNormal, Eigen::aligned_allocator<pcl:: PointNormal> >::iterator it;
00138     for(size_t idx = 0 ; idx < pointcloud2_current_.points.size(); idx++ )
00139     {
00140       kdtree_.radiusSearch(pointcloud2_current_, idx, radius_overlap_, nn_indices, nn_dists, max_nn_overlap_);
00141 
00142       if(nn_indices.size() > 0 )
00143       {
00144         overlap_current.points.push_back(pointcloud2_current_.points[idx]);
00145               for(size_t i = 0 ; i < nn_indices.size(); i++)
00146         {
00147           overlap_model.points.push_back (kdtree_.getInputCloud()->points[nn_indices[i]]);
00148         }
00149       }
00150     }
00151     end = time(NULL);
00152     ROS_INFO("[PointCloudRegistration:] found overlapping points in %d seconds with points %ld", 
00153              (int)(end - start), overlap_current.points.size());
00154 
00155     //Getting rid of duplicate points in model
00156     ROS_INFO("[PointCloudRegistration:] removing duplicate points");
00157     start = time(NULL);
00158     std::sort(overlap_model.points.begin(), overlap_model.points.end(), pclSort);
00159     it = std::unique(overlap_model.points.begin(), overlap_model.points.end(), pclUnique);
00160     overlap_model.points.resize(it - overlap_model.points.begin());
00161     end = time(NULL);
00162     ROS_INFO("[PointCloudRegistration:] removed duplicate points %d seconds", (int)(end - start));
00163 
00164     icp_.setInputTarget(boost::make_shared< pcl::PointCloud < pcl::PointNormal> > (overlap_model));
00165     icp_.setInputCloud(boost::make_shared< pcl::PointCloud < pcl::PointNormal> > (overlap_current));
00166 
00167     ROS_INFO("[PointCloudRegistration: ] aligning");
00168     icp_.align(pointcloud2_transformed_);
00169 
00170     ROS_INFO("[PointCloudRegistration:] getting final transformation");
00171     transformation = icp_.getFinalTransformation();
00172     return (transformation);
00173   }
00174 }
00175 
00177 
00178 void PointCloudRegistration::publishPointCloud(pcl::PointCloud<pcl::PointNormal> &pointcloud)
00179 {
00180   sensor_msgs::PointCloud2 mycloud;
00181   pcl::toROSMsg(pointcloud, mycloud);
00182 
00183   if( downsample_pointcloud_after_ == true)
00184   {
00185     // for downsampling of pointclouds
00186     pcl::VoxelGrid<sensor_msgs::PointCloud2> sor_;
00187     sensor_msgs::PointCloud2 mycloud_downsampled;
00188 
00189     //Now we will downsample the point cloud
00190     sor_.setInputCloud (boost::make_shared<sensor_msgs::PointCloud2> (mycloud));
00191     sor_.setLeafSize (downsample_leafsize_, downsample_leafsize_, downsample_leafsize_);
00192     sor_.filter (mycloud_downsampled);
00193     mycloud_downsampled.header.frame_id = frame_id_;
00194     mycloud_downsampled.header.stamp = ros::Time::now();
00195 
00196     pointcloud_merged_publisher_.publish(mycloud_downsampled);
00197   }
00198   else
00199   {
00200     mycloud.header.frame_id = frame_id_;
00201     mycloud.header.stamp = ros::Time::now();
00202 
00203     pointcloud_merged_publisher_.publish(mycloud);
00204   }
00205   ROS_INFO("[PointCloudRegistration:] Merged Point cloud published");
00206 }
00208 
00209 pcl::PointCloud<pcl::PointNormal> PointCloudRegistration::convertFromMsgToPointCloud(const sensor_msgs::PointCloud2& pointcloud_msg)
00210 {
00211   pcl::PointCloud<pcl::PointNormal> pointcloud_pcl_normals;
00212   pcl::fromROSMsg(pointcloud_msg, pointcloud_pcl_normals);
00213   return (pointcloud_pcl_normals);
00215   //TODO: make this work again with Hokuyo scans
00217   // //incrementing the scan index
00218   // scan_index_++;
00219   // // Declaring some variables required in this function
00220   // sensor_msgs::PointCloud2 pointcloud2_msg = pointcloud_msg;
00221   // pcl::PointCloud<pcl::PointXYZ> pointcloud_pcl_step01, pointcloud_pcl_step02;
00222   // pcl::PointCloud<pcl::PointNormal> pointcloud_pcl_normals;
00223   // pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
00224   // pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree_ptr_;
00225   // tree_ptr_ = boost::make_shared<pcl::KdTreeFLANN<pcl::PointXYZ> > ();
00226   // std::vector<int> indices;
00227 
00228   // // Converting from PointCloud msg format to PointCloud2 msg format
00229 
00230   // //sensor_msgs::convertPointCloudToPointCloud2(pointcloud_msg, pointcloud2_msg);
00231 
00232   // //This is done because there is a bug in PCL.
00233   // // for(u_int i = 0 ; i < pointcloud2_msg.fields.size(); i++)
00234   // // {
00235   // //   if(pointcloud2_msg.fields[i].name == "intensities")
00236   // //   {
00237   // //     pointcloud2_msg.fields[i].name = "intensity";
00238   // //   }
00239   // // }
00240 
00241   // // STEP 01: Check if we should downsample the input cloud or not
00242   // if(downsample_pointcloud_before_ == true)
00243   // {
00244   //   ROS_INFO ("PointCloud before downsampling: %d .", pointcloud2_msg.width * pointcloud2_msg.height);
00245   //   sensor_msgs::PointCloud2 pointcloud_downsampled_msg;
00246   //   pcl::VoxelGrid<sensor_msgs::PointCloud2> sor_; // for downsampling of pointclouds
00247 
00248   //   //Now we will downsample the point cloud
00249   //   sor_.setInputCloud (boost::make_shared<sensor_msgs::PointCloud2> (pointcloud2_msg));
00250   //   sor_.setLeafSize (downsample_leafsize_, downsample_leafsize_, downsample_leafsize_);
00251   //   sor_.filter (pointcloud_downsampled_msg);
00252   //   ROS_INFO ("PointCloud after downsampling: %d .", pointcloud_downsampled_msg.width * pointcloud_downsampled_msg.height);
00253 
00254   //   // Converting from PointCloud2 msg format to pcl pointcloud format
00255   //   pcl::fromROSMsg(pointcloud_downsampled_msg, pointcloud_pcl_step01);
00256   // }
00257   // else
00258   // {
00259   //   // Converting from PointCloud2 msg format to pcl pointcloud format
00260   //   pcl::fromROSMsg(pointcloud2_msg, pointcloud_pcl_step01);
00261   // }
00262 
00263   // // STEP 02: Check if we should filter the outliers or not
00264 
00265   // if(filter_outliers_)
00266   // {
00267   //   // Removing outliers
00268   //   pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
00269   //   sor.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(pointcloud_pcl_step01));
00270   //   sor.setMeanK (50);
00271   //   sor.setStddevMulThresh (1.0);
00272   //   sor.filter (pointcloud_pcl_step02);
00273   // }
00274   // else
00275   // {
00276   //   pointcloud_pcl_step02 = pointcloud_pcl_step01;
00277   // }
00278   // tree_ptr_->setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> > (pointcloud_pcl_step02));
00279   // indices.resize (pointcloud_pcl_step02.points.size ());
00280   // for (size_t i = 0; i < indices.size (); ++i)
00281   // {
00282   //   indices[i] = i;
00283   // }
00284 
00285   // return (pointcloud_pcl_normals);
00286   // // STEP 03: Here we perform Normal Estimation on the input cloud
00287 
00288   // // Object
00289   // pcl::PointCloud<pcl::Normal> normals;
00290   // // set parameters
00291 
00292   // n.setInputCloud (boost::make_shared <const pcl::PointCloud<pcl::PointXYZ> > (pointcloud_pcl_step02));
00293   // n.setIndices (boost::make_shared <std::vector<int> > (indices));
00294   // n.setSearchMethod (tree_ptr_);
00295   // n.setKSearch (10);
00296 
00297   // // estimate
00298   // n.compute (normals);
00299 
00300   // // STEP 04: Here we copy data from the normals and the input cloud into the pcl::PointNormal cloud
00301 
00302   // pointcloud_pcl_normals.points.resize(pointcloud_pcl_step02.points.size());
00303 
00304   // for(u_int i = 0 ; i < pointcloud_pcl_step02.points.size(); i++)
00305   // {
00306   //   pointcloud_pcl_normals.points[i].x = pointcloud_pcl_step02.points[i].x;
00307   //   pointcloud_pcl_normals.points[i].y = pointcloud_pcl_step02.points[i].y;
00308   //   pointcloud_pcl_normals.points[i].z = pointcloud_pcl_step02.points[i].z;
00309   //   //pointcloud_pcl_normals.points[i].rgb = pointcloud_pcl_step02.points[i].rgb;
00310   //   //    pointcloud_pcl_normals.points[i].intensity = pointcloud_pcl_step02.points[i].intensity;
00311   //   pointcloud_pcl_normals.points[i].normal[0] = normals.points[i].normal[0];
00312   //   pointcloud_pcl_normals.points[i].normal[1] = normals.points[i].normal[1];
00313   //   pointcloud_pcl_normals.points[i].normal[2] = normals.points[i].normal[2];
00314   //   pointcloud_pcl_normals.points[i].curvature = normals.points[i].curvature;
00315   //   //pointcloud_pcl_normals.points[i].scan_index = scan_index_;
00316   // }
00317 
00318   // pointcloud_pcl_normals.header.frame_id = pointcloud_pcl_normals.header.frame_id;
00319   // pointcloud_pcl_normals.header.stamp = pointcloud_pcl_normals.header.stamp;
00320   // pointcloud_pcl_normals.width    = pointcloud_pcl_normals.points.size ();
00321   // pointcloud_pcl_normals.height   = 1;
00322   // pointcloud_pcl_normals.is_dense = false;
00323 
00324   // return (pointcloud_pcl_normals);
00325 }
00326 
00328 
00329 PointCloudRegistration::PointCloudRegistration(): nh_("~")
00330 {
00331   nh_.param("publish_merged_pointcloud_topic", merged_pointcloud_topic_, std::string("/merged_pointcloud"));
00332   nh_.param("subscribe_pointcloud_topic", subscribe_pointcloud_topic_, std::string("/shoulder_cloud"));
00333   nh_.param("max_number_of_iterations_icp", max_number_of_iterations_icp_, 100);
00334   nh_.param("max_nn_icp", max_nn_icp_, 100);
00335   nh_.param("max_nn_overlap", max_nn_overlap_, 10);
00336   nh_.param("radius_icp", radius_icp_, 0.005);
00337   nh_.param("radius_overlap", radius_overlap_, 0.005);
00338   nh_.param("curvature_check", curvature_check_, true);
00339   nh_.param("downsample_pointcloud_before", downsample_pointcloud_before_, false);
00340   nh_.param("downsample_pointcloud_after", downsample_pointcloud_after_, false);
00341   nh_.param("filter_outliers", filter_outliers_, true);
00342   nh_.param("downsample_leafsize", downsample_leafsize_, 0.05);
00343   nh_.param("epsilon_z", epsilon_z_, 0.001);
00344   nh_.param("epsilon_curvature", epsilon_curvature_, 0.001);
00345   nh_.param("epsilon_transformation", epsilon_transformation_, 1e-8);
00346   nh_.param("field", field_, std::string("x"));
00347   firstCloudReceived_ = false;
00348   secondCloudReceived_ = false;
00349   scan_index_ = 0;
00350   counter_ = 0;
00351   icp_.setMaximumIterations(max_number_of_iterations_icp_);
00352   icp_.setTransformationEpsilon(epsilon_transformation_);
00353   icp_.setParameters(radius_icp_, max_nn_icp_, epsilon_z_, epsilon_curvature_, curvature_check_, field_);
00354   ROS_INFO("[PointCloudRegistration:] pointcloud_registration node is up and running.");
00355   pointcloud_subscriber_ = nh_.subscribe(subscribe_pointcloud_topic_, 100, &PointCloudRegistration::pointcloudRegistrationCallBack, this);
00356   pointcloud_merged_publisher_ = nh_.advertise<sensor_msgs::PointCloud2>(merged_pointcloud_topic_, 100);
00357 }
00358 
00360 
00361 PointCloudRegistration::~PointCloudRegistration()
00362 {
00363   ROS_INFO("[PointCloudRegistration:] Shutting down pointcloud_registration node!.");
00364 }
00365 
00367 
00368 void PointCloudRegistration::pointcloudRegistrationCallBack(const sensor_msgs::PointCloud2& pointcloud_msg)
00369 {
00370   counter_++;
00371   frame_id_ = pointcloud_msg.header.frame_id;
00372   start = time(NULL);
00373   if( firstCloudReceived_ == false)
00374   {
00375     pointcloud2_current_ = convertFromMsgToPointCloud(pointcloud_msg);
00376     ROS_INFO("[PointCloudRegistration:] Size of point cloud received = %d", (int) pointcloud2_current_.points.size());
00377     firstCloudReceived_ = true;
00378     ROS_INFO("[PointCloudRegistration:] Received first point cloud with points %ld:", pointcloud2_current_.points.size());
00379     kdtree_.setInputCloud(boost::make_shared< pcl::PointCloud < pcl::PointNormal> > (pointcloud2_current_));
00380 
00381     pointcloud2_merged_ = pointcloud2_current_;
00382   }
00383   else if( secondCloudReceived_ == false)
00384   {
00385     secondCloudReceived_ = true;
00386     pointcloud2_current_ = convertFromMsgToPointCloud(pointcloud_msg);
00387     ROS_INFO("[PointCloudRegistration:] Received second point cloud with points %ld.", pointcloud2_current_.points.size());
00388 
00389     //Now we get the transformation from the overlapped regions of the 2 point clouds
00390     final_transformation_= getOverlapTransformation();
00391     pcl::transformPointCloud(pointcloud2_current_, pointcloud2_transformed_, final_transformation_);
00392     pointcloud2_merged_ += pointcloud2_transformed_;
00393 
00394   }
00395   else
00396   {
00397     pointcloud2_current_ = convertFromMsgToPointCloud(pointcloud_msg);
00398     ROS_INFO("[PointCloudRegistration:] Received point cloud number: %d with points %ld.", counter_, pointcloud2_current_.points.size());
00399     kdtree_.setInputCloud(boost::make_shared< pcl::PointCloud < pcl::PointNormal> > (pointcloud2_merged_));
00400 
00401     //Now we get the transformation from the overlapped regions of the 2 point clouds
00402     final_transformation_= getOverlapTransformation();
00403     pcl::transformPointCloud(pointcloud2_current_, pointcloud2_transformed_, final_transformation_);
00404 
00405     pointcloud2_merged_ += pointcloud2_transformed_;
00406   }
00407 
00408   publishPointCloud(pointcloud2_merged_);
00409   end = time(NULL);
00410   ROS_INFO("[PointCloudRegistration:] Time taken: %d seconds", (int)(end - start));
00411 
00412 }
00413 
00414    void PointCloudRegistration::spin (int argc, char** argv)
00415     {
00416       for (int i = 1; i < argc; i++)
00417       {
00418         ROS_INFO("[PointCloudRegistration:] New Cloud ===========================");
00419         ROS_INFO("[PointCloudRegistration:] Reading pcd %s.", argv[i]);
00420         pcl::PCDReader reader;
00421         pcl::PointCloud<PointT> input_cloud;
00422         sensor_msgs::PointCloud2 input_cloud_msg;
00423         reader.read (argv[i], input_cloud);
00424         if (input_cloud.points.size() == 0)
00425         {
00426           ROS_ERROR("[PointCloudRegistration:] input_cloud.points.size(): %ld", input_cloud.points.size());
00427           continue;
00428         }
00429         pcl::toROSMsg(input_cloud, input_cloud_msg);
00430         pointcloudRegistrationCallBack (input_cloud_msg);
00431         ros::spinOnce();
00432       }
00433       ROS_INFO("[PointCloudRegistration:] Done and Exiting!");
00434       ros::shutdown();
00435     }
00436 
00438 int main(int argc, char** argv)
00439 {
00440     ros::init(argc, argv, "pointcloud_registration");
00441     PointCloudRegistration pointcloud_registration;
00442 
00443     if (argc == 1)
00444     {
00445       ros::spin();
00446     }
00447     else if (argc >= 2  && std::string(argv[1]) != "-h")
00448     {
00449       pointcloud_registration.spin(argc, argv);
00450     }
00451     else
00452     {
00453       std::cerr << "Usage: " << std::endl 
00454                 << argv[0] << " - get the pointcloud from the topic" << std::endl 
00455                 << argv[0] << " <input_cloud.pcd> - get the pointcloud on cmd" << std::endl;
00456     }
00457     return(0);
00458 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


pointcloud_registration
Author(s): Hozefa Indorewala
autogenerated on Sun Oct 6 2013 11:55:57