#include <RBFilter.h>

Inheritance diagram for omip::RBFilter:

Public Types
enum	PredictionHypothesis { BASED_ON_VELOCITY = 0, BASED_ON_BRAKING_EVENT = 1, BASED_ON_KINEMATICS = 2 }
typedef std::vector < Eigen::Matrix4d >	Trajectory
Public Member Functions
virtual void	addPredictedFeatureLocation (const Feature::Location &predicted_location_velocity, const Feature::Location &predicted_location_brake, const Feature::Id feature_id)
virtual void	addSupportingFeature (Feature::Id supporting_feat_id)
RBFilter::Ptr	clone () const
virtual void	correctState ()
virtual void	doNotUsePredictionFromHigherLevel ()
virtual void	estimateBestPredictionAndSupportingFeatures ()
FeatureCloudPCLwc::Ptr	getFeaturesAtBirth ()
virtual FeaturesDataBase::Ptr	getFeaturesDatabase () const
FeatureCloudPCLwc::Ptr	getFeaturesPredicted ()
virtual RB_id_t	getId () const
virtual Feature::Location	getIntialLocationOfCentroid () const
virtual void	GetLocationOfFeatureAtBirthOfRB (Feature::Ptr feature, bool contemporary_last_feat_loc_and_last_pose_in_trajectory, Feature::Location &location_at_birth) const
	Get the location of the feature at the frame when the RB was detected first. If the feature is "younger" we use the previous motion of the RB to estimate what would have been the location of the feature at the initial frame.
virtual int	getNumberSupportingFeatures () const
virtual Eigen::Matrix4d	getPose () const
virtual Eigen::Matrix< double, 6, 6 >	getPoseCovariance () const
virtual geometry_msgs::TwistWithCovariancePtr	getPoseECWithCovariance () const
virtual geometry_msgs::PoseWithCovariancePtr	getPoseWithCovariance () const
virtual FeatureCloudPCLwc	getPredictedMeasurement (PredictionHypothesis hypothesis=BASED_ON_VELOCITY)
virtual std::vector< Feature::Id >	getSupportingFeatures () const
virtual std::vector < Eigen::Matrix4d >	getTrajectory () const
virtual Eigen::Twistd	getVelocity () const
virtual Eigen::Matrix< double, 6, 6 >	getVelocityCovariance () const
virtual geometry_msgs::TwistWithCovariancePtr	getVelocityWithCovariance () const
virtual void	Init ()
virtual void	integrateShapeBasedPose (const geometry_msgs::TwistWithCovariance twist_refinement, double pose_time_elapsed_ns)
virtual void	PredictFeatureLocation (Feature::Ptr feature, const Eigen::Matrix4d &predicted_pose, bool contemporary_last_feat_loc_and_last_pose_in_trajectory, Feature::Location &predicted_location, bool publish_locations=false) const
virtual void	predictMeasurement ()
	Second step when updating the filter. The next measurement is predicted from the predicted next state.
virtual void	predictState (double time_interval_ns=-1.)
	RBFilter ()
	RBFilter (double loop_period_ns, const std::vector< Eigen::Matrix4d > &trajectory, const Eigen::Twistd &initial_velocity, FeaturesDataBase::Ptr feats_database, double estimation_error_threshold)
	RBFilter (const RBFilter &rbm)
void	resetFeaturesAtBirth ()
void	resetFeaturesPredicted ()
virtual void	setCovarianceSystemAccelerationRx (double v)
virtual void	setCovarianceSystemAccelerationRy (double v)
virtual void	setCovarianceSystemAccelerationRz (double v)
virtual void	setCovarianceSystemAccelerationTx (double v)
virtual void	setCovarianceSystemAccelerationTy (double v)
virtual void	setCovarianceSystemAccelerationTz (double v)
virtual void	setEstimationThreshold (double v)
virtual void	setFeaturesDatabase (FeaturesDataBase::Ptr feats_database)
virtual void	setMeasurementDepthFactor (double v)
virtual void	setMinCovarianceMeasurementX (double v)
virtual void	setMinCovarianceMeasurementY (double v)
virtual void	setMinCovarianceMeasurementZ (double v)
virtual void	setMinNumberOfSupportingFeaturesToCorrectPredictedState (int v)
virtual void	setMotionConstraint (int motion_constraint)
	Set a constraint for the motion of the RB Right now this is only used by the StaticEnvironmentFilter.
virtual void	setNumberOfTrackedFeatures (int v)
virtual void	setPredictedState (omip_msgs::RigidBodyPoseAndVelMsg hypothesis)
virtual void	setPriorCovariancePose (double v)
virtual void	setPriorCovarianceVelocity (double v)
virtual	~RBFilter ()
Public Attributes
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef boost::shared_ptr < RBFilter >	Ptr
Protected Member Functions
virtual void	_initializeAuxiliarMatrices ()
virtual RBFilter *	doClone () const
Protected Attributes
double	_cov_sys_acc_rx
double	_cov_sys_acc_ry
double	_cov_sys_acc_rz
double	_cov_sys_acc_tx
double	_cov_sys_acc_ty
double	_cov_sys_acc_tz
Eigen::Matrix< double, 3, 6 >	_D_T_p0_circle
double	_estimation_error_threshold
FeatureCloudPCLwc::Ptr	_features_at_birth
FeaturesDataBase::Ptr	_features_database
FeatureCloudPCLwc::Ptr	_features_predicted
Eigen::MatrixXd	_G_t_memory
RB_id_t	_id
Feature::Location	_initial_location_of_centroid
double	_last_time_interval_ns
double	_loop_period_ns
double	_meas_depth_factor
double	_min_cov_meas_x
double	_min_cov_meas_y
double	_min_cov_meas_z
int	_min_num_supporting_feats_to_correct
int	_num_tracked_feats
Eigen::Matrix4d	_pose
Eigen::Matrix< double, 6, 6 >	_pose_covariance
Eigen::Matrix4d	_predicted_delta_pose_kh
FeatureCloudPCLwc::Ptr	_predicted_measurement
std::map< Feature::Id, Feature::Location >	_predicted_measurement_map
std::map< Feature::Id, Feature::Location >	_predicted_measurement_map_bh
std::map< Feature::Id, Feature::Location >	_predicted_measurement_map_kh
std::map< Feature::Id, Feature::Location >	_predicted_measurement_map_vh
FeatureCloudPCLwc::Ptr	_predicted_measurement_pc_bh
FeatureCloudPCLwc::Ptr	_predicted_measurement_pc_kh
FeatureCloudPCLwc::Ptr	_predicted_measurement_pc_vh
Eigen::Matrix4d	_predicted_pose
Eigen::Matrix4d	_predicted_pose_bh
Eigen::Matrix< double, 6, 6 >	_predicted_pose_cov_bh
Eigen::Matrix< double, 6, 6 >	_predicted_pose_cov_kh
Eigen::Matrix< double, 6, 6 >	_predicted_pose_cov_vh
Eigen::Matrix< double, 6, 6 >	_predicted_pose_covariance
Eigen::Matrix4d	_predicted_pose_kh
Eigen::Matrix4d	_predicted_pose_vh
Eigen::Twistd	_predicted_velocity
Eigen::Twistd	_predicted_velocity_bh
Eigen::Matrix< double, 6, 6 >	_predicted_velocity_cov_bh
Eigen::Matrix< double, 6, 6 >	_predicted_velocity_cov_kh
Eigen::Matrix< double, 6, 6 >	_predicted_velocity_cov_vh
Eigen::Matrix< double, 6, 6 >	_predicted_velocity_covariance
Eigen::Twistd	_predicted_velocity_kh
Eigen::Twistd	_predicted_velocity_vh
double	_prior_cov_pose
double	_prior_cov_vel
Eigen::VectorXd	_R_inv_times_innovation_memory
std::map< Feature::Id, double >	_supporting_feats_errors
std::vector< Feature::Id >	_supporting_features_ids
std::vector< double >	_supporting_features_probs
std::vector< Eigen::Matrix4d >	_trajectory
bool	_use_predicted_measurement_from_kh
bool	_use_predicted_state_from_kh
Eigen::Twistd	_velocity
Eigen::Matrix< double, 6, 6 >	_velocity_covariance
Static Protected Attributes
static RB_id_t	_rb_id_generator = 1

Detailed Description

Class RBFilter Represents one moving rigid body Tracks the pose of this rigid body (state) Estimates the Features that move coherently with the pose and velocity of this rigid body and uses them to update the state

Definition at line 98 of file RBFilter.h.

Member Typedef Documentation

typedef std::vector<Eigen::Matrix4d> omip::RBFilter::Trajectory

Definition at line 108 of file RBFilter.h.

Member Enumeration Documentation

enum omip::RBFilter::PredictionHypothesis

Enumerator:

BASED_ON_VELOCITY
BASED_ON_BRAKING_EVENT
BASED_ON_KINEMATICS

Definition at line 110 of file RBFilter.h.

Constructor & Destructor Documentation

RBFilter::RBFilter ( )

Default constructor

Definition at line 30 of file RBFilter.cpp.

RBFilter::RBFilter	(	double	loop_period_ns,
		const std::vector< Eigen::Matrix4d > &	trajectory,
		const Eigen::Twistd &	initial_velocity,
		FeaturesDataBase::Ptr	feats_database,
		double	estimation_error_threshold
	)

Constructor

Parameters:

first_transform	- Twist of the initial pose of the body in SF
initial_velocity	- Twist of the initial velocity of the body in SF
feats_database	- Shared pointer to the database of Features
estimation_error_threshold	- Maximum error allowed between predicted and measured Feature location to consider that it still supports this RBM

Definition at line 45 of file RBFilter.cpp.

RBFilter::~RBFilter ( ) [virtual]

Destructor

Definition at line 87 of file RBFilter.cpp.

RBFilter::RBFilter ( const RBFilter & rbm )

Copy constructor

Parameters:

rbm	- Object constant reference to be copied

Definition at line 70 of file RBFilter.cpp.

Member Function Documentation

void RBFilter::_initializeAuxiliarMatrices ( ) [protected, virtual]

NOTE: The system model predicts the next state based on the previous step We maintain in parallel two system models:

One model supposes that the rigid body moves with constant velocity
One model supposes that the rigid body stops abruptly (p'=p) Initialize the auxiliar matrices that are needed in the EKF computation

Definition at line 784 of file RBFilter.cpp.

void RBFilter::addPredictedFeatureLocation	(	const Feature::Location &	predicted_location_velocity,
		const Feature::Location &	predicted_location_brake,
		const Feature::Id	feature_id
	)		`[virtual]`

Parameters:

predicted_location_velocity
predicted_location_brake
feature_id

Definition at line 553 of file RBFilter.cpp.

void RBFilter::addSupportingFeature ( Feature::Id supporting_feat_id ) [virtual]

Add the ID of a feature to the list of supporting features of this RB

Parameters:

supporting_feat_id - ID of the Feature that now supports this RB

Reimplemented in omip::StaticEnvironmentFilter.

Definition at line 493 of file RBFilter.cpp.

RBFilter::Ptr omip::RBFilter::clone ( ) const [inline]

Clones this RBFilter

Returns:: - Shared pointer to the clone of this RBFilter

Definition at line 153 of file RBFilter.h.

void RBFilter::correctState ( ) [virtual]

Correct the predicted pose of the RB using the Features that support this RB

Reimplemented in omip::StaticEnvironmentFilter.

Definition at line 196 of file RBFilter.cpp.

virtual RBFilter* omip::RBFilter::doClone ( ) const [inline, protected, virtual]

Definition at line 539 of file RBFilter.h.

virtual void omip::RBFilter::doNotUsePredictionFromHigherLevel ( ) [inline, virtual]

Definition at line 409 of file RBFilter.h.

void RBFilter::estimateBestPredictionAndSupportingFeatures ( ) [virtual]

Estimate the Features that support this RB (Features that move with the motion of this RB) There are two options: to test all Features or to test the previously supporting Features only (second option implemented)

Definition at line 376 of file RBFilter.cpp.

FeatureCloudPCLwc::Ptr omip::RBFilter::getFeaturesAtBirth ( ) [inline]

Definition at line 414 of file RBFilter.h.

FeaturesDataBase::Ptr RBFilter::getFeaturesDatabase ( ) const [virtual]

Get the Feature data base used by this RB

Returns:: - Shared pointer ot the Feature data base

Definition at line 779 of file RBFilter.cpp.

FeatureCloudPCLwc::Ptr omip::RBFilter::getFeaturesPredicted ( ) [inline]

Definition at line 419 of file RBFilter.h.

RB_id_t RBFilter::getId ( ) const [virtual]

Get the ID of this RB

Returns:: - ID of this RB

Definition at line 663 of file RBFilter.cpp.

virtual Feature::Location omip::RBFilter::getIntialLocationOfCentroid ( ) const [inline, virtual]

Definition at line 404 of file RBFilter.h.

void RBFilter::GetLocationOfFeatureAtBirthOfRB	(	Feature::Ptr	feature,
		bool	contemporary_last_feat_loc_and_last_pose_in_trajectory,
		Feature::Location &	location_at_birth
	)		const `[virtual]`

Get the location of the feature at the frame when the RB was detected first. If the feature is "younger" we use the previous motion of the RB to estimate what would have been the location of the feature at the initial frame.

Parameters:

feature	- Feature whose intial location we want to know
contemporary_last_feat_loc_and_last_pose_in_trajectory	- True if the last location and the last entry of the _trajectory vector are of the same time step This is the case only at the end of the loop, after the correction step (when we predict the next feature location.
location_at_birth	- Location of the Feature at the first frame that the body was tracked

Definition at line 619 of file RBFilter.cpp.

int RBFilter::getNumberSupportingFeatures ( ) const [virtual]

Get the number of the current Features support this RB

Returns:: - Number of the Features that support this RB

Definition at line 774 of file RBFilter.cpp.

Eigen::Matrix4d RBFilter::getPose ( ) const [virtual]

Get the current believed pose in homogeneous matrix representation

Returns:: - Eigen matrix containing the homogeneous matrix of the current believed pose

Definition at line 668 of file RBFilter.cpp.

Eigen::Matrix< double, 6, 6 > RBFilter::getPoseCovariance ( ) const [virtual]

Get the covariance of the current believed pose

Parameters:

hypothesis - hypothesis from which we want to get the covariance of the believed pose: velocity-update or brake-event update

Returns:: - Eigen matrix containing the covariance of the current believed pose

Definition at line 673 of file RBFilter.cpp.

geometry_msgs::TwistWithCovariancePtr RBFilter::getPoseECWithCovariance ( ) const [virtual]

Get the current believed pose in exponential coordinates and its covariance (6x6) as a shared pointer to a TwistWithCovariance ROS message

Returns:: - Shared pointer to a ROS message containing the believed pose and its covariance

Definition at line 707 of file RBFilter.cpp.

geometry_msgs::PoseWithCovariancePtr RBFilter::getPoseWithCovariance ( ) const [virtual]

Get the current believed pose and its covariance (6x6) as a shared pointer to a PoseWithCovariance ROS message

Returns:: - Shared pointer to a ROS message containing the believed pose and its covariance

Definition at line 688 of file RBFilter.cpp.

FeatureCloudPCLwc RBFilter::getPredictedMeasurement ( PredictionHypothesis hypothesis = BASED_ON_VELOCITY ) [virtual]

Get a point cloud with the predicted next 3D locations of the supporting Features of this RB supposing the RB moves with the tracked velocity Implements the feedback to lower inference levels (FeatureTracker)

Returns:: - Point cloud with the 3D predicted locations of the supporting Features

Definition at line 751 of file RBFilter.cpp.

std::vector< Feature::Id > RBFilter::getSupportingFeatures ( ) const [virtual]

Get the IDs of the current Features support this RB

Returns:: - Vector of IDs of the Features that support this RB

Definition at line 769 of file RBFilter.cpp.

std::vector< Eigen::Matrix4d > RBFilter::getTrajectory ( ) const [virtual]

Get the temporal series of poses of this RB

Returns:: - Vector of twists of the poses of this RB over time

Definition at line 746 of file RBFilter.cpp.

Eigen::Twistd RBFilter::getVelocity ( ) const [virtual]

Get the current believed velocity in exponential coordinates

Parameters:

hypothesis - hypothesis from which we want to get the believed velocity: velocity-update or brake-event update (the later with be zero)

Returns:: - Eigen twist containing the exponential coordinates of the current believed velocity

Definition at line 678 of file RBFilter.cpp.

Eigen::Matrix< double, 6, 6 > RBFilter::getVelocityCovariance ( ) const [virtual]

Get the covariance of the current believed velocity

Parameters:

hypothesis - hypothesis from which we want to get the covariance of the believed velocity: velocity-update or brake-event update

Returns:: - Eigen matrix containing the covariance of the current believed velocity

Definition at line 683 of file RBFilter.cpp.

geometry_msgs::TwistWithCovariancePtr RBFilter::getVelocityWithCovariance ( ) const [virtual]

Get the current believed velocity and its covariance (6x6) as a shared pointer to a TwistWithCovariance ROS message

Parameters:

hypothesis - hypothesis from which we want to get the believed twist: velocity-update or brake-event update

Returns:: - Shared pointer to a ROS message containing the believed twist and its covariance

Definition at line 727 of file RBFilter.cpp.

void RBFilter::Init ( ) [virtual]

Reimplemented in omip::StaticEnvironmentFilter.

Definition at line 65 of file RBFilter.cpp.

void RBFilter::integrateShapeBasedPose	(	const geometry_msgs::TwistWithCovariance	twist_refinement,
		double	pose_time_elapsed_ns
	)		`[virtual]`

Definition at line 526 of file RBFilter.cpp.

void RBFilter::PredictFeatureLocation	(	Feature::Ptr	feature,
		const Eigen::Matrix4d &	predicted_pose,
		bool	contemporary_last_feat_loc_and_last_pose_in_trajectory,
		Feature::Location &	predicted_location,
		bool	publish_locations = `false`
	)		const `[virtual]`

Predict the location of a Feature for one of the two possible event hypotheses

Parameters:

feature	- Feature whose location we want to predict using this RB
predicted_pose	- Predicted pose as twist
contemporary_last_feat_loc_and_last_pose_in_trajectory	- True if the last location and the last entry of the _trajectory vector are of the same time step This is the case only at the end of the loop, after the correction step (when we predict the next feature location.

Returns:: - Predicted location of the Feature

Definition at line 577 of file RBFilter.cpp.

void RBFilter::predictMeasurement ( ) [virtual]

Second step when updating the filter. The next measurement is predicted from the predicted next state.

Definition at line 147 of file RBFilter.cpp.

void RBFilter::predictState ( double time_interval_ns = -1. ) [virtual]

Predict the next pose of the RB based on the current pose and the current velocity

Reimplemented in omip::StaticEnvironmentFilter.

Definition at line 91 of file RBFilter.cpp.

void omip::RBFilter::resetFeaturesAtBirth ( ) [inline]

Definition at line 424 of file RBFilter.h.

void omip::RBFilter::resetFeaturesPredicted ( ) [inline]

Definition at line 429 of file RBFilter.h.

virtual void omip::RBFilter::setCovarianceSystemAccelerationRx ( double v ) [inline, virtual]

Definition at line 374 of file RBFilter.h.

virtual void omip::RBFilter::setCovarianceSystemAccelerationRy ( double v ) [inline, virtual]

Definition at line 379 of file RBFilter.h.

virtual void omip::RBFilter::setCovarianceSystemAccelerationRz ( double v ) [inline, virtual]

Definition at line 384 of file RBFilter.h.

virtual void omip::RBFilter::setCovarianceSystemAccelerationTx ( double v ) [inline, virtual]

Definition at line 359 of file RBFilter.h.

virtual void omip::RBFilter::setCovarianceSystemAccelerationTy ( double v ) [inline, virtual]

Definition at line 364 of file RBFilter.h.

virtual void omip::RBFilter::setCovarianceSystemAccelerationTz ( double v ) [inline, virtual]

Definition at line 369 of file RBFilter.h.

virtual void omip::RBFilter::setEstimationThreshold ( double v ) [inline, virtual]

Definition at line 399 of file RBFilter.h.

void RBFilter::setFeaturesDatabase ( FeaturesDataBase::Ptr feats_database ) [virtual]

Set the Feature data base to be used by this RB

Parameters:

feats_database - Shared pointer to the Feature data base

Definition at line 572 of file RBFilter.cpp.

virtual void omip::RBFilter::setMeasurementDepthFactor ( double v ) [inline, virtual]

Definition at line 354 of file RBFilter.h.

virtual void omip::RBFilter::setMinCovarianceMeasurementX ( double v ) [inline, virtual]

Definition at line 339 of file RBFilter.h.

virtual void omip::RBFilter::setMinCovarianceMeasurementY ( double v ) [inline, virtual]

Definition at line 344 of file RBFilter.h.

virtual void omip::RBFilter::setMinCovarianceMeasurementZ ( double v ) [inline, virtual]

Definition at line 349 of file RBFilter.h.

virtual void omip::RBFilter::setMinNumberOfSupportingFeaturesToCorrectPredictedState ( int v ) [inline, virtual]

Definition at line 394 of file RBFilter.h.

virtual void omip::RBFilter::setMotionConstraint ( int motion_constraint ) [inline, virtual]

Set a constraint for the motion of the RB Right now this is only used by the StaticEnvironmentFilter.

Parameters:

motion_constraint Value of the motion constraint. The meaning of each value is in motion_estimation.h

Reimplemented in omip::StaticEnvironmentFilter.

Definition at line 327 of file RBFilter.h.

virtual void omip::RBFilter::setNumberOfTrackedFeatures ( int v ) [inline, virtual]

Definition at line 389 of file RBFilter.h.

void RBFilter::setPredictedState ( omip_msgs::RigidBodyPoseAndVelMsg hypothesis ) [virtual]

Add a new prediction of the next pose and velocity of this rigid body, computed by the higher level (JointTracker) Implements the feedback from higher inference levels (JointTracker)

Parameters:

twist_hyp - Prediction of the next rigid body pose and velocity (with covariance)

Definition at line 498 of file RBFilter.cpp.

virtual void omip::RBFilter::setPriorCovariancePose ( double v ) [inline, virtual]

Definition at line 329 of file RBFilter.h.

virtual void omip::RBFilter::setPriorCovarianceVelocity ( double v ) [inline, virtual]

Definition at line 334 of file RBFilter.h.