#include <icp_registration.h>

Inheritance diagram for jsk_pcl_ros::ICPRegistration:

Public Types
typedef jsk_pcl_ros::ICPRegistrationConfig	Config

typedef message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, geometry_msgs::PoseStamped >	OffsetSyncPolicy

typedef pcl::PointXYZRGBNormal	PointT

typedef message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, sensor_msgs::PointCloud2 >	ReferenceSyncPolicy

typedef message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, jsk_recognition_msgs::BoundingBox >	SyncPolicy

Public Member Functions
	ICPRegistration ()

Public Member Functions inherited from jsk_topic_tools::ConnectionBasedNodelet
	ConnectionBasedNodelet ()

Public Member Functions inherited from nodelet::Nodelet
void	init (const std::string &name, const M_string &remapping_args, const V_string &my_argv, ros::CallbackQueueInterface st_queue=NULL, ros::CallbackQueueInterface mt_queue=NULL)

	Nodelet ()

virtual	~Nodelet ()

Protected Member Functions
virtual void	align (const sensor_msgs::PointCloud2::ConstPtr &msg)

virtual void	align (const sensor_msgs::PointCloud2::ConstPtr &msg, const sensor_msgs::PointCloud2::ConstPtr &reference_msg)

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)

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 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)

virtual jsk_recognition_msgs::ICPResult	alignPointcloudWithReferences (pcl::PointCloud< PointT >::Ptr &cloud, const Eigen::Affine3f &offset, const std_msgs::Header &header)

virtual bool	alignService (jsk_recognition_msgs::ICPAlign::Request &req, jsk_recognition_msgs::ICPAlign::Response &res)

virtual void	alignWithBox (const sensor_msgs::PointCloud2::ConstPtr &msg, const jsk_recognition_msgs::BoundingBox::ConstPtr &box_msg)

virtual bool	alignWithBoxService (jsk_recognition_msgs::ICPAlignWithBox::Request &req, jsk_recognition_msgs::ICPAlignWithBox::Response &res)

virtual void	alignWithOffset (const sensor_msgs::PointCloud2::ConstPtr &msg, const geometry_msgs::PoseStamped::ConstPtr &pose_msg)

virtual void	cameraInfoCallback (const sensor_msgs::CameraInfo::ConstPtr &msg)

virtual void	configCallback (Config &config, uint32_t level)

virtual void	onInit ()

virtual void	publishDebugCloud (ros::Publisher &pub, const pcl::PointCloud< PointT > &cloud, const std_msgs::Header &header)

virtual void	referenceAddCallback (const sensor_msgs::PointCloud2::ConstPtr &msg)

virtual void	referenceArrayCallback (const jsk_recognition_msgs::PointsArray::ConstPtr &msg)

virtual void	referenceCallback (const sensor_msgs::PointCloud2::ConstPtr &msg)

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	subscribe ()

virtual void	unsubscribe ()

Protected Member Functions inherited from jsk_topic_tools::ConnectionBasedNodelet
ros::Publisher	advertise (ros::NodeHandle &nh, std::string topic, int queue_size)

image_transport::CameraPublisher	advertiseCamera (ros::NodeHandle &nh, image_transport::ImageTransport &it, const std::string &topic, int queue_size)

image_transport::CameraPublisher	advertiseCamera (ros::NodeHandle &nh, const std::string &topic, int queue_size)

image_transport::Publisher	advertiseImage (ros::NodeHandle &nh, image_transport::ImageTransport &it, const std::string &topic, int queue_size)

image_transport::Publisher	advertiseImage (ros::NodeHandle &nh, const std::string &topic, int queue_size)

virtual void	cameraConnectionBaseCallback ()

virtual void	cameraConnectionCallback (const image_transport::SingleSubscriberPublisher &pub)

virtual void	cameraInfoConnectionCallback (const ros::SingleSubscriberPublisher &pub)

virtual void	connectionCallback (const ros::SingleSubscriberPublisher &pub)

virtual void	imageConnectionCallback (const image_transport::SingleSubscriberPublisher &pub)

virtual bool	isSubscribed ()

virtual void	onInitPostProcess ()

virtual void	warnNeverSubscribedCallback (const ros::WallTimerEvent &event)

virtual void	warnOnInitPostProcessCalledCallback (const ros::WallTimerEvent &event)

Protected Member Functions inherited from nodelet::Nodelet
ros::CallbackQueueInterface &	getMTCallbackQueue () const

ros::NodeHandle &	getMTNodeHandle () const

ros::NodeHandle &	getMTPrivateNodeHandle () const

const V_string &	getMyArgv () const

const std::string &	getName () const

ros::NodeHandle &	getNodeHandle () const

ros::NodeHandle &	getPrivateNodeHandle () const

const M_string &	getRemappingArgs () const

ros::CallbackQueueInterface &	getSTCallbackQueue () const

std::string	getSuffixedName (const std::string &suffix) const

Protected Attributes
int	algorithm_

bool	align_box_

sensor_msgs::CameraInfo::ConstPtr	camera_info_msg_

int	correspondence_algorithm_

double	correspondence_distance_

int	correspondence_randomness_

bool	done_init_

double	euclidean_fittness_epsilon_

int	max_iteration_

int	maximum_optimizer_iterations_

boost::mutex	mutex_

double	ndt_outlier_ratio_

double	ndt_resolution_

double	ndt_step_size_

ros::Publisher	pub_average_time_

ros::Publisher	pub_debug_flipped_cloud_

ros::Publisher	pub_debug_result_cloud_

ros::Publisher	pub_debug_source_cloud_

ros::Publisher	pub_debug_target_cloud_

ros::Publisher	pub_icp_result

ros::Publisher	pub_latest_time_

ros::Publisher	pub_result_cloud_

ros::Publisher	pub_result_pose_

double	ransac_iterations_

double	ransac_outlier_threshold_

std::vector< pcl::PointCloud< PointT >::Ptr >	reference_cloud_list_

double	rotation_epsilon_

boost::shared_ptr< dynamic_reconfigure::Server< Config > >	srv_

ros::ServiceServer	srv_icp_align_

ros::ServiceServer	srv_icp_align_with_box_

ros::Subscriber	sub_

message_filters::Subscriber< jsk_recognition_msgs::BoundingBox >	sub_box_

ros::Subscriber	sub_camera_info_

message_filters::Subscriber< sensor_msgs::PointCloud2 >	sub_input_

message_filters::Subscriber< geometry_msgs::PoseStamped >	sub_offset_

ros::Subscriber	sub_reference_

ros::Subscriber	sub_reference_add

ros::Subscriber	sub_reference_array_

message_filters::Subscriber< sensor_msgs::PointCloud2 >	sub_sync_input_

message_filters::Subscriber< sensor_msgs::PointCloud2 >	sub_sync_reference_

boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > >	sync_

boost::shared_ptr< message_filters::Synchronizer< OffsetSyncPolicy > >	sync_offset_

boost::shared_ptr< message_filters::Synchronizer< ReferenceSyncPolicy > >	sync_reference_

bool	synchronize_reference_

tf::TransformListener *	tf_listener_

jsk_recognition_utils::WallDurationTimer	timer_

bool	transform_3dof_
	set via ~transform_3dof parameter. default is false. More...

double	transform_epsilon_

bool	use_flipped_initial_pose_

bool	use_normal_
	Store value of ~use_normal. If this parameter is true, ICPRegistration nodelet expects reference and input pointcloud have normal_x, normal_y and normal_z fields. More...

bool	use_offset_pose_
	set via ~use_offset_pose parameter. default is false. More...

Protected Attributes inherited from jsk_topic_tools::ConnectionBasedNodelet
bool	always_subscribe_

std::vector< image_transport::CameraPublisher >	camera_publishers_

boost::mutex	connection_mutex_

ConnectionStatus	connection_status_

bool	ever_subscribed_

std::vector< image_transport::Publisher >	image_publishers_

boost::shared_ptr< ros::NodeHandle >	nh_

bool	on_init_post_process_called_

boost::shared_ptr< ros::NodeHandle >	pnh_

std::vector< ros::Publisher >	publishers_

bool	subscribed_

ros::WallTimer	timer_warn_never_subscribed_

ros::WallTimer	timer_warn_on_init_post_process_called_

bool	verbose_connection_

Detailed Description

Definition at line 59 of file icp_registration.h.

Member Typedef Documentation

typedef jsk_pcl_ros::ICPRegistrationConfig jsk_pcl_ros::ICPRegistration::Config

Definition at line 63 of file icp_registration.h.

typedef message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, geometry_msgs::PoseStamped > jsk_pcl_ros::ICPRegistration::OffsetSyncPolicy

Definition at line 69 of file icp_registration.h.

typedef pcl::PointXYZRGBNormal jsk_pcl_ros::ICPRegistration::PointT

Definition at line 62 of file icp_registration.h.

typedef message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, sensor_msgs::PointCloud2 > jsk_pcl_ros::ICPRegistration::ReferenceSyncPolicy

Definition at line 73 of file icp_registration.h.

typedef message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, jsk_recognition_msgs::BoundingBox > jsk_pcl_ros::ICPRegistration::SyncPolicy

Definition at line 66 of file icp_registration.h.

Constructor & Destructor Documentation

jsk_pcl_ros::ICPRegistration::ICPRegistration ( )

inline

Definition at line 74 of file icp_registration.h.

Member Function Documentation

void jsk_pcl_ros::ICPRegistration::align ( const sensor_msgs::PointCloud2::ConstPtr & msg )

protectedvirtual

Definition at line 375 of file icp_registration_nodelet.cpp.

void jsk_pcl_ros::ICPRegistration::align	(	const sensor_msgs::PointCloud2::ConstPtr &	msg,
		const sensor_msgs::PointCloud2::ConstPtr &	reference_msg
	)

protectedvirtual

Definition at line 402 of file icp_registration_nodelet.cpp.

double jsk_pcl_ros::ICPRegistration::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
	)

protectedvirtual

Definition at line 517 of file icp_registration_nodelet.cpp.

double jsk_pcl_ros::ICPRegistration::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
	)

protectedvirtual

Definition at line 564 of file icp_registration_nodelet.cpp.

double jsk_pcl_ros::ICPRegistration::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
	)

protectedvirtual

Definition at line 533 of file icp_registration_nodelet.cpp.

jsk_recognition_msgs::ICPResult jsk_pcl_ros::ICPRegistration::alignPointcloudWithReferences	(	pcl::PointCloud< PointT >::Ptr &	cloud,
		const Eigen::Affine3f &	offset,
		const std_msgs::Header &	header
	)

protectedvirtual

Definition at line 427 of file icp_registration_nodelet.cpp.

bool jsk_pcl_ros::ICPRegistration::alignService	(	jsk_recognition_msgs::ICPAlign::Request &	req,
		jsk_recognition_msgs::ICPAlign::Response &	res
	)

protectedvirtual

Definition at line 235 of file icp_registration_nodelet.cpp.

void jsk_pcl_ros::ICPRegistration::alignWithBox	(	const sensor_msgs::PointCloud2::ConstPtr &	msg,
		const jsk_recognition_msgs::BoundingBox::ConstPtr &	box_msg
	)

protectedvirtual

Definition at line 284 of file icp_registration_nodelet.cpp.

bool jsk_pcl_ros::ICPRegistration::alignWithBoxService	(	jsk_recognition_msgs::ICPAlignWithBox::Request &	req,
		jsk_recognition_msgs::ICPAlignWithBox::Response &	res
	)

protectedvirtual

Definition at line 202 of file icp_registration_nodelet.cpp.

void jsk_pcl_ros::ICPRegistration::alignWithOffset	(	const sensor_msgs::PointCloud2::ConstPtr &	msg,
		const geometry_msgs::PoseStamped::ConstPtr &	pose_msg
	)

protectedvirtual

Definition at line 325 of file icp_registration_nodelet.cpp.

void jsk_pcl_ros::ICPRegistration::cameraInfoCallback ( const sensor_msgs::CameraInfo::ConstPtr & msg )

protectedvirtual

Definition at line 510 of file icp_registration_nodelet.cpp.

void jsk_pcl_ros::ICPRegistration::configCallback	(	Config &	config,
		uint32_t	level
	)

protectedvirtual

Definition at line 182 of file icp_registration_nodelet.cpp.

void jsk_pcl_ros::ICPRegistration::onInit ( void )

protectedvirtual

Reimplemented from jsk_topic_tools::ConnectionBasedNodelet.

Definition at line 54 of file icp_registration_nodelet.cpp.

void jsk_pcl_ros::ICPRegistration::publishDebugCloud	(	ros::Publisher &	pub,
		const pcl::PointCloud< PointT > &	cloud,
		const std_msgs::Header &	header
	)

protectedvirtual

Definition at line 169 of file icp_registration_nodelet.cpp.

void jsk_pcl_ros::ICPRegistration::referenceAddCallback ( const sensor_msgs::PointCloud2::ConstPtr & msg )

protectedvirtual

Definition at line 726 of file icp_registration_nodelet.cpp.

void jsk_pcl_ros::ICPRegistration::referenceArrayCallback ( const jsk_recognition_msgs::PointsArray::ConstPtr & msg )

protectedvirtual

Definition at line 710 of file icp_registration_nodelet.cpp.

void jsk_pcl_ros::ICPRegistration::referenceCallback ( const sensor_msgs::PointCloud2::ConstPtr & msg )

protectedvirtual

Definition at line 689 of file icp_registration_nodelet.cpp.

double jsk_pcl_ros::ICPRegistration::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
	)

protectedvirtual

Definition at line 645 of file icp_registration_nodelet.cpp.

void jsk_pcl_ros::ICPRegistration::subscribe ( )

protectedvirtual

Implements jsk_topic_tools::ConnectionBasedNodelet.

Definition at line 111 of file icp_registration_nodelet.cpp.

void jsk_pcl_ros::ICPRegistration::unsubscribe ( )

protectedvirtual

Implements jsk_topic_tools::ConnectionBasedNodelet.

Definition at line 151 of file icp_registration_nodelet.cpp.

Member Data Documentation

int jsk_pcl_ros::ICPRegistration::algorithm_

protected

Definition at line 196 of file icp_registration.h.

bool jsk_pcl_ros::ICPRegistration::align_box_

protected

Definition at line 162 of file icp_registration.h.

sensor_msgs::CameraInfo::ConstPtr jsk_pcl_ros::ICPRegistration::camera_info_msg_

protected

Definition at line 205 of file icp_registration.h.

int jsk_pcl_ros::ICPRegistration::correspondence_algorithm_

protected

Definition at line 197 of file icp_registration.h.

double jsk_pcl_ros::ICPRegistration::correspondence_distance_

protected

Definition at line 200 of file icp_registration.h.

int jsk_pcl_ros::ICPRegistration::correspondence_randomness_

protected

Definition at line 211 of file icp_registration.h.

bool jsk_pcl_ros::ICPRegistration::done_init_

protected

Definition at line 221 of file icp_registration.h.

double jsk_pcl_ros::ICPRegistration::euclidean_fittness_epsilon_

protected

Definition at line 202 of file icp_registration.h.

int jsk_pcl_ros::ICPRegistration::max_iteration_

protected

Definition at line 199 of file icp_registration.h.

int jsk_pcl_ros::ICPRegistration::maximum_optimizer_iterations_

protected

Definition at line 212 of file icp_registration.h.

boost::mutex jsk_pcl_ros::ICPRegistration::mutex_

protected

Definition at line 164 of file icp_registration.h.

double jsk_pcl_ros::ICPRegistration::ndt_outlier_ratio_

protected

Definition at line 219 of file icp_registration.h.

double jsk_pcl_ros::ICPRegistration::ndt_resolution_

protected

Definition at line 217 of file icp_registration.h.

double jsk_pcl_ros::ICPRegistration::ndt_step_size_

protected

Definition at line 218 of file icp_registration.h.

ros::Publisher jsk_pcl_ros::ICPRegistration::pub_average_time_

protected

Definition at line 153 of file icp_registration.h.

ros::Publisher jsk_pcl_ros::ICPRegistration::pub_debug_flipped_cloud_

protected

Definition at line 154 of file icp_registration.h.

ros::Publisher jsk_pcl_ros::ICPRegistration::pub_debug_result_cloud_

protected

Definition at line 154 of file icp_registration.h.

ros::Publisher jsk_pcl_ros::ICPRegistration::pub_debug_source_cloud_

protected

Definition at line 154 of file icp_registration.h.

ros::Publisher jsk_pcl_ros::ICPRegistration::pub_debug_target_cloud_

protected

Definition at line 154 of file icp_registration.h.

ros::Publisher jsk_pcl_ros::ICPRegistration::pub_icp_result

protected

Definition at line 158 of file icp_registration.h.

ros::Publisher jsk_pcl_ros::ICPRegistration::pub_latest_time_

protected

Definition at line 152 of file icp_registration.h.

ros::Publisher jsk_pcl_ros::ICPRegistration::pub_result_cloud_

protected

Definition at line 151 of file icp_registration.h.

ros::Publisher jsk_pcl_ros::ICPRegistration::pub_result_pose_

protected

Definition at line 150 of file icp_registration.h.

double jsk_pcl_ros::ICPRegistration::ransac_iterations_

protected

Definition at line 203 of file icp_registration.h.

double jsk_pcl_ros::ICPRegistration::ransac_outlier_threshold_

protected

Definition at line 204 of file icp_registration.h.

std::vector<pcl::PointCloud<PointT>::Ptr> jsk_pcl_ros::ICPRegistration::reference_cloud_list_

protected

Definition at line 198 of file icp_registration.h.

double jsk_pcl_ros::ICPRegistration::rotation_epsilon_

protected

Definition at line 210 of file icp_registration.h.

boost::shared_ptr<dynamic_reconfigure::Server<Config> > jsk_pcl_ros::ICPRegistration::srv_

protected

Definition at line 163 of file icp_registration.h.

ros::ServiceServer jsk_pcl_ros::ICPRegistration::srv_icp_align_

protected

Definition at line 161 of file icp_registration.h.

ros::ServiceServer jsk_pcl_ros::ICPRegistration::srv_icp_align_with_box_

protected

Definition at line 160 of file icp_registration.h.

ros::Subscriber jsk_pcl_ros::ICPRegistration::sub_

protected

Definition at line 144 of file icp_registration.h.

message_filters::Subscriber<jsk_recognition_msgs::BoundingBox> jsk_pcl_ros::ICPRegistration::sub_box_

protected

Definition at line 166 of file icp_registration.h.

ros::Subscriber jsk_pcl_ros::ICPRegistration::sub_camera_info_

protected

Definition at line 143 of file icp_registration.h.

message_filters::Subscriber<sensor_msgs::PointCloud2> jsk_pcl_ros::ICPRegistration::sub_input_

protected

Definition at line 165 of file icp_registration.h.

message_filters::Subscriber<geometry_msgs::PoseStamped> jsk_pcl_ros::ICPRegistration::sub_offset_

protected

Definition at line 167 of file icp_registration.h.

ros::Subscriber jsk_pcl_ros::ICPRegistration::sub_reference_

protected

Definition at line 145 of file icp_registration.h.

ros::Subscriber jsk_pcl_ros::ICPRegistration::sub_reference_add

protected

Definition at line 146 of file icp_registration.h.

ros::Subscriber jsk_pcl_ros::ICPRegistration::sub_reference_array_

protected

Definition at line 147 of file icp_registration.h.

message_filters::Subscriber<sensor_msgs::PointCloud2> jsk_pcl_ros::ICPRegistration::sub_sync_input_

protected

Definition at line 148 of file icp_registration.h.

message_filters::Subscriber<sensor_msgs::PointCloud2> jsk_pcl_ros::ICPRegistration::sub_sync_reference_

protected

Definition at line 149 of file icp_registration.h.

boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> > jsk_pcl_ros::ICPRegistration::sync_

protected

Definition at line 168 of file icp_registration.h.

boost::shared_ptr<message_filters::Synchronizer<OffsetSyncPolicy> > jsk_pcl_ros::ICPRegistration::sync_offset_

protected

Definition at line 169 of file icp_registration.h.

boost::shared_ptr<message_filters::Synchronizer<ReferenceSyncPolicy> > jsk_pcl_ros::ICPRegistration::sync_reference_

protected

Definition at line 170 of file icp_registration.h.

bool jsk_pcl_ros::ICPRegistration::synchronize_reference_

protected

Definition at line 194 of file icp_registration.h.

tf::TransformListener* jsk_pcl_ros::ICPRegistration::tf_listener_

protected

Definition at line 171 of file icp_registration.h.

jsk_recognition_utils::WallDurationTimer jsk_pcl_ros::ICPRegistration::timer_

protected

Definition at line 159 of file icp_registration.h.

bool jsk_pcl_ros::ICPRegistration::transform_3dof_

protected

set via ~transform_3dof parameter. default is false.

Definition at line 177 of file icp_registration.h.

double jsk_pcl_ros::ICPRegistration::transform_epsilon_

protected

Definition at line 201 of file icp_registration.h.

bool jsk_pcl_ros::ICPRegistration::use_flipped_initial_pose_

protected

Definition at line 195 of file icp_registration.h.

bool jsk_pcl_ros::ICPRegistration::use_normal_

protected

Store value of ~use_normal. If this parameter is true, ICPRegistration nodelet expects reference and input pointcloud have normal_x, normal_y and normal_z fields.

Definition at line 189 of file icp_registration.h.

bool jsk_pcl_ros::ICPRegistration::use_offset_pose_

protected

set via ~use_offset_pose parameter. default is false.

Definition at line 182 of file icp_registration.h.

The documentation for this class was generated from the following files:

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Member Typedef Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation