/home/laradmin/lar/perception/planarobstacles/mtt/src/pmht.cpp File Reference

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.
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).
void	CreateTreeMarker (vector< visualization_msgs::Marker > &mrk, tree< t_nodePtr >::iterator root)
void	DataHandler (const sensor_msgs::PointCloud2 &msg)
	Handler for the incoming data.
MatrixXd	EllipseParametric (Matrix2d &cov, Vector2d &mu, double MahThreshold)
	Create a parametric ellipse from covariance matrix.
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.
void *	GraphicalThread (void *data)
double	Mahalanobis (Vector2d &y, Vector2d &mean, Matrix2d &cov)
	Calculate the Mahalanobis distance.
double	Mahalanobis_distance (t_nodePtr &node, t_clusterPtr &cluster)
	Calculate the distance between a node and a cluster.
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.
void	ReAgeTree (void)
void	RemoveOld (int size)
	Remove old nodes from the tree.
Variables
tree< t_nodePtr >	global_tree
	Main super hyper tree.
GVGraph *	graph_context
	Graph context pointer, used in the xgtk plug-in.
pthread_t	graph_thread
	Additional thread to run the graphviz plug-in.
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.
long	iteration = 0
	Current iteration counter, used in the naming of tree nodes.
Publisher	markers_publisher
	Marker publisher.
Publisher	markers_tree_publisher
	Tree marker publisher, only leafs.
double	max_mahalanobis = 7
	Max Mahalanobis distance used in gating.
string	tracking_frame
	Tracking frame, frame id of the current tracking frame.

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.