35 #include <mtt/tree.hh>
36 #include <mtt/tree_util.hh>
38 #include <colormap/colormap.h>
40 #include <laser_geometry/laser_geometry.h>
42 #include <pcl/point_types.h>
43 #include <pcl/io/pcd_io.h>
44 #include <pcl/kdtree/kdtree_flann.h>
45 #include <pcl/surface/mls.h>
47 #include <Eigen/Dense>
48 #include <Eigen/Cholesky>
50 #include <visualization_msgs/Marker.h>
51 #include <visualization_msgs/MarkerArray.h>
56 #include <graphviz/gvc.h>
57 #include <graphviz/types.h>
58 #include <graphviz/gvplugin.h>
91 void CreateMarkers(vector<visualization_msgs::Marker>& marker_vector,vector<t_clusterPtr>&clusters);
112 void CreateObjects(vector<t_pointPtr>&data,vector<t_clusterPtr>&clusters,
double clustering_distance);
123 double Mahalanobis(Vector2d&y,Vector2d&mean,Matrix2d& cov);
145 return sqrt( pow(node->x_p(0)-cluster->centroid.x,2) + pow(node->x_p(1)-cluster->centroid.y,2));
158 return sqrt( pow(p1.
x-p2.
x,2) + pow(p1.
y-p2.
y,2));
void CreateMarkers(vector< visualization_msgs::Marker > &marker_vector, vector< t_clusterPtr > &clusters)
Create markers for Rviz.
double Euclidean_distance(t_nodePtr &node, t_clusterPtr &cluster)
Calculate the Euclidean distance from node to cluster.
boost::shared_ptr< t_node > t_nodePtr
Shared pointer to the t_node class.
void CreateObjects(vector< t_pointPtr > &data, vector< t_clusterPtr > &clusters, double clustering_distance)
This function converts a set of points into groups (aka clusters)
Header for type declaration, only constant velocity Kalman filter.
vector< t_clusterPtr >::iterator GetClosestCandidate(t_nodePtr &node, vector< t_clusterPtr > &clusters)
Get the closest candidate to the node from all the clusters.
double x
x coordinate (Cartesian)
void gvconfig_plugin_install_from_library(GVC_t *, char *, gvplugin_library_t *)
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...
double Mahalanobis_distance(t_nodePtr &node, t_clusterPtr &cluster)
Calculate the distance between a node and a cluster.
Mht implementation main header.
double y
y coordinate (Cartesian)
GVGraph class declaration.
boost::shared_ptr< t_cluster > t_clusterPtr
Shared pointer to the t_cluster class.
MatrixXd EllipseParametric(Matrix2d &cov, Vector2d &mu, double MahThreshold)
Create a parametric ellipse from covariance matrix.
double Mahalanobis(Vector2d &y, Vector2d &mean, Matrix2d &cov)
Calculate the Mahalanobis distance.