omip Namespace Reference

Classes
struct	_FeaturePCLwc
class	Feature
struct	FeaturePCLwc
class	FeaturesDataBase
class	MultiRBTracker
class	MultiRBTrackerNode
class	RBFilter
class	RecursiveEstimatorFilterInterface
class	RecursiveEstimatorNodeInterface
class	StaticEnvironmentFilter

Typedefs
typedef pcl::PointCloud< FeaturePCL >	FeatureCloudPCL
typedef pcl::PointCloud< FeaturePCLwc >	FeatureCloudPCLwc
typedef pcl::PointXYZL	FeaturePCL
typedef void	ft_measurement_ros_t
typedef std::pair< cv_bridge::CvImagePtr, cv_bridge::CvImagePtr >	ft_measurement_t
typedef sensor_msgs::PointCloud2	ft_state_ros_t
typedef FeatureCloudPCLwc::Ptr	ft_state_t
typedef long int	Joint_id_t
typedef std::pair< omip_msgs::RigidBodyPoseAndVelMsg, omip_msgs::RigidBodyPoseAndVelMsg >	joint_measurement_t
typedef std::map< std::pair< int, int >, boost::shared_ptr< JointCombinedFilter > >	KinematicModel
typedef omip_msgs::RigidBodyPosesAndVelsMsg	ks_measurement_ros_t
typedef omip_msgs::RigidBodyPosesAndVelsMsg	ks_measurement_t
typedef omip_msgs::KinematicStructureMsg::Ptr	ks_state_ros_t
typedef KinematicModel	ks_state_t
typedef pcl::PointCloud< pcl::PointXYZRGB >	PointCloudPCL
typedef pcl::PointCloud< PointPCL >	PointCloudPCLNoColor
typedef pcl::PointXYZ	PointPCL
typedef long int	RB_id_t
typedef sensor_msgs::PointCloud2	rbt_measurement_ros_t
typedef FeatureCloudPCLwc::Ptr	rbt_measurement_t
typedef omip_msgs::RigidBodyPosesAndVelsMsg	rbt_state_ros_t
typedef omip_msgs::RigidBodyPosesAndVelsMsg	rbt_state_t
typedef pcl::PointCloud< PointPCL >	RigidBodyShape

Enumerations
enum	MotionConstraint {   NO_CONSTRAINED = 0, NO_ROLL_PITCH = 1, NO_ROLL_PITCH_TZ = 2, NO_TRANSLATION = 3,   NO_TRANSLATION_ROLL_YAW = 4, NO_ROTATION = 5, NO_MOTION = 6, ROBOT_XY_BASELINK_PLANE = 7 }
enum	shape_model_selector_t
enum	static_environment_tracker_t

Functions
void	EigenAffine2TranslationAndEulerAngles (const Eigen::Affine3d &t, double &x, double &y, double &z, double &roll, double &pitch, double &yaw)
void	EigenTwist2GeometryMsgsTwist (Eigen::Twistd &eigen_twist, geometry_msgs::Twist &gm_twist)
void	GeometryMsgsTwist2EigenTwist (const geometry_msgs::Twist &gm_twist, Eigen::Twistd &eigen_twist)
void	invert3x3Matrix (const MatrixWrapper::Matrix &to_inv, MatrixWrapper::Matrix &inverse)
void	invert3x3MatrixEigen (const Eigen::Map< Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > > &to_inv, Eigen::Map< Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > > &inverse)
void	invert3x3MatrixEigen2 (const Eigen::Matrix3d &to_inv, Eigen::Matrix3d &inverse)
Eigen::Twistd	invertTwist (Eigen::Twistd &current_twist, Eigen::Twistd &previous_twist, bool &inverted)
bool	isFinite (const Eigen::Matrix4d &transformation)
double	L2Distance (const Feature::Location &first, const Feature::Location &second)
void	Location2PointPCL (const Feature::Location &point_location, pcl::PointXYZ &point_pcl)
void	LocationAndId2FeaturePCL (const Feature::Location &feature_location, const Feature::Id &feature_id, pcl::PointXYZL &feature_pcl)
void	LocationAndId2FeaturePCLwc (const Feature::Location &feature_location, const Feature::Id &feature_id, omip::FeaturePCLwc &feature_pcl)
void	LocationOfFeature2ColumnVector (const Feature::Location &lof, MatrixWrapper::ColumnVector &col_vec)
void	LocationOfFeature2ColumnVectorHomogeneous (const Feature::Location &lof, MatrixWrapper::ColumnVector &col_vec)
void	LocationOfFeature2EigenVectorHomogeneous (const Feature::Location &lof, Eigen::Vector4d &eig_vec)
Feature::Location	operator+ (const Feature::Location &location1, const Feature::Location &location2)
Feature::Location	operator- (const Feature::Location &location1, const Feature::Location &location2)
std::ostream &	operator<< (std::ostream &os, std::vector< Feature::Id > vector_ids)
std::ostream &	operator<< (std::ostream &os, Eigen::Twistd twistd)
std::ostream &	operator<< (std::ostream &os, Feature::Location location)
void	ROSTwist2EigenTwist (const geometry_msgs::Twist &ros_twist, Eigen::Twistd &eigen_twist)
double	sampleNormal (double mean, double std_dev)
void	TransformLocation (const Feature::Location &origin, const Eigen::Matrix4d &transformation, Feature::Location &new_location)
void	TransformLocation (const Feature::Location &origin, const geometry_msgs::Twist &twist, Feature::Location &new_location)
void	TransformLocation (const Feature::Location &origin, const Eigen::Twistd &twist, Feature::Location &new_location, int feat_id=0)
void	TransformMatrix2Twist (const Eigen::Matrix4d &transformation_matrix, Eigen::Twistd &twist)
void	TransformMatrix2TwistUnwrapped (const Eigen::Matrix4d &transformation_matrix, Eigen::Twistd &twist, const Eigen::Twistd &twist_previous)
void	TranslationAndEulerAngles2EigenAffine (const double &x, const double &y, const double &z, const double &roll, const double &pitch, const double &yaw, Eigen::Transform< double, 3, Eigen::Affine > &t)
void	Twist2TransformMatrix (const Eigen::Twistd &transformation_twist, Eigen::Matrix4d &matrix)
Eigen::Twistd	unwrapTwist (Eigen::Twistd &current_twist, Eigen::Displacementd &current_displacement, Eigen::Twistd &previous_twist, bool &changed)

Variables
	BASED_ON_COLOR
	BASED_ON_DEPTH
	BASED_ON_EXT_COLOR
	BASED_ON_EXT_DEPTH
	BASED_ON_EXT_DEPTH_AND_COLOR
	STATIC_ENVIRONMENT_EKF_TRACKER
	STATIC_ENVIRONMENT_ICP_TRACKER

Detailed Description

NOTE: The purpose of this program is to construct a kalman filter (EKF) for the problem of tracking a rigid body from a varying set of 3d tracked points.

We maintain in parallel two linear SYSTEM MODELS (leading to two EKFs) to update the state based on the previous step (and an input signal that in our EKF is always 0)

• One model supposes that the rigid body moves with the velocity estimated in the previous step (pose'=pose+velocity) pose_k = pose_{k-1} + v_{k-1} delta_t
• One model supposes that the rigid body stops abruptly (p'=p) pose_k = pose_{k-1}

The MEASUREMENT MODEL predicts the next measurement based on the next state predicted by the SYSTEM MODEL. The MEASUREMENT MODEL of our two parallel EKFs is: z = M*[(pose_k)*initial_point_location]

Enumeration Type Documentation

enum omip::MotionConstraint

Enumerator
NO_CONSTRAINED
NO_ROLL_PITCH
NO_ROLL_PITCH_TZ
NO_TRANSLATION
NO_TRANSLATION_ROLL_YAW
NO_ROTATION
NO_MOTION
ROBOT_XY_BASELINK_PLANE

Definition at line 43 of file StaticEnvironmentFilter.h.