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

virtual	~TargetAdaptiveTracking ()

Protected Member Functions
virtual void	onInit ()

virtual void	subscribe ()

virtual void	unsubscribe ()

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_

std::string	child_frame_id_

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_

std::string	parent_frame_id_

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_tf_

bool	use_transform_

float	vfh_scaling_

double	voxel_resolution_

Detailed Description

Definition at line 67 of file target_adaptive_tracking.h.

Member Typedef Documentation

◆ Config

typedef jsk_pcl_ros::TargetAdaptiveTrackingConfig jsk_pcl_ros::TargetAdaptiveTracking::Config

private

Definition at line 164 of file target_adaptive_tracking.h.

◆ Models

typedef std::vector<ReferenceModel> jsk_pcl_ros::TargetAdaptiveTracking::Models

private

Definition at line 95 of file target_adaptive_tracking.h.

◆ ModelsPtr

typedef boost::shared_ptr<Models> jsk_pcl_ros::TargetAdaptiveTracking::ModelsPtr

private

Definition at line 96 of file target_adaptive_tracking.h.

◆ MotionHistory

typedef std::vector<PointXYZRPY> jsk_pcl_ros::TargetAdaptiveTracking::MotionHistory

private

Definition at line 98 of file target_adaptive_tracking.h.

◆ ObjectSyncPolicy

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

private

Definition at line 108 of file target_adaptive_tracking.h.

◆ PointT

typedef pcl::PointXYZRGB jsk_pcl_ros::TargetAdaptiveTracking::PointT

private

Definition at line 69 of file target_adaptive_tracking.h.

◆ PointXYZRPY

typedef pcl::tracking::ParticleXYZRPY jsk_pcl_ros::TargetAdaptiveTracking::PointXYZRPY

private

Definition at line 97 of file target_adaptive_tracking.h.

◆ SyncPolicy

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

private

Definition at line 101 of file target_adaptive_tracking.h.

Constructor & Destructor Documentation

◆ TargetAdaptiveTracking()

jsk_pcl_ros::TargetAdaptiveTracking::TargetAdaptiveTracking ( )

Definition at line 10 of file target_adaptive_tracking_nodelet.cpp.

◆ ~TargetAdaptiveTracking()

jsk_pcl_ros::TargetAdaptiveTracking::~TargetAdaptiveTracking ( )

virtual

Definition at line 22 of file target_adaptive_tracking_nodelet.cpp.

Member Function Documentation

◆ backgroundReferenceLikelihood()

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

Definition at line 1151 of file target_adaptive_tracking_nodelet.cpp.

◆ callback()

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

virtual

Definition at line 174 of file target_adaptive_tracking_nodelet.cpp.

◆ cloudMeanNormal()

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

Definition at line 1412 of file target_adaptive_tracking_nodelet.cpp.

◆ clusterPointIndicesToPointIndices()

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

virtual

Definition at line 1341 of file target_adaptive_tracking_nodelet.cpp.

◆ compute3DCentroids()

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

Definition at line 1396 of file target_adaptive_tracking_nodelet.cpp.

◆ computeCloudClusterRPYHistogram()

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 1198 of file target_adaptive_tracking_nodelet.cpp.

◆ computeCoherency()

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

Definition at line 1471 of file target_adaptive_tracking_nodelet.cpp.

◆ computeColorHistogram()

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 1275 of file target_adaptive_tracking_nodelet.cpp.

◆ computeLocalPairwiseFeautures()

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 1513 of file target_adaptive_tracking_nodelet.cpp.

◆ computePointFPFH()

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 1302 of file target_adaptive_tracking_nodelet.cpp.

◆ computeScatterMatrix()

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

Definition at line 1444 of file target_adaptive_tracking_nodelet.cpp.

◆ configCallback()

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

privatevirtual

Definition at line 1869 of file target_adaptive_tracking_nodelet.cpp.

◆ convertVector4fToPointXyzRgbNormal()

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

Definition at line 1480 of file target_adaptive_tracking_nodelet.cpp.

◆ estimatedPFPose()

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

Definition at line 1354 of file target_adaptive_tracking_nodelet.cpp.

◆ estimatePointCloudNormals()

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 1178 of file target_adaptive_tracking_nodelet.cpp.

◆ filterCloudForBoundingBoxViz()

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

Definition at line 1699 of file target_adaptive_tracking_nodelet.cpp.

◆ filterPointCloud()

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 1628 of file target_adaptive_tracking_nodelet.cpp.

◆ getRotationMatrixFromRPY()

template<typename T >

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

Definition at line 1498 of file target_adaptive_tracking_nodelet.cpp.

◆ localVoxelConvexityLikelihood()

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 1127 of file target_adaptive_tracking_nodelet.cpp.

◆ objInitCallback()

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 114 of file target_adaptive_tracking_nodelet.cpp.

◆ onInit()

void jsk_pcl_ros::TargetAdaptiveTracking::onInit ( )

protectedvirtual

Definition at line 34 of file target_adaptive_tracking_nodelet.cpp.

◆ plotJetColour()

template<typename T , typename U , typename V >

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

Definition at line 1746 of file target_adaptive_tracking_nodelet.cpp.

◆ processInitCloud()

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

Definition at line 147 of file target_adaptive_tracking_nodelet.cpp.

◆ processVoxelForReferenceModel()

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 1034 of file target_adaptive_tracking_nodelet.cpp.

◆ publishSupervoxel()

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 1826 of file target_adaptive_tracking_nodelet.cpp.

◆ subscribe()

void jsk_pcl_ros::TargetAdaptiveTracking::subscribe ( )

protectedvirtual

Definition at line 84 of file target_adaptive_tracking_nodelet.cpp.

◆ supervoxelSegmentation() [1/2]

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 1800 of file target_adaptive_tracking_nodelet.cpp.

◆ supervoxelSegmentation() [2/2]

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 1772 of file target_adaptive_tracking_nodelet.cpp.

◆ targetCandidateToReferenceLikelihood()

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 1086 of file target_adaptive_tracking_nodelet.cpp.

◆ targetDescriptiveSurfelsEstimationAndUpdate()

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 363 of file target_adaptive_tracking_nodelet.cpp.

◆ targetDescriptiveSurfelsIndices()

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 1856 of file target_adaptive_tracking_nodelet.cpp.

◆ templateCloudFilterLenght()

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

Definition at line 1611 of file target_adaptive_tracking_nodelet.cpp.

◆ transformModelPrimitives()

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

Definition at line 1676 of file target_adaptive_tracking_nodelet.cpp.

◆ unsubscribe()

void jsk_pcl_ros::TargetAdaptiveTracking::unsubscribe ( )

protectedvirtual

Definition at line 106 of file target_adaptive_tracking_nodelet.cpp.

◆ voxelizeAndProcessPointCloud()

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