37 #ifndef JSK_PCL_ROS_INCREMENTAL_MODEL_REGISTRATION_H_ 38 #define JSK_PCL_ROS_INCREMENTAL_MODEL_REGISTRATION_H_ 40 #include <geometry_msgs/PoseStamped.h> 41 #include <sensor_msgs/PointCloud.h> 47 #include <pcl/visualization/pcl_visualizer.h> 48 #include <std_srvs/Empty.h> 58 const Eigen::Affine3f& original_pose);
64 pcl::PointCloud<pcl::PointXYZRGB> cloud);
81 sensor_msgs::PointCloud2,
82 pcl_msgs::PointIndices,
88 virtual void onInit();
93 virtual void newsampleCallback(
94 const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
95 const pcl_msgs::PointIndices::ConstPtr& indices_msg,
96 const geometry_msgs::PoseStamped::ConstPtr& pose_msg);
97 virtual void transformPointCloudRepsectedToPose(
98 pcl::PointCloud<pcl::PointXYZRGB>::Ptr input,
99 pcl::PointCloud<pcl::PointXYZRGB>::Ptr
output,
100 const geometry_msgs::PoseStamped::ConstPtr& pose_msg);
101 virtual bool startRegistration(
102 std_srvs::Empty::Request&
req,
103 std_srvs::Empty::Response&
res);
104 virtual void callICP(
105 pcl::PointCloud<pcl::PointXYZRGB>::Ptr reference,
106 pcl::PointCloud<pcl::PointXYZRGB>::Ptr target,
107 Eigen::Affine3f& output_transform);
124 std::vector<CapturedSamplePointCloud::Ptr>
samples_;
pcl::PointCloud< pcl::PointXYZRGB >::Ptr original_cloud_
message_filters::Subscriber< pcl_msgs::PointIndices > sub_indices_
virtual void setRefinedPointCloud(pcl::PointCloud< pcl::PointXYZRGB > cloud)
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_cloud_
virtual Eigen::Affine3f getRefinedPose()
Eigen::Affine3f original_pose_
pcl::PointCloud< pcl::PointXYZRGB > all_cloud_
virtual void updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat)
ros::Publisher pub_cloud_non_registered_
virtual pcl::PointCloud< pcl::PointXYZRGB >::Ptr getOriginalPointCloud()
CapturedSamplePointCloud()
virtual void unsubscribe()
void output(int index, double value)
ros::ServiceServer start_registration_srv_
message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, pcl_msgs::PointIndices, geometry_msgs::PoseStamped > SyncPolicy
boost::shared_ptr< pcl::visualization::PCLVisualizer > viewer_
IncrementalModelRegistration()
virtual pcl::PointCloud< pcl::PointXYZRGB >::Ptr getRefinedPointCloud()
pcl::PointCloud< pcl::PointXYZRGB >::Ptr refined_cloud_
message_filters::Subscriber< geometry_msgs::PoseStamped > sub_pose_
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
boost::shared_ptr< CapturedSamplePointCloud > Ptr
boost::mutex mutex
global mutex.
virtual Eigen::Affine3f getOriginalPose()
virtual void setRefinedPose(Eigen::Affine3f pose)
std::vector< CapturedSamplePointCloud::Ptr > samples_
ros::Publisher pub_registered_
Eigen::Affine3f refined_pose_