incremental_model_registration_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, 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 
00037 #include <sstream>
00038 #include "jsk_pcl_ros/incremental_model_registration.h"
00039 #include "jsk_recognition_utils/pcl_conversion_util.h"
00040 #include <pcl/common/transforms.h>
00041 #include <pcl/filters/extract_indices.h>
00042 #include <jsk_recognition_msgs/ICPAlign.h>
00043 
00044 namespace jsk_pcl_ros
00045 {
00046   CapturedSamplePointCloud::CapturedSamplePointCloud()
00047   {
00048 
00049   }
00050 
00051   CapturedSamplePointCloud::CapturedSamplePointCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
00052                          const Eigen::Affine3f& pose):
00053     original_cloud_(cloud), original_pose_(pose),
00054     refined_cloud_(new pcl::PointCloud<pcl::PointXYZRGB>)
00055   {
00056     
00057   }
00058 
00059   pcl::PointCloud<pcl::PointXYZRGB>::Ptr
00060   CapturedSamplePointCloud::getOriginalPointCloud()
00061   {
00062     return original_cloud_;
00063   }
00064 
00065   Eigen::Affine3f CapturedSamplePointCloud::getOriginalPose()
00066   {
00067     return original_pose_;
00068   }
00069 
00070   pcl::PointCloud<pcl::PointXYZRGB>::Ptr
00071   CapturedSamplePointCloud::getRefinedPointCloud()
00072   {
00073     return refined_cloud_;
00074   }
00075 
00076   Eigen::Affine3f CapturedSamplePointCloud::getRefinedPose()
00077   {
00078     return refined_pose_;
00079   }
00080 
00081   void CapturedSamplePointCloud::setRefinedPointCloud(
00082     pcl::PointCloud<pcl::PointXYZRGB> cloud)
00083   {
00084     *refined_cloud_ = cloud;   // copying
00085   }
00086 
00087   void CapturedSamplePointCloud::setRefinedPose(
00088     Eigen::Affine3f pose)
00089   {
00090     refined_pose_ = Eigen::Affine3f(pose);
00091   }
00092   
00093   void IncrementalModelRegistration::onInit()
00094   {
00095     DiagnosticNodelet::onInit();
00096     pnh_->param("frame_id", frame_id_,
00097                 std::string("multisense/left_camera_optical_frame"));
00098     start_registration_srv_
00099       = pnh_->advertiseService(
00100         "start_registration", &IncrementalModelRegistration::startRegistration,
00101         this);
00102     pub_cloud_non_registered_
00103       = pnh_->advertise<sensor_msgs::PointCloud2>("output/non_registered", 1);
00104     pub_registered_
00105       = pnh_->advertise<sensor_msgs::PointCloud2>("output/registered", 1);
00106     sub_cloud_.subscribe(*pnh_, "input", 1);
00107     sub_indices_.subscribe(*pnh_, "input/indices", 1);
00108     sub_pose_.subscribe(*pnh_, "input/pose", 1);
00109     sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00110     sync_->connectInput(sub_cloud_, sub_indices_, sub_pose_);
00111     sync_->registerCallback(boost::bind(
00112                               &IncrementalModelRegistration::newsampleCallback,
00113                               this, _1, _2, _3));
00114     onInitPostProcess();
00115   }
00116 
00117   void IncrementalModelRegistration::transformPointCloudRepsectedToPose(
00118     pcl::PointCloud<pcl::PointXYZRGB>::Ptr input,
00119     pcl::PointCloud<pcl::PointXYZRGB>::Ptr output,
00120     const geometry_msgs::PoseStamped::ConstPtr& pose_msg)
00121   {
00122     Eigen::Affine3f posef;
00123     tf::poseMsgToEigen(pose_msg->pose, posef);
00124     Eigen::Affine3f transform = posef.inverse();
00125     
00126     pcl::transformPointCloud<pcl::PointXYZRGB>(
00127       *input, *output, transform);
00128   }
00129   
00130   void IncrementalModelRegistration::newsampleCallback(
00131     const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
00132     const pcl_msgs::PointIndices::ConstPtr& indices_msg,
00133     const geometry_msgs::PoseStamped::ConstPtr& pose_msg)
00134   {
00135     boost::mutex::scoped_lock lock(mutex_);
00136     pcl::PointCloud<pcl::PointXYZRGB>::Ptr
00137       cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00138     pcl::PointCloud<pcl::PointXYZRGB>::Ptr
00139       filtered_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00140     pcl::fromROSMsg(*cloud_msg, *cloud);
00141     pcl::PointCloud<pcl::PointXYZRGB>::Ptr
00142       transformed_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00143     pcl::PointIndices::Ptr
00144       indices (new pcl::PointIndices);
00145     indices->indices = indices_msg->indices;
00146     pcl::ExtractIndices<pcl::PointXYZRGB> ex;
00147     ex.setInputCloud(cloud);
00148     ex.setIndices(indices);
00149     ex.filter(*filtered_cloud);
00150     transformPointCloudRepsectedToPose(
00151       filtered_cloud, transformed_cloud, pose_msg);
00152     Eigen::Affine3f initial_pose;
00153     if (samples_.size() == 0) {
00154       // first pointcloud, it will be the `origin`
00155       tf::poseMsgToEigen(pose_msg->pose, origin_);
00156       initial_pose = origin_;
00157     }
00158     else {
00159       // compute transformation from origin_
00160       Eigen::Affine3f offset;
00161       tf::poseMsgToEigen(pose_msg->pose, offset);
00162       initial_pose = origin_.inverse() * offset;
00163     }
00164     CapturedSamplePointCloud::Ptr sample (new CapturedSamplePointCloud(transformed_cloud, initial_pose));
00165     samples_.push_back(sample);
00166     
00167     all_cloud_ = all_cloud_ + *transformed_cloud;
00168     sensor_msgs::PointCloud2 ros_all_cloud;
00169     pcl::toROSMsg(all_cloud_, ros_all_cloud);
00170     ros_all_cloud.header = cloud_msg->header;
00171     pub_cloud_non_registered_.publish(ros_all_cloud);
00172   }
00173 
00174   void IncrementalModelRegistration::callICP(
00175     pcl::PointCloud<pcl::PointXYZRGB>::Ptr reference,
00176     pcl::PointCloud<pcl::PointXYZRGB>::Ptr target,
00177     Eigen::Affine3f& output_transform)
00178   {
00179     ros::ServiceClient icp
00180       = pnh_->serviceClient<jsk_recognition_msgs::ICPAlign>("icp_service");
00181     sensor_msgs::PointCloud2 reference_ros, target_ros;
00182     pcl::toROSMsg(*reference, reference_ros);
00183     pcl::toROSMsg(*target, target_ros);
00184     ros::Time now = ros::Time::now();
00185     reference_ros.header.stamp = target_ros.header.stamp = now;
00186     reference_ros.header.frame_id = target_ros.header.frame_id = "map";
00187     jsk_recognition_msgs::ICPAlign srv;
00188     srv.request.reference_cloud = reference_ros;
00189     srv.request.target_cloud = target_ros;
00190     icp.call(srv);
00191     tf::poseMsgToEigen(srv.response.result.pose,
00192                             output_transform);
00193   }
00194   
00195   bool IncrementalModelRegistration::startRegistration(
00196     std_srvs::Empty::Request& req,
00197     std_srvs::Empty::Response& res)
00198   {
00199     if (samples_.size() <= 1) {
00200       ROS_ERROR("no enough samples");
00201       return false;
00202     }
00203     ROS_INFO("Starting registration %lu samples", samples_.size());
00204     // setup initial
00205     CapturedSamplePointCloud::Ptr initial_sample = samples_[0];
00206     initial_sample->setRefinedPointCloud(
00207       *(initial_sample->getOriginalPointCloud()));
00208     initial_sample->setRefinedPose(initial_sample->getOriginalPose());
00209     for (size_t i = 0; i < samples_.size() - 1; i++) {
00210       CapturedSamplePointCloud::Ptr from_sample = samples_[i];
00211       CapturedSamplePointCloud::Ptr to_sample = samples_[i+1];
00212       pcl::PointCloud<pcl::PointXYZRGB>::Ptr reference_cloud
00213         = from_sample->getRefinedPointCloud();
00214       pcl::PointCloud<pcl::PointXYZRGB>::Ptr target_cloud
00215         = to_sample->getOriginalPointCloud();
00216       Eigen::Affine3f transform;
00217       callICP(reference_cloud, target_cloud, transform);
00218       to_sample->setRefinedPose(to_sample->getOriginalPose() * transform);
00219       // transform pointcloud
00220       pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_cloud
00221         (new pcl::PointCloud<pcl::PointXYZRGB>);
00222       pcl::transformPointCloud<pcl::PointXYZRGB>(
00223         *target_cloud, *transformed_cloud, transform);
00224       to_sample->setRefinedPointCloud(*transformed_cloud);
00225     }
00226     pcl::PointCloud<pcl::PointXYZRGB>::Ptr registered_cloud
00227       (new pcl::PointCloud<pcl::PointXYZRGB>);
00228     pcl::PointCloud<pcl::PointXYZRGB>::Ptr non_registered_cloud
00229       (new pcl::PointCloud<pcl::PointXYZRGB>);
00230     for (size_t i = 0; i < samples_.size(); i++) {
00231       *registered_cloud = *(samples_[i]->getRefinedPointCloud()) + *registered_cloud;
00232       *non_registered_cloud = *(samples_[i]->getOriginalPointCloud()) + *non_registered_cloud;
00233     }
00234     sensor_msgs::PointCloud2 registered_ros_cloud, nonregistered_ros_cloud;
00235     pcl::toROSMsg(*registered_cloud, registered_ros_cloud);
00236     registered_ros_cloud.header.stamp = ros::Time::now();
00237     registered_ros_cloud.header.frame_id = frame_id_;
00238     pub_registered_.publish(registered_ros_cloud);
00239     pcl::toROSMsg(*non_registered_cloud, nonregistered_ros_cloud);
00240     nonregistered_ros_cloud.header.stamp = ros::Time::now();
00241     nonregistered_ros_cloud.header.frame_id = frame_id_;
00242     pub_cloud_non_registered_.publish(nonregistered_ros_cloud);
00243   }
00244   
00245 }
00246 
00247 #include <pluginlib/class_list_macros.h>
00248 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::IncrementalModelRegistration, nodelet::Nodelet);


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Sun Oct 8 2017 02:43:49