37 #ifndef JSK_PCL_ROS_ICP_REGISTRATION_H_ 38 #define JSK_PCL_ROS_ICP_REGISTRATION_H_ 41 #include <dynamic_reconfigure/server.h> 42 #include <jsk_pcl_ros/ICPRegistrationConfig.h> 43 #include <jsk_recognition_msgs/BoundingBox.h> 44 #include <jsk_recognition_msgs/ICPAlignWithBox.h> 45 #include <jsk_recognition_msgs/ICPAlign.h> 46 #include <jsk_recognition_msgs/ICPResult.h> 52 #include <jsk_recognition_msgs/PointsArray.h> 53 #include <sensor_msgs/CameraInfo.h> 55 #include <std_msgs/Float32.h> 62 typedef pcl::PointXYZRGBNormal
PointT;
63 typedef jsk_pcl_ros::ICPRegistrationConfig
Config;
65 sensor_msgs::PointCloud2,
68 sensor_msgs::PointCloud2,
71 sensor_msgs::PointCloud2,
72 sensor_msgs::PointCloud2
80 virtual void align(
const sensor_msgs::PointCloud2::ConstPtr&
msg);
81 virtual void align(
const sensor_msgs::PointCloud2::ConstPtr& msg,
82 const sensor_msgs::PointCloud2::ConstPtr& reference_msg);
84 const sensor_msgs::PointCloud2::ConstPtr& msg,
85 const jsk_recognition_msgs::BoundingBox::ConstPtr&
box_msg);
87 const sensor_msgs::PointCloud2::ConstPtr& msg,
88 const geometry_msgs::PoseStamped::ConstPtr& pose_msg);
90 jsk_recognition_msgs::ICPAlignWithBox::Request&
req,
91 jsk_recognition_msgs::ICPAlignWithBox::Response&
res);
93 jsk_recognition_msgs::ICPAlign::Request& req,
94 jsk_recognition_msgs::ICPAlign::Response& res);
96 const sensor_msgs::PointCloud2::ConstPtr& msg);
98 const jsk_recognition_msgs::PointsArray::ConstPtr& msg);
100 const sensor_msgs::PointCloud2::ConstPtr& msg);
104 const pcl::PointCloud<PointT>&
cloud,
105 const std_msgs::Header&
header);
107 pcl::PointCloud<PointT>::Ptr& cloud,
108 pcl::PointCloud<PointT>::Ptr& reference,
109 const Eigen::Affine3f& offset,
110 pcl::PointCloud<PointT>::Ptr& output_cloud,
111 Eigen::Affine3d& output_transform);
113 pcl::PointCloud<PointT>::Ptr& cloud,
114 pcl::PointCloud<PointT>::Ptr& reference,
115 const Eigen::Affine3f& offset,
116 pcl::PointCloud<PointT>::Ptr& output_cloud,
117 Eigen::Affine3d& output_transform);
119 pcl::PointCloud<PointT>::Ptr& cloud,
120 pcl::PointCloud<PointT>::Ptr& reference,
121 const Eigen::Affine3f& offset,
122 pcl::PointCloud<PointT>::Ptr& output_cloud,
123 Eigen::Affine3d& output_transform);
125 pcl::PointCloud<PointT>::Ptr& cloud,
126 const Eigen::Affine3f& offset,
127 const std_msgs::Header& header);
129 pcl::PointCloud<PointT>::Ptr& cloud,
130 pcl::PointCloud<PointT>::Ptr& reference,
131 const Eigen::Affine3f& offset,
132 Eigen::Affine3f& offset_result,
133 pcl::PointCloud<PointT>::Ptr transformed_cloud,
134 Eigen::Affine3d& transform_result);
136 const sensor_msgs::CameraInfo::ConstPtr& msg);
jsk_recognition_msgs::BoundingBoxArray::ConstPtr box_msg
virtual double alignPointcloudWithICP(pcl::PointCloud< PointT >::Ptr &cloud, pcl::PointCloud< PointT >::Ptr &reference, const Eigen::Affine3f &offset, pcl::PointCloud< PointT >::Ptr &output_cloud, Eigen::Affine3d &output_transform)
virtual bool alignService(jsk_recognition_msgs::ICPAlign::Request &req, jsk_recognition_msgs::ICPAlign::Response &res)
bool use_normal_
Store value of ~use_normal. If this parameter is true, ICPRegistration nodelet expects reference and ...
ros::Subscriber sub_reference_array_
message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, sensor_msgs::PointCloud2 > ReferenceSyncPolicy
virtual double scorePointcloudAlignment(pcl::PointCloud< PointT >::Ptr &cloud, pcl::PointCloud< PointT >::Ptr &reference, const Eigen::Affine3f &offset, Eigen::Affine3f &offset_result, pcl::PointCloud< PointT >::Ptr transformed_cloud, Eigen::Affine3d &transform_result)
virtual void publishDebugCloud(ros::Publisher &pub, const pcl::PointCloud< PointT > &cloud, const std_msgs::Header &header)
ros::Publisher pub_debug_flipped_cloud_
double transform_epsilon_
ros::Publisher pub_icp_result
message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, jsk_recognition_msgs::BoundingBox > SyncPolicy
virtual void alignWithOffset(const sensor_msgs::PointCloud2::ConstPtr &msg, const geometry_msgs::PoseStamped::ConstPtr &pose_msg)
virtual void unsubscribe()
ros::Publisher pub_result_pose_
tf::TransformListener * tf_listener_
ros::Publisher pub_debug_target_cloud_
virtual void align(const sensor_msgs::PointCloud2::ConstPtr &msg)
bool synchronize_reference_
ros::Publisher pub_result_cloud_
ros::ServiceServer srv_icp_align_
ros::Publisher pub_average_time_
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
ros::Publisher pub_debug_result_cloud_
ros::Subscriber sub_camera_info_
sensor_msgs::CameraInfo::ConstPtr camera_info_msg_
double ndt_outlier_ratio_
virtual void cameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr &msg)
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_input_
boost::shared_ptr< message_filters::Synchronizer< ReferenceSyncPolicy > > sync_reference_
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
message_filters::Subscriber< jsk_recognition_msgs::BoundingBox > sub_box_
pcl::PointXYZRGBNormal PointT
jsk_recognition_utils::WallDurationTimer timer_
ros::Subscriber sub_reference_
virtual void referenceCallback(const sensor_msgs::PointCloud2::ConstPtr &msg)
int maximum_optimizer_iterations_
double euclidean_fittness_epsilon_
virtual double alignPointcloudWithNDT(pcl::PointCloud< PointT >::Ptr &cloud, pcl::PointCloud< PointT >::Ptr &reference, const Eigen::Affine3f &offset, pcl::PointCloud< PointT >::Ptr &output_cloud, Eigen::Affine3d &output_transform)
message_filters::Subscriber< geometry_msgs::PoseStamped > sub_offset_
int correspondence_randomness_
virtual void configCallback(Config &config, uint32_t level)
boost::mutex mutex
global mutex.
ros::Publisher pub_debug_source_cloud_
virtual jsk_recognition_msgs::ICPResult alignPointcloudWithReferences(pcl::PointCloud< PointT >::Ptr &cloud, const Eigen::Affine3f &offset, const std_msgs::Header &header)
virtual void referenceArrayCallback(const jsk_recognition_msgs::PointsArray::ConstPtr &msg)
virtual void referenceAddCallback(const sensor_msgs::PointCloud2::ConstPtr &msg)
std::vector< pcl::PointCloud< PointT >::Ptr > reference_cloud_list_
double correspondence_distance_
bool use_offset_pose_
set via ~use_offset_pose parameter. default is false.
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_sync_input_
boost::shared_ptr< message_filters::Synchronizer< OffsetSyncPolicy > > sync_offset_
bool use_flipped_initial_pose_
double ransac_iterations_
double ransac_outlier_threshold_
int correspondence_algorithm_
virtual bool alignWithBoxService(jsk_recognition_msgs::ICPAlignWithBox::Request &req, jsk_recognition_msgs::ICPAlignWithBox::Response &res)
ros::ServiceServer srv_icp_align_with_box_
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_sync_reference_
ros::Subscriber sub_reference_add
bool transform_3dof_
set via ~transform_3dof parameter. default is false.
virtual void alignWithBox(const sensor_msgs::PointCloud2::ConstPtr &msg, const jsk_recognition_msgs::BoundingBox::ConstPtr &box_msg)
message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, geometry_msgs::PoseStamped > OffsetSyncPolicy
jsk_pcl_ros::ICPRegistrationConfig Config
ros::Publisher pub_latest_time_
virtual double alignPointcloud(pcl::PointCloud< PointT >::Ptr &cloud, pcl::PointCloud< PointT >::Ptr &reference, const Eigen::Affine3f &offset, pcl::PointCloud< PointT >::Ptr &output_cloud, Eigen::Affine3d &output_transform)