#include <StaticEnvironmentFilter.h>

Inheritance diagram for omip::StaticEnvironmentFilter:

Public Types
typedef boost::shared_ptr< StaticEnvironmentFilter >	Ptr

Public Types inherited from omip::RBFilter
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	addSupportingFeature (Feature::Id supporting_feat_id)

virtual void	constrainMotion ()
	Function that constrains the last estimated pose of the static environment to the camera according to the motion constraint. More...

virtual void	correctState ()

virtual void	estimateDeltaMotion (std::vector< Feature::Id > &supporting_features_ids, tf::Transform &previous_current_Tf)
	Estimate the motion between the last 2 frames of the set of supporting features, assuming they are all on the static environment. More...

virtual void	Init ()

virtual void	iterativeICP (pcl::PointCloud< pcl::PointXYZ > &previous_locations, pcl::PointCloud< pcl::PointXYZ > &current_locations, tf::Transform &previous_current_Tf)
	Estimate the transformation between 2 sets of feature locations by iteratively estimating ICP until either the maximum number of iterations is reached or the iterations do not change the transformation (convergence) More...

virtual void	predictState (double time_interval_ns)

virtual void	setComputationType (static_environment_tracker_t computation_type)
	Set the computation type for the static environment Options: ICP based (t-1 to t) or EKF based (similar to any other RB) More...

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

	StaticEnvironmentFilter (double loop_period_ns, FeaturesDataBase::Ptr feats_database, double environment_motion_threshold)

virtual	~StaticEnvironmentFilter ()

Public Member Functions inherited from omip::RBFilter
virtual void	addPredictedFeatureLocation (const Feature::Location &predicted_location_velocity, const Feature::Location &predicted_location_brake, const Feature::Id feature_id)

RBFilter::Ptr	clone () const

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

	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	setNumberOfTrackedFeatures (int v)

virtual void	setPredictedState (omip_msgs::RigidBodyPoseAndVelMsg hypothesis)

virtual void	setPriorCovariancePose (double v)

virtual void	setPriorCovarianceVelocity (double v)

virtual	~RBFilter ()

Private Attributes
static_environment_tracker_t	_computation_type

double	_environment_motion_threshold

int	_max_iterations

int	_motion_constraint

ros::NodeHandle	_nh

pcl::registration::TransformationEstimationSVD< pcl::PointXYZ, pcl::PointXYZ >	_svdecomposer

double	_tf_epsilon_angular

double	_tf_epsilon_linear

tf::TransformListener	_tf_listener

Additional Inherited Members
Public Attributes inherited from omip::RBFilter
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef boost::shared_ptr< RBFilter >	Ptr

Protected Member Functions inherited from omip::RBFilter
virtual void	_initializeAuxiliarMatrices ()

virtual RBFilter *	doClone () const

Protected Attributes inherited from omip::RBFilter
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 inherited from omip::RBFilter
static RB_id_t	_rb_id_generator = 1

Detailed Description

Class StaticEnvironmentICPFilter Represents an special type of RBFilter that tracks the motion of the static environment wrt to the camera Features of the environment that are not moving in the environment support this filter and should be used to estimate the motion of the environment wrt to the camera

Definition at line 63 of file StaticEnvironmentFilter.h.

Member Typedef Documentation

typedef boost::shared_ptr<StaticEnvironmentFilter> omip::StaticEnvironmentFilter::Ptr

Definition at line 68 of file StaticEnvironmentFilter.h.

Constructor & Destructor Documentation

StaticEnvironmentFilter::StaticEnvironmentFilter	(	double	loop_period_ns,
		FeaturesDataBase::Ptr	feats_database,
		double	environment_motion_threshold
	)

Constructor

Parameters

static_motion_threshold - Threshold to detect Features that move (do not support the static rigid body any longer)

Definition at line 35 of file StaticEnvironmentFilter.cpp.

virtual omip::StaticEnvironmentFilter::~StaticEnvironmentFilter ( )

inlinevirtual

Destructor

Definition at line 79 of file StaticEnvironmentFilter.h.

Member Function Documentation

void StaticEnvironmentFilter::addSupportingFeature ( Feature::Id supporting_feat_id )

virtual

Add the ID of a feature to the list of supporting features of this RB This function is called when a new feature is added to the list of tracked features We create a prediction about its next position (the same it has now) to be used in the support

Parameters

supporting_feat_id - ID of the Feature that now supports this RB

Reimplemented from omip::RBFilter.

Definition at line 172 of file StaticEnvironmentFilter.cpp.

void StaticEnvironmentFilter::constrainMotion ( )

virtual

Function that constrains the last estimated pose of the static environment to the camera according to the motion constraint.

Definition at line 260 of file StaticEnvironmentFilter.cpp.

void StaticEnvironmentFilter::correctState ( )

virtual

Correct Correct the predicted next state based on the supporting Features For the StaticRB the state (pose and velocity) is always zero

Reimplemented from omip::RBFilter.

Definition at line 100 of file StaticEnvironmentFilter.cpp.

void StaticEnvironmentFilter::estimateDeltaMotion	(	std::vector< Feature::Id > &	supporting_features_ids,
		tf::Transform &	previous_current_Tf
	)

virtual

Estimate the motion between the last 2 frames of the set of supporting features, assuming they are all on the static environment.

Parameters

supporting_features_ids	IDs of the features that support the static environment
previous_current_Tf	Resulting transformation as a tf transform. It is the transformation of the static environment from the previous to the current frame

Definition at line 193 of file StaticEnvironmentFilter.cpp.

void StaticEnvironmentFilter::Init ( )

virtual

Reimplemented from omip::RBFilter.

Definition at line 62 of file StaticEnvironmentFilter.cpp.

void StaticEnvironmentFilter::iterativeICP	(	pcl::PointCloud< pcl::PointXYZ > &	previous_locations,
		pcl::PointCloud< pcl::PointXYZ > &	current_locations,
		tf::Transform &	previous_current_Tf
	)

virtual

Estimate the transformation between 2 sets of feature locations by iteratively estimating ICP until either the maximum number of iterations is reached or the iterations do not change the transformation (convergence)

Parameters

previous_locations	Point cloud with the previous locations of the features
current_locations	Point cloud with the current locations of the features
previous_current_Tf	Resulting transformation as a tf transform. It is the transformation of the static environment from the previous to the current frame

Definition at line 217 of file StaticEnvironmentFilter.cpp.

void StaticEnvironmentFilter::predictState ( double time_interval_ns )

virtual

Predict Generate a prediction of the next state based on the previous state For the StaticRB the state (pose and velocity) is always zero

Reimplemented from omip::RBFilter.

Definition at line 69 of file StaticEnvironmentFilter.cpp.

void StaticEnvironmentFilter::setComputationType ( static_environment_tracker_t computation_type )

virtual

Set the computation type for the static environment Options: ICP based (t-1 to t) or EKF based (similar to any other RB)

Parameters

computation_type Value of the type of computation (defined in omip_common/OMIPTypeDefs.h)

Definition at line 167 of file StaticEnvironmentFilter.cpp.

void StaticEnvironmentFilter::setMotionConstraint ( int motion_constraint )

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 from omip::RBFilter.

Definition at line 162 of file StaticEnvironmentFilter.cpp.

Member Data Documentation

static_environment_tracker_t omip::StaticEnvironmentFilter::_computation_type

private

Definition at line 161 of file StaticEnvironmentFilter.h.

double omip::StaticEnvironmentFilter::_environment_motion_threshold

private

Definition at line 151 of file StaticEnvironmentFilter.h.

int omip::StaticEnvironmentFilter::_max_iterations

private

Definition at line 154 of file StaticEnvironmentFilter.h.

int omip::StaticEnvironmentFilter::_motion_constraint

private

Definition at line 152 of file StaticEnvironmentFilter.h.

ros::NodeHandle omip::StaticEnvironmentFilter::_nh

private

Definition at line 158 of file StaticEnvironmentFilter.h.

pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> omip::StaticEnvironmentFilter::_svdecomposer

private

Definition at line 163 of file StaticEnvironmentFilter.h.

double omip::StaticEnvironmentFilter::_tf_epsilon_angular

private

Definition at line 156 of file StaticEnvironmentFilter.h.

double omip::StaticEnvironmentFilter::_tf_epsilon_linear

private

Definition at line 155 of file StaticEnvironmentFilter.h.

tf::TransformListener omip::StaticEnvironmentFilter::_tf_listener

private

Definition at line 159 of file StaticEnvironmentFilter.h.

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

Public Types

Public Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Member Typedef Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation