#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. More...

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. More...

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. More...

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 ( )

protectedvirtual

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 781 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 550 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 490 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 191 of file RBFilter.cpp.

virtual RBFilter* omip::RBFilter::doClone ( ) const

inlineprotectedvirtual

Definition at line 539 of file RBFilter.h.

virtual void omip::RBFilter::doNotUsePredictionFromHigherLevel ( )

inlinevirtual

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 373 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 776 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 660 of file RBFilter.cpp.

virtual Feature::Location omip::RBFilter::getIntialLocationOfCentroid ( ) const

inlinevirtual

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 616 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 771 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 665 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 670 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 704 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 685 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 748 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 766 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 743 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 675 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 680 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 724 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 523 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 574 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 142 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 )

inlinevirtual

Definition at line 374 of file RBFilter.h.

virtual void omip::RBFilter::setCovarianceSystemAccelerationRy ( double v )

inlinevirtual

Definition at line 379 of file RBFilter.h.

virtual void omip::RBFilter::setCovarianceSystemAccelerationRz ( double v )

inlinevirtual

Definition at line 384 of file RBFilter.h.

virtual void omip::RBFilter::setCovarianceSystemAccelerationTx ( double v )

inlinevirtual

Definition at line 359 of file RBFilter.h.

virtual void omip::RBFilter::setCovarianceSystemAccelerationTy ( double v )

inlinevirtual

Definition at line 364 of file RBFilter.h.

virtual void omip::RBFilter::setCovarianceSystemAccelerationTz ( double v )

inlinevirtual

Definition at line 369 of file RBFilter.h.

virtual void omip::RBFilter::setEstimationThreshold ( double v )

inlinevirtual

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 569 of file RBFilter.cpp.

virtual void omip::RBFilter::setMeasurementDepthFactor ( double v )

inlinevirtual

Definition at line 354 of file RBFilter.h.

virtual void omip::RBFilter::setMinCovarianceMeasurementX ( double v )

inlinevirtual

Definition at line 339 of file RBFilter.h.

virtual void omip::RBFilter::setMinCovarianceMeasurementY ( double v )

inlinevirtual

Definition at line 344 of file RBFilter.h.

virtual void omip::RBFilter::setMinCovarianceMeasurementZ ( double v )

inlinevirtual

Definition at line 349 of file RBFilter.h.

virtual void omip::RBFilter::setMinNumberOfSupportingFeaturesToCorrectPredictedState ( int v )

inlinevirtual

Definition at line 394 of file RBFilter.h.

virtual void omip::RBFilter::setMotionConstraint ( int motion_constraint )

inlinevirtual

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 )

inlinevirtual

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 495 of file RBFilter.cpp.

virtual void omip::RBFilter::setPriorCovariancePose ( double v )

inlinevirtual

Definition at line 329 of file RBFilter.h.

virtual void omip::RBFilter::setPriorCovarianceVelocity ( double v )

inlinevirtual

Definition at line 334 of file RBFilter.h.

Member Data Documentation

double omip::RBFilter::_cov_sys_acc_rx

protected

Definition at line 465 of file RBFilter.h.

double omip::RBFilter::_cov_sys_acc_ry

protected

Definition at line 464 of file RBFilter.h.

double omip::RBFilter::_cov_sys_acc_rz

protected

Definition at line 466 of file RBFilter.h.

double omip::RBFilter::_cov_sys_acc_tx

protected

Definition at line 461 of file RBFilter.h.

double omip::RBFilter::_cov_sys_acc_ty

protected

Definition at line 462 of file RBFilter.h.

double omip::RBFilter::_cov_sys_acc_tz

protected

Definition at line 463 of file RBFilter.h.

Eigen::Matrix<double, 3, 6> omip::RBFilter::_D_T_p0_circle

protected

Definition at line 472 of file RBFilter.h.

double omip::RBFilter::_estimation_error_threshold

protected

Definition at line 451 of file RBFilter.h.

FeatureCloudPCLwc::Ptr omip::RBFilter::_features_at_birth

protected

Definition at line 522 of file RBFilter.h.

FeaturesDataBase::Ptr omip::RBFilter::_features_database

protected

Definition at line 498 of file RBFilter.h.

FeatureCloudPCLwc::Ptr omip::RBFilter::_features_predicted

protected

Definition at line 521 of file RBFilter.h.

Eigen::MatrixXd omip::RBFilter::_G_t_memory

protected

Definition at line 469 of file RBFilter.h.

RB_id_t omip::RBFilter::_id

protected

Definition at line 476 of file RBFilter.h.

Feature::Location omip::RBFilter::_initial_location_of_centroid

protected

Definition at line 491 of file RBFilter.h.

double omip::RBFilter::_last_time_interval_ns

protected

Definition at line 537 of file RBFilter.h.

double omip::RBFilter::_loop_period_ns

protected

Definition at line 536 of file RBFilter.h.

double omip::RBFilter::_meas_depth_factor

protected

Definition at line 460 of file RBFilter.h.

double omip::RBFilter::_min_cov_meas_x

protected

Definition at line 457 of file RBFilter.h.

double omip::RBFilter::_min_cov_meas_y

protected

Definition at line 458 of file RBFilter.h.

double omip::RBFilter::_min_cov_meas_z

protected

Definition at line 459 of file RBFilter.h.

int omip::RBFilter::_min_num_supporting_feats_to_correct

protected

Definition at line 545 of file RBFilter.h.

int omip::RBFilter::_num_tracked_feats

protected

Definition at line 544 of file RBFilter.h.

Eigen::Matrix4d omip::RBFilter::_pose

protected

Definition at line 479 of file RBFilter.h.

Eigen::Matrix<double, 6, 6> omip::RBFilter::_pose_covariance

protected

Definition at line 480 of file RBFilter.h.

Eigen::Matrix4d omip::RBFilter::_predicted_delta_pose_kh

protected

Definition at line 503 of file RBFilter.h.

FeatureCloudPCLwc::Ptr omip::RBFilter::_predicted_measurement

protected

Definition at line 528 of file RBFilter.h.

std::map<Feature::Id, Feature::Location> omip::RBFilter::_predicted_measurement_map

protected

Definition at line 534 of file RBFilter.h.

std::map<Feature::Id, Feature::Location> omip::RBFilter::_predicted_measurement_map_bh

protected

Definition at line 531 of file RBFilter.h.

std::map<Feature::Id, Feature::Location> omip::RBFilter::_predicted_measurement_map_kh

protected

Definition at line 532 of file RBFilter.h.

std::map<Feature::Id, Feature::Location> omip::RBFilter::_predicted_measurement_map_vh

protected

Definition at line 530 of file RBFilter.h.

FeatureCloudPCLwc::Ptr omip::RBFilter::_predicted_measurement_pc_bh

protected

Definition at line 525 of file RBFilter.h.

FeatureCloudPCLwc::Ptr omip::RBFilter::_predicted_measurement_pc_kh

protected

Definition at line 526 of file RBFilter.h.

FeatureCloudPCLwc::Ptr omip::RBFilter::_predicted_measurement_pc_vh

protected

Definition at line 524 of file RBFilter.h.

Eigen::Matrix4d omip::RBFilter::_predicted_pose

protected

Definition at line 485 of file RBFilter.h.

Eigen::Matrix4d omip::RBFilter::_predicted_pose_bh

protected

Definition at line 502 of file RBFilter.h.

Eigen::Matrix<double, 6, 6> omip::RBFilter::_predicted_pose_cov_bh

protected

Definition at line 507 of file RBFilter.h.

Eigen::Matrix<double, 6, 6> omip::RBFilter::_predicted_pose_cov_kh

protected

Definition at line 508 of file RBFilter.h.

Eigen::Matrix<double, 6, 6> omip::RBFilter::_predicted_pose_cov_vh

protected

Definition at line 506 of file RBFilter.h.

Eigen::Matrix<double, 6, 6> omip::RBFilter::_predicted_pose_covariance

protected

Definition at line 486 of file RBFilter.h.

Eigen::Matrix4d omip::RBFilter::_predicted_pose_kh

protected

Definition at line 504 of file RBFilter.h.

Eigen::Matrix4d omip::RBFilter::_predicted_pose_vh

protected

Definition at line 501 of file RBFilter.h.

Eigen::Twistd omip::RBFilter::_predicted_velocity

protected

Definition at line 488 of file RBFilter.h.

Eigen::Twistd omip::RBFilter::_predicted_velocity_bh

protected

Definition at line 511 of file RBFilter.h.

Eigen::Matrix<double, 6, 6> omip::RBFilter::_predicted_velocity_cov_bh

protected

Definition at line 515 of file RBFilter.h.

Eigen::Matrix<double, 6, 6> omip::RBFilter::_predicted_velocity_cov_kh

protected

Definition at line 516 of file RBFilter.h.

Eigen::Matrix<double, 6, 6> omip::RBFilter::_predicted_velocity_cov_vh

protected

Definition at line 514 of file RBFilter.h.

Eigen::Matrix<double, 6, 6> omip::RBFilter::_predicted_velocity_covariance

protected

Definition at line 489 of file RBFilter.h.

Eigen::Twistd omip::RBFilter::_predicted_velocity_kh

protected

Definition at line 512 of file RBFilter.h.

Eigen::Twistd omip::RBFilter::_predicted_velocity_vh

protected

Definition at line 510 of file RBFilter.h.

double omip::RBFilter::_prior_cov_pose

protected

Definition at line 455 of file RBFilter.h.

double omip::RBFilter::_prior_cov_vel

protected

Definition at line 456 of file RBFilter.h.

Eigen::VectorXd omip::RBFilter::_R_inv_times_innovation_memory

protected

Definition at line 470 of file RBFilter.h.

RB_id_t RBFilter::_rb_id_generator = 1

staticprotected

Definition at line 474 of file RBFilter.h.

std::map<Feature::Id, double> omip::RBFilter::_supporting_feats_errors

protected

Definition at line 453 of file RBFilter.h.

std::vector<Feature::Id> omip::RBFilter::_supporting_features_ids

protected

Definition at line 495 of file RBFilter.h.

std::vector<double> omip::RBFilter::_supporting_features_probs

protected

Definition at line 496 of file RBFilter.h.

std::vector<Eigen::Matrix4d> omip::RBFilter::_trajectory

protected

Definition at line 493 of file RBFilter.h.

bool omip::RBFilter::_use_predicted_measurement_from_kh

protected

Definition at line 519 of file RBFilter.h.

bool omip::RBFilter::_use_predicted_state_from_kh

protected

Definition at line 518 of file RBFilter.h.

Eigen::Twistd omip::RBFilter::_velocity

protected

Definition at line 482 of file RBFilter.h.

Eigen::Matrix<double, 6, 6> omip::RBFilter::_velocity_covariance

protected

Definition at line 483 of file RBFilter.h.

EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef boost::shared_ptr<RBFilter> omip::RBFilter::Ptr

Definition at line 105 of file RBFilter.h.

The documentation for this class was generated from the following files:

Public Types

Public Member Functions

Public Attributes

Protected Member Functions

Protected Attributes

Static Protected Attributes

Detailed Description

Member Typedef Documentation

Member Enumeration Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation