#include <target_adaptive_tracking.h>

Inheritance diagram for jsk_pcl_ros::TargetAdaptiveTracking:

Classes
struct	AdjacentInfo
struct	ReferenceModel
Public Member Functions
void	backgroundReferenceLikelihood (const ModelsPtr, const ModelsPtr, std::map< uint32_t, float >)
virtual void	callback (const sensor_msgs::PointCloud2::ConstPtr &, const geometry_msgs::PoseStamped::ConstPtr &)
Eigen::Vector4f	cloudMeanNormal (const pcl::PointCloud< pcl::Normal >::Ptr, bool=true)
virtual std::vector < pcl::PointIndices::Ptr >	clusterPointIndicesToPointIndices (const jsk_recognition_msgs::ClusterPointIndicesConstPtr &)
void	compute3DCentroids (const pcl::PointCloud< PointT >::Ptr, Eigen::Vector4f &) const
void	computeCloudClusterRPYHistogram (const pcl::PointCloud< PointT >::Ptr, const pcl::PointCloud< pcl::Normal >::Ptr, cv::Mat &) const
float	computeCoherency (const float, const float)
void	computeColorHistogram (const pcl::PointCloud< PointT >::Ptr, cv::Mat &, const int=16, const int=16, bool=true) const
void	computeLocalPairwiseFeautures (const std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr > &, const std::map< uint32_t, std::vector< uint32_t > > &, cv::Mat &, const int=3)
void	computePointFPFH (const pcl::PointCloud< PointT >::Ptr, pcl::PointCloud< pcl::Normal >::Ptr, cv::Mat &, bool=true) const
void	computeScatterMatrix (const pcl::PointCloud< PointT >::Ptr, const Eigen::Vector4f)
pcl::PointXYZRGBNormal	convertVector4fToPointXyzRgbNormal (const Eigen::Vector4f &, const Eigen::Vector4f &, const cv::Scalar)
void	estimatedPFPose (const geometry_msgs::PoseStamped::ConstPtr &, PointXYZRPY &)
template<class T >
void	estimatePointCloudNormals (const pcl::PointCloud< PointT >::Ptr, pcl::PointCloud< pcl::Normal >::Ptr, T=0.05f, bool=false) const
void	filterCloudForBoundingBoxViz (pcl::PointCloud< PointT >::Ptr, const ModelsPtr, const float=0.6f)
bool	filterPointCloud (pcl::PointCloud< PointT >::Ptr, const Eigen::Vector4f, const ModelsPtr, const float)
template<typename T >
void	getRotationMatrixFromRPY (const PointXYZRPY &, Eigen::Matrix< T, 3, 3 > &)
template<class T >
T	localVoxelConvexityLikelihood (Eigen::Vector4f, Eigen::Vector4f, Eigen::Vector4f, Eigen::Vector4f)
virtual void	objInitCallback (const sensor_msgs::PointCloud2::ConstPtr &, const sensor_msgs::PointCloud2::ConstPtr &, const geometry_msgs::PoseStamped::ConstPtr &)
template<typename T , typename U , typename V >
cv::Scalar	plotJetColour (T, U, V)
void	processInitCloud (const pcl::PointCloud< PointT >::Ptr, ModelsPtr)
void	processVoxelForReferenceModel (const std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr >, const std::multimap< uint32_t, uint32_t >, const uint32_t, TargetAdaptiveTracking::ReferenceModel *)
void	publishSupervoxel (const std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr >, sensor_msgs::PointCloud2 &, jsk_recognition_msgs::ClusterPointIndices &, const std_msgs::Header &)
void	supervoxelSegmentation (const pcl::PointCloud< PointT >::Ptr, std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr > &, std::multimap< uint32_t, uint32_t > &)
void	supervoxelSegmentation (const pcl::PointCloud< PointT >::Ptr, std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr > &, std::multimap< uint32_t, uint32_t > &, const float)
	TargetAdaptiveTracking ()
template<class T >
T	targetCandidateToReferenceLikelihood (const ReferenceModel &, const pcl::PointCloud< PointT >::Ptr, const pcl::PointCloud< pcl::Normal >::Ptr, const Eigen::Vector4f &, ReferenceModel *=NULL)
void	targetDescriptiveSurfelsEstimationAndUpdate (pcl::PointCloud< PointT >::Ptr, const Eigen::Affine3f &, const TargetAdaptiveTracking::PointXYZRPY &, const std_msgs::Header)
void	targetDescriptiveSurfelsIndices (const jsk_recognition_msgs::ClusterPointIndices &, const std::vector< uint32_t > &, jsk_recognition_msgs::ClusterPointIndices &)
float	templateCloudFilterLenght (const pcl::PointCloud< PointT >::Ptr)
void	transformModelPrimitives (const ModelsPtr &, ModelsPtr, const Eigen::Affine3f &)
void	voxelizeAndProcessPointCloud (const pcl::PointCloud< PointT >::Ptr cloud, const std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr > &, const std::multimap< uint32_t, uint32_t > &, std::vector< AdjacentInfo > &, ModelsPtr &, bool=true, bool=true, bool=true, bool=false)
Protected Member Functions
virtual void	onInit ()
virtual void	subscribe ()
virtual void	unsubscribe ()
virtual void	updateDiagnostic (diagnostic_updater::DiagnosticStatusWrapper &)
Private Types
typedef jsk_pcl_ros::TargetAdaptiveTrackingConfig	Config
typedef std::vector < ReferenceModel >	Models
typedef boost::shared_ptr< Models >	ModelsPtr
typedef std::vector< PointXYZRPY >	MotionHistory
typedef message_filters::sync_policies::ApproximateTime < sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, geometry_msgs::PoseStamped >	ObjectSyncPolicy
typedef pcl::PointXYZRGB	PointT
typedef pcl::tracking::ParticleXYZRPY	PointXYZRPY
typedef message_filters::sync_policies::ApproximateTime < sensor_msgs::PointCloud2, geometry_msgs::PoseStamped >	SyncPolicy
Private Member Functions
virtual void	configCallback (Config &, uint32_t)
Private Attributes
ModelsPtr	background_reference_
int	bin_size_
double	color_importance_
float	color_scaling_
Eigen::Vector4f	current_pose_
float	eps_distance_
int	eps_min_samples_
float	growth_rate_
int	history_window_size_
int	init_counter_
int	min_cluster_size_
MotionHistory	motion_history_
boost::mutex	mutex_
double	normal_importance_
boost::shared_ptr < message_filters::Synchronizer < ObjectSyncPolicy > >	obj_sync_
ModelsPtr	object_reference_
float	previous_distance_
Eigen::Vector4f	previous_pose_
pcl::PointCloud< PointT >::Ptr	previous_template_
tf::Transform	previous_transform_
ros::Publisher	pub_centroids_
ros::Publisher	pub_cloud_
ros::Publisher	pub_inliers_
ros::Publisher	pub_normal_
ros::Publisher	pub_pose_
ros::Publisher	pub_prob_
ros::Publisher	pub_scloud_
ros::Publisher	pub_sindices_
ros::Publisher	pub_tdp_
ros::Publisher	pub_templ_
double	seed_resolution_
double	spatial_importance_
boost::shared_ptr < dynamic_reconfigure::Server < Config > >	srv_
float	structure_scaling_
message_filters::Subscriber < sensor_msgs::PointCloud2 >	sub_bkgd_cloud_
message_filters::Subscriber < sensor_msgs::PointCloud2 >	sub_cloud_
message_filters::Subscriber < sensor_msgs::PointCloud2 >	sub_obj_cloud_
message_filters::Subscriber < geometry_msgs::PoseStamped >	sub_obj_pose_
message_filters::Subscriber < geometry_msgs::PoseStamped >	sub_pose_
boost::shared_ptr < message_filters::Synchronizer < SyncPolicy > >	sync_
float	threshold_
PointXYZRPY	tracker_pose_
int	update_counter_
bool	update_filter_template_
bool	update_tracker_reference_
bool	use_transform_
float	vfh_scaling_
double	voxel_resolution_

Detailed Description

Definition at line 64 of file target_adaptive_tracking.h.

Member Typedef Documentation

typedef jsk_pcl_ros::TargetAdaptiveTrackingConfig jsk_pcl_ros::TargetAdaptiveTracking::Config [private]

Definition at line 157 of file target_adaptive_tracking.h.

typedef std::vector<ReferenceModel> jsk_pcl_ros::TargetAdaptiveTracking::Models [private]

Definition at line 92 of file target_adaptive_tracking.h.

typedef boost::shared_ptr<Models> jsk_pcl_ros::TargetAdaptiveTracking::ModelsPtr [private]

Definition at line 93 of file target_adaptive_tracking.h.

typedef std::vector<PointXYZRPY> jsk_pcl_ros::TargetAdaptiveTracking::MotionHistory [private]

Definition at line 95 of file target_adaptive_tracking.h.

typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::PointCloud2, sensor_msgs::PointCloud2, geometry_msgs::PoseStamped> jsk_pcl_ros::TargetAdaptiveTracking::ObjectSyncPolicy [private]

Definition at line 105 of file target_adaptive_tracking.h.

typedef pcl::PointXYZRGB jsk_pcl_ros::TargetAdaptiveTracking::PointT [private]

Definition at line 66 of file target_adaptive_tracking.h.

typedef pcl::tracking::ParticleXYZRPY jsk_pcl_ros::TargetAdaptiveTracking::PointXYZRPY [private]

Definition at line 94 of file target_adaptive_tracking.h.

typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::PointCloud2, geometry_msgs::PoseStamped> jsk_pcl_ros::TargetAdaptiveTracking::SyncPolicy [private]

Definition at line 98 of file target_adaptive_tracking.h.

Constructor & Destructor Documentation

jsk_pcl_ros::TargetAdaptiveTracking::TargetAdaptiveTracking ( )

Definition at line 9 of file target_adaptive_tracking_nodelet.cpp.

Member Function Documentation

void jsk_pcl_ros::TargetAdaptiveTracking::backgroundReferenceLikelihood	(	const ModelsPtr	background_reference,
		const ModelsPtr	target_voxels,
		std::map< uint32_t, float >	max_prob
	)

Definition at line 1148 of file target_adaptive_tracking_nodelet.cpp.

void jsk_pcl_ros::TargetAdaptiveTracking::callback	(	const sensor_msgs::PointCloud2::ConstPtr &	cloud_msg,
		const geometry_msgs::PoseStamped::ConstPtr &	pose_msg
	)		`[virtual]`

Definition at line 168 of file target_adaptive_tracking_nodelet.cpp.

Eigen::Vector4f jsk_pcl_ros::TargetAdaptiveTracking::cloudMeanNormal	(	const pcl::PointCloud< pcl::Normal >::Ptr	normal,
		bool	isnorm = `true`
	)

Definition at line 1409 of file target_adaptive_tracking_nodelet.cpp.

std::vector< pcl::PointIndices::Ptr > jsk_pcl_ros::TargetAdaptiveTracking::clusterPointIndicesToPointIndices ( const jsk_recognition_msgs::ClusterPointIndicesConstPtr & indices_mgs ) [virtual]

Definition at line 1338 of file target_adaptive_tracking_nodelet.cpp.

void jsk_pcl_ros::TargetAdaptiveTracking::compute3DCentroids	(	const pcl::PointCloud< PointT >::Ptr	cloud,
		Eigen::Vector4f &	centre
	)		const

Definition at line 1393 of file target_adaptive_tracking_nodelet.cpp.

void jsk_pcl_ros::TargetAdaptiveTracking::computeCloudClusterRPYHistogram	(	const pcl::PointCloud< PointT >::Ptr	cloud,
		const pcl::PointCloud< pcl::Normal >::Ptr	normal,
		cv::Mat &	histogram
	)		const

Definition at line 1195 of file target_adaptive_tracking_nodelet.cpp.

float jsk_pcl_ros::TargetAdaptiveTracking::computeCoherency	(	const float	dist,
		const float	weight
	)

Definition at line 1468 of file target_adaptive_tracking_nodelet.cpp.

void jsk_pcl_ros::TargetAdaptiveTracking::computeColorHistogram	(	const pcl::PointCloud< PointT >::Ptr	cloud,
		cv::Mat &	hist,
		const int	hBin = `16`,
		const int	sBin = `16`,
		bool	is_norm = `true`
	)		const

Definition at line 1272 of file target_adaptive_tracking_nodelet.cpp.

void jsk_pcl_ros::TargetAdaptiveTracking::computeLocalPairwiseFeautures	(	const std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr > &	supervoxel_clusters,
		const std::map< uint32_t, std::vector< uint32_t > > &	adjacency_list,
		cv::Mat &	histogram,
		const int	feature_count = `3`
	)

Definition at line 1510 of file target_adaptive_tracking_nodelet.cpp.

void jsk_pcl_ros::TargetAdaptiveTracking::computePointFPFH	(	const pcl::PointCloud< PointT >::Ptr	cloud,
		pcl::PointCloud< pcl::Normal >::Ptr	normals,
		cv::Mat &	histogram,
		bool	holistic = `true`
	)		const

Definition at line 1299 of file target_adaptive_tracking_nodelet.cpp.

void jsk_pcl_ros::TargetAdaptiveTracking::computeScatterMatrix	(	const pcl::PointCloud< PointT >::Ptr	cloud,
		const Eigen::Vector4f	centroid
	)

Definition at line 1441 of file target_adaptive_tracking_nodelet.cpp.

void jsk_pcl_ros::TargetAdaptiveTracking::configCallback	(	Config &	config,
		uint32_t	level
	)		`[private, virtual]`

Definition at line 1858 of file target_adaptive_tracking_nodelet.cpp.

pcl::PointXYZRGBNormal jsk_pcl_ros::TargetAdaptiveTracking::convertVector4fToPointXyzRgbNormal	(	const Eigen::Vector4f &	centroid,
		const Eigen::Vector4f &	normal,
		const cv::Scalar	color
	)

Definition at line 1477 of file target_adaptive_tracking_nodelet.cpp.

void jsk_pcl_ros::TargetAdaptiveTracking::estimatedPFPose	(	const geometry_msgs::PoseStamped::ConstPtr &	pose_msg,
		PointXYZRPY &	motion_displacement
	)

Definition at line 1351 of file target_adaptive_tracking_nodelet.cpp.

template<class T >

void jsk_pcl_ros::TargetAdaptiveTracking::estimatePointCloudNormals	(	const pcl::PointCloud< PointT >::Ptr	cloud,
		pcl::PointCloud< pcl::Normal >::Ptr	normals,
		T	k = `0.05f`,
		bool	use_knn = `false`
	)		const

Definition at line 1175 of file target_adaptive_tracking_nodelet.cpp.

void jsk_pcl_ros::TargetAdaptiveTracking::filterCloudForBoundingBoxViz	(	pcl::PointCloud< PointT >::Ptr	cloud,
		const ModelsPtr	background_reference,
		const float	threshold = `0.6f`
	)

Definition at line 1696 of file target_adaptive_tracking_nodelet.cpp.

bool jsk_pcl_ros::TargetAdaptiveTracking::filterPointCloud	(	pcl::PointCloud< PointT >::Ptr	cloud,
		const Eigen::Vector4f	tracker_position,
		const ModelsPtr	template_model,
		const float	scaling_factor
	)

Definition at line 1625 of file target_adaptive_tracking_nodelet.cpp.

template<typename T >

void jsk_pcl_ros::TargetAdaptiveTracking::getRotationMatrixFromRPY	(	const PointXYZRPY &	motion_displacement,
		Eigen::Matrix< T, 3, 3 > &	rotation
	)

Definition at line 1495 of file target_adaptive_tracking_nodelet.cpp.

template<class T >

T jsk_pcl_ros::TargetAdaptiveTracking::localVoxelConvexityLikelihood	(	Eigen::Vector4f	c_centroid,
		Eigen::Vector4f	c_normal,
		Eigen::Vector4f	n_centroid,
		Eigen::Vector4f	n_normal
	)

Definition at line 1124 of file target_adaptive_tracking_nodelet.cpp.

void jsk_pcl_ros::TargetAdaptiveTracking::objInitCallback	(	const sensor_msgs::PointCloud2::ConstPtr &	cloud_msg,
		const sensor_msgs::PointCloud2::ConstPtr &	bkgd_msg,
		const geometry_msgs::PoseStamped::ConstPtr &	pose_msg
	)		`[virtual]`

Definition at line 108 of file target_adaptive_tracking_nodelet.cpp.

void jsk_pcl_ros::TargetAdaptiveTracking::onInit ( void ) [protected, virtual]

Reimplemented from jsk_topic_tools::DiagnosticNodelet.

Definition at line 21 of file target_adaptive_tracking_nodelet.cpp.

template<typename T , typename U , typename V >

cv::Scalar jsk_pcl_ros::TargetAdaptiveTracking::plotJetColour	(	T	v,
		U	vmin,
		V	vmax
	)

Definition at line 1743 of file target_adaptive_tracking_nodelet.cpp.

void jsk_pcl_ros::TargetAdaptiveTracking::processInitCloud	(	const pcl::PointCloud< PointT >::Ptr	cloud,
		ModelsPtr	object_reference
	)

Definition at line 141 of file target_adaptive_tracking_nodelet.cpp.

void jsk_pcl_ros::TargetAdaptiveTracking::processVoxelForReferenceModel	(	const std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr >	supervoxel_clusters,
		const std::multimap< uint32_t, uint32_t >	supervoxel_adjacency,
		const uint32_t	match_index,
		TargetAdaptiveTracking::ReferenceModel *	ref_model
	)

Definition at line 1031 of file target_adaptive_tracking_nodelet.cpp.

void jsk_pcl_ros::TargetAdaptiveTracking::publishSupervoxel	(	const std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr >	supervoxel_clusters,
		sensor_msgs::PointCloud2 &	ros_cloud,
		jsk_recognition_msgs::ClusterPointIndices &	ros_indices,
		const std_msgs::Header &	header
	)

Definition at line 1815 of file target_adaptive_tracking_nodelet.cpp.

void jsk_pcl_ros::TargetAdaptiveTracking::subscribe ( ) [protected, virtual]

Implements jsk_topic_tools::ConnectionBasedNodelet.

Definition at line 66 of file target_adaptive_tracking_nodelet.cpp.

void jsk_pcl_ros::TargetAdaptiveTracking::supervoxelSegmentation	(	const pcl::PointCloud< PointT >::Ptr	cloud,
		std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr > &	supervoxel_clusters,
		std::multimap< uint32_t, uint32_t > &	supervoxel_adjacency
	)

Definition at line 1793 of file target_adaptive_tracking_nodelet.cpp.

void jsk_pcl_ros::TargetAdaptiveTracking::supervoxelSegmentation	(	const pcl::PointCloud< PointT >::Ptr	cloud,
		std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr > &	supervoxel_clusters,
		std::multimap< uint32_t, uint32_t > &	supervoxel_adjacency,
		const float	seed_resolution
	)

Definition at line 1769 of file target_adaptive_tracking_nodelet.cpp.

template<class T >

T jsk_pcl_ros::TargetAdaptiveTracking::targetCandidateToReferenceLikelihood	(	const ReferenceModel &	reference_model,
		const pcl::PointCloud< PointT >::Ptr	cloud,
		const pcl::PointCloud< pcl::Normal >::Ptr	normal,
		const Eigen::Vector4f &	centroid,
		ReferenceModel *	voxel_model = `NULL`
	)

Definition at line 1083 of file target_adaptive_tracking_nodelet.cpp.

void jsk_pcl_ros::TargetAdaptiveTracking::targetDescriptiveSurfelsEstimationAndUpdate	(	pcl::PointCloud< PointT >::Ptr	cloud,
		const Eigen::Affine3f &	transformation_matrix,
		const TargetAdaptiveTracking::PointXYZRPY &	motion_disp,
		const std_msgs::Header	header
	)

Definition at line 360 of file target_adaptive_tracking_nodelet.cpp.

void jsk_pcl_ros::TargetAdaptiveTracking::targetDescriptiveSurfelsIndices	(	const jsk_recognition_msgs::ClusterPointIndices &	sv_indices,
		const std::vector< uint32_t > &	tdp_list,
		jsk_recognition_msgs::ClusterPointIndices &	ros_indices
	)

Definition at line 1845 of file target_adaptive_tracking_nodelet.cpp.

float jsk_pcl_ros::TargetAdaptiveTracking::templateCloudFilterLenght ( const pcl::PointCloud< PointT >::Ptr cloud )

Definition at line 1608 of file target_adaptive_tracking_nodelet.cpp.

void jsk_pcl_ros::TargetAdaptiveTracking::transformModelPrimitives	(	const ModelsPtr &	obj_ref,
		ModelsPtr	trans_models,
		const Eigen::Affine3f &	transform_model
	)

Definition at line 1673 of file target_adaptive_tracking_nodelet.cpp.

void jsk_pcl_ros::TargetAdaptiveTracking::unsubscribe ( ) [protected, virtual]

Implements jsk_topic_tools::ConnectionBasedNodelet.

Definition at line 88 of file target_adaptive_tracking_nodelet.cpp.

void jsk_pcl_ros::TargetAdaptiveTracking::updateDiagnostic ( diagnostic_updater::DiagnosticStatusWrapper & stat ) [protected, virtual]

Reimplemented from jsk_topic_tools::DiagnosticNodelet.

Definition at line 96 of file target_adaptive_tracking_nodelet.cpp.

void jsk_pcl_ros::TargetAdaptiveTracking::voxelizeAndProcessPointCloud	(	const pcl::PointCloud< PointT >::Ptr	cloud,
		const std::map< uint32_t, pcl::Supervoxel< PointT >::Ptr > &	supervoxel_clusters,
		const std::multimap< uint32_t, uint32_t > &	supervoxel_adjacency,
		std::vector< AdjacentInfo > &	supervoxel_list,
		TargetAdaptiveTracking::ModelsPtr &	models,
		bool	norm_flag = `true`,
		bool	feat_flag = `true`,
		bool	cent_flag = `true`,
		bool	neigh_pfh = `false`
	)