33 #ifndef _TYPES_DECLARATION_H_
34 #define _TYPES_DECLARATION_H_
36 #error This file should not be used, DEPRECATED
40 #include <kfilter/ekfilter.hpp>
42 #include <Eigen/Dense>
43 #include <Eigen/Cholesky>
45 #include <visualization_msgs/Marker.h>
46 #include <visualization_msgs/MarkerArray.h>
50 using namespace Kalman;
52 using Eigen::Matrix4d;
53 using Eigen::Matrix2d;
54 using Eigen::Vector2d;
55 using Eigen::Vector4d;
56 using Eigen::MatrixXd;
57 using Eigen::VectorXd;
87 void SetIdentity(
Matrix& M,
int size,
double value=1);
97 void SetZero(
Matrix& M,
int size);
105 void InitFilter(Vector4d& x_init);
123 void makeCommonProcess();
216 void Scale(
double val);
271 void CalculateCentroid(
void);
306 enum e_mode {NEW,MAIN,SIBLING,FAILED,TEST} mode;
321 void IncreaseAge(
int increment=1);
359 void CommonConstructor();
EKFilter< double, 0, false, false, false >::Matrix Matrix
EKfilter matrix type.
Constant velocity Kalman filter.
constant_velocity_ekfilter estimator
Local estimator for process, Kalman filter with a constant velocity.
double n
auxiliary identifier
boost::shared_ptr< constant_velocity_ekfilter > constant_velocity_ekfilterPtr
Shared pointer to the Kalman constant velocity filter.
long iteration
Current iteration counter, used in the naming of tree nodes.
long data_iteration
Iteration from the data used in its creation.
Time lt
Time of the last call to the makeCommonProcess function.
vector< int > associations
Association vector, multiple associations maybe possible.
boost::shared_ptr< t_node > t_nodePtr
Shared pointer to the t_node class.
t_point centroid
Centroid of the cluster.
double ni
auxiliary identifier 2
double x
x coordinate (Cartesian)
vector< NodePtr > & operator+=(vector< NodePtr > &a, vector< NodePtr > &b)
vector< t_pointPtr > points
Vector of the support points of the cluster.
double z
z coordinate (Cartesian)
long age
Number of iterations that this node suffered.
t_cluster local_cluster
Support points for this node.
double dt
Time interval between measurements.
double y
y coordinate (Cartesian)
Cluster type class, clusters are groups of points in close proximity.
Vector2d z
Measurement vector.
boost::shared_ptr< t_point > t_pointPtr
Shared pointer to the t_point class.
Class handler for tree nodes.
visualization_msgs::Marker marker
Marker for rviz.
boost::shared_ptr< t_cluster > t_clusterPtr
Shared pointer to the t_cluster class.
int failed_associations_counter
Number of iterations with failed associations.
EKFilter< double, 0, false, false, false >::Vector Vector
EKfilter vector type.
Matrix4d P
Posteriori error covariance.
double id
Id of the cluster.
Vector4d x
Estimated state.
int successful_association_counter
Number of iterations with successful associations.
ostream & operator<<(ostream &o, const t_nodePtr &i)
Overload of the operator << for the t_nodePtr typedef.