Source code for the Multi-Hypothesis Tracking data association algorithm. More...

#include <mtt/pmht.h>

Include dependency graph for pmht.cpp:

Functions
void	CreateMarkers (vector< visualization_msgs::Marker > &marker_vector, vector< t_clusterPtr > &clusters)
	Create markers for Rviz. More...

void	CreateMarkersFromTreeLeafs (vector< visualization_msgs::Marker > &marker_vector)

void	CreateObjects (vector< t_pointPtr > &data, vector< t_clusterPtr > &clusters, double clustering_distance)
	This function converts a set of points into groups (aka clusters) More...

void	CreateTreeMarker (vector< visualization_msgs::Marker > &mrk, tree< t_nodePtr >::iterator root)

void	DataHandler (const sensor_msgs::PointCloud2 &msg)
	Handler for the incoming data. More...

MatrixXd	EllipseParametric (Matrix2d &cov, Vector2d &mu, double MahThreshold)
	Create a parametric ellipse from covariance matrix. More...

vector< vector< t_clusterPtr > ::iterator >	GateMeasurements (t_nodePtr &node, vector< t_clusterPtr > &clusters, double max_gating)

vector< t_clusterPtr >::iterator	GetClosestCandidate (t_nodePtr &node, vector< t_clusterPtr > &clusters)
	Get the closest candidate to the node from all the clusters. More...

void *	GraphicalThread (void *data)

double	Mahalanobis (Vector2d &y, Vector2d &mean, Matrix2d &cov)
	Calculate the Mahalanobis distance. More...

double	Mahalanobis_distance (t_nodePtr &node, t_clusterPtr &cluster)
	Calculate the distance between a node and a cluster. More...

int	main (int argc, char **argv)

void	nthPruning (int size)

void	PointCloud2ToVector (const sensor_msgs::PointCloud2 &cloud, vector< t_pointPtr > &data)
	Convert from a point cloud representation to a stl vector of shared pointers to the class t_point. More...

void	ReAgeTree (void)

void	RemoveOld (int size)
	Remove old nodes from the tree. More...

Variables
tree< t_nodePtr >	global_tree
	Main super hyper tree. More...

GVGraph *	graph_context
	Graph context pointer, used in the xgtk plug-in. More...

pthread_t	graph_thread
	Additional thread to run the graphviz plug-in. More...

gvplugin_library_t	gvplugin_xgtk_LTX_library
	Xgtk external graphviz library/plugin (only plugin, there's no need to create a library since it can be linked internally. More...

long	iteration =0
	Current iteration counter, used in the naming of tree nodes. More...

Publisher	markers_publisher
	Marker publisher. More...

Publisher	markers_tree_publisher
	Tree marker publisher, only leafs. More...

double	max_mahalanobis =7
	Max Mahalanobis distance used in gating. More...

string	tracking_frame
	Tracking frame, frame id of the current tracking frame. More...

Detailed Description

Source code for the Multi-Hypothesis Tracking data association algorithm.

For additional info on the Mht algorithm check here.

Definition in file pmht.cpp.

Function Documentation

void CreateMarkers	(	vector< visualization_msgs::Marker > &	marker_vector,
		vector< t_clusterPtr > &	clusters
	)

Create markers for Rviz.

Definition at line 379 of file pmht.cpp.

void CreateMarkersFromTreeLeafs ( vector< visualization_msgs::Marker > & marker_vector )

Definition at line 209 of file pmht.cpp.

void CreateObjects	(	vector< t_pointPtr > &	data,
		vector< t_clusterPtr > &	clusters,
		double	clustering_distance
	)

This function converts a set of points into groups (aka clusters)

The clustering algorithm is solely based on the distance between consecutive points in the polar grid.
It copies the support points of the cluster into the own cluster so no reference to the original data is needed not even desired.

Parameters

data	vector of data
clusters	vector of clusters (this is the output so it will be filled)
clustering_distance	maximum distance between two points for them to be in the same cluster

Definition at line 525 of file pmht.cpp.

void CreateTreeMarker	(	vector< visualization_msgs::Marker > &	mrk,
		tree< t_nodePtr >::iterator	root
	)

Definition at line 64 of file pmht.cpp.

void DataHandler ( const sensor_msgs::PointCloud2 & msg )

Handler for the incoming data.

Parameters

msg	incoming point cloud

Convert from the pointcloud unsorted structure to a scan like sorted structure

Create objects, for now is simple clustering

New nodes are created from clusters, maybe in the future they will be created from other type of data

Insert into the tree the start clusters, at root

Definition at line 711 of file pmht.cpp.

MatrixXd EllipseParametric	(	Matrix2d &	cov,
		Vector2d &	mu,
		double	MahThreshold
	)

Create a parametric ellipse from covariance matrix.

This function creates a parametric ellipse from a covariance matrix, the mean values and a threshold. The main axes directions are calculated using the eigenvectors of the covariance matrix, then a crawling algorithm detects the threshold points obtaining the lengths of the axes. After that the generic parametric equation is used to calculate points in the threshold boundary.

Parameters

cov	covariance matrix
mu	mean values
MahThreshold	Mahalanobis threshold to use

Returns: the xy points of the ellipse, the last points is equal to the first so the ellipse closes

Definition at line 125 of file pmht.cpp.

vector<vector<t_clusterPtr>::iterator> GateMeasurements	(	t_nodePtr &	node,
		vector< t_clusterPtr > &	clusters,
		double	max_gating
	)

Definition at line 622 of file pmht.cpp.

vector<t_clusterPtr>::iterator GetClosestCandidate	(	t_nodePtr &	node,
		vector< t_clusterPtr > &	clusters
	)

Get the closest candidate to the node from all the clusters.

This function calls the CalculateDistance function to do the real node to cluster evaluation.

Parameters

node	node to use
clusters	clusters to search

Returns: iterator to a member of the clusters

Definition at line 601 of file pmht.cpp.

void* GraphicalThread ( void * data )

Definition at line 858 of file pmht.cpp.

double Mahalanobis	(	Vector2d &	y,
		Vector2d &	mean,
		Matrix2d &	cov
	)

Calculate the Mahalanobis distance.

Calculate the Mahalanobis distance for a measurement y from a set with a given mean and covariance.

Parameters

y	measurement
mean	mean value of the variable
cov	covariance of the variable

Returns: the Mahalanobis distance

Definition at line 572 of file pmht.cpp.

double Mahalanobis_distance	(	t_nodePtr &	node,
		t_clusterPtr &	cluster
	)

Calculate the distance between a node and a cluster.

This function uses the Mahalanobis distance between the node position estimation and the cluster position.

Parameters

node	tree node to use
cluster	cluster to use

Returns: the Mahalanobis distance

Definition at line 580 of file pmht.cpp.

int main	(	int	argc,
		char **	argv
	)

Definition at line 942 of file pmht.cpp.

void nthPruning ( int size )

Definition at line 678 of file pmht.cpp.

void PointCloud2ToVector	(	const sensor_msgs::PointCloud2 &	cloud,
		vector< t_pointPtr > &	data
	)

Convert from a point cloud representation to a stl vector of shared pointers to the class t_point.

This function first puts all the points in a polar grid representation and reorders them to be angularly sequenced.
This function has an important parameter which is the angular resolution of the grid that should be a ros param.

Todo:

Convert the current hard coded angular resolution of the grid (vulgar grid spacing).

Parameters

cloud	input point cloud
data	stl vector of boost::shared_ptr of the class t_point

Definition at line 464 of file pmht.cpp.

void ReAgeTree ( void )

Definition at line 640 of file pmht.cpp.

void RemoveOld ( int size )

Remove old nodes from the tree.

This will remove root nodes that are too old.
The age of a root node is calculated via its maximum depth.
The root node must only have one child for this process to work.

Parameters

size	age limit, maximum depth for this branch

Definition at line 656 of file pmht.cpp.

Variable Documentation

tree<t_nodePtr> global_tree

Main super hyper tree.

External variable linking to the main tree pointer in pmht.cpp.

Definition at line 37 of file pmht.cpp.

GVGraph* graph_context

Graph context pointer, used in the xgtk plug-in.

External variable linking to the main graph context created in the mht main source code pmht.cpp.

Definition at line 43 of file pmht.cpp.

pthread_t graph_thread

Additional thread to run the graphviz plug-in.

Definition at line 58 of file pmht.cpp.

gvplugin_library_t gvplugin_xgtk_LTX_library

Xgtk external graphviz library/plugin (only plugin, there's no need to create a library since it can be linked internally.

Xgtk external graphviz library/plugin (only plugin, there's no need to create a library since it can be linked internally).

Definition at line 256 of file gvplugin_xgtk.cpp.

long iteration =0

Current iteration counter, used in the naming of tree nodes.

Definition at line 61 of file pmht.cpp.

Publisher markers_publisher

Marker publisher.

Definition at line 46 of file pmht.cpp.

Publisher markers_tree_publisher

Tree marker publisher, only leafs.

Definition at line 49 of file pmht.cpp.

double max_mahalanobis =7

Max Mahalanobis distance used in gating.

Definition at line 55 of file pmht.cpp.

string tracking_frame

Tracking frame, frame id of the current tracking frame.

Definition at line 52 of file pmht.cpp.

Functions

Variables

Detailed Description

Function Documentation

Variable Documentation