Classes
struct	_FeaturePCLwc
class	Feature
	Minimal class that represents a Point of Interest by an ID and stores its successive locations $Author: Martin. More...
struct	FeaturePCLwc
class	FeaturesDataBase
class	RecursiveEstimatorFilterInterface
	This class defines an INTERFACE that all recursive estimator filters need to implement It is ROS free It is templated on the type of the state that it estimates and the type of measurements it accepts as input TODO: For multimodality I will need different measurement types. More...
class	RecursiveEstimatorNodeInterface
	This class defines an INTERFACE that all recursive estimator NODES need to implement It uses ROS TODO: For multimodality I will need different measurement types. More...
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 Eigen::Matrix< double, 6, 6 >	Matrix6d
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	shape_model_selector_t { BASED_ON_DEPTH = 1, BASED_ON_COLOR = 2, BASED_ON_EXT_DEPTH = 3, BASED_ON_EXT_COLOR = 4, BASED_ON_EXT_DEPTH_AND_COLOR = 5 }
enum	static_environment_tracker_t { STATIC_ENVIRONMENT_EKF_TRACKER = 1, STATIC_ENVIRONMENT_ICP_TRACKER = 2 }
Functions
void	adjointXcovXadjointT (const Eigen::Twistd &pose_ec, const Eigen::Matrix< double, 6, 6 > &cov, Eigen::Matrix< double, 6, 6 > &transformed_cov_out)
	adjointXcovXadjointT Transforms a covariance from one reference frame to another
void	adjointXcovXadjointT (const Eigen::Displacementd &pose_disp, const Eigen::Matrix< double, 6, 6 > &cov, Eigen::Matrix< double, 6, 6 > &transformed_cov_out)
void	adjointXinvAdjointXcovXinvAdjointTXadjointT (const Eigen::Displacementd &pose_disp1, const Eigen::Displacementd &pose_disp2, const Eigen::Matrix< double, 6, 6 > &cov, Eigen::Matrix< double, 6, 6 > &transformed_cov_out)
void	computeAdjoint (const Eigen::Twistd &pose_ec, Eigen::Matrix< double, 6, 6 > &adjoint_out)
	computeAdjoint Returns the adjoint matrix of the given pose. Necessary because the LGSM library represents first rotation and then translation while we represent first translation and then rotation
void	computeAdjoint (const Eigen::Displacementd pose_disp, Eigen::Matrix< double, 6, 6 > &adjoint_out)
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, Feature::Location location)
std::ostream &	operator<< (std::ostream &os, Eigen::Twistd twistd)
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 Eigen::Twistd &twist, Feature::Location &new_location, int feat_id=0)
void	TransformLocation (const Feature::Location &origin, const geometry_msgs::Twist &twist, Feature::Location &new_location)
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)

Typedef Documentation

typedef pcl::PointCloud<FeaturePCL> omip::FeatureCloudPCL

Definition at line 107 of file OMIPTypeDefs.h.

typedef pcl::PointCloud<FeaturePCLwc> omip::FeatureCloudPCLwc

Definition at line 108 of file OMIPTypeDefs.h.

typedef pcl::PointXYZL omip::FeaturePCL

Definition at line 76 of file OMIPTypeDefs.h.

typedef void omip::ft_measurement_ros_t

Definition at line 120 of file OMIPTypeDefs.h.

typedef std::pair<cv_bridge::CvImagePtr, cv_bridge::CvImagePtr> omip::ft_measurement_t

Definition at line 123 of file OMIPTypeDefs.h.

typedef sensor_msgs::PointCloud2 omip::ft_state_ros_t

Definition at line 126 of file OMIPTypeDefs.h.

typedef FeatureCloudPCLwc::Ptr omip::ft_state_t

Definition at line 129 of file OMIPTypeDefs.h.

typedef long int omip::Joint_id_t

Definition at line 111 of file OMIPTypeDefs.h.

typedef std::pair<omip_msgs::RigidBodyPoseAndVelMsg, omip_msgs::RigidBodyPoseAndVelMsg> omip::joint_measurement_t

Definition at line 156 of file OMIPTypeDefs.h.

typedef std::map<std::pair<int, int>, boost::shared_ptr<JointCombinedFilter> > omip::KinematicModel

Definition at line 115 of file OMIPTypeDefs.h.

typedef omip_msgs::RigidBodyPosesAndVelsMsg omip::ks_measurement_ros_t

Definition at line 144 of file OMIPTypeDefs.h.

typedef omip_msgs::RigidBodyPosesAndVelsMsg omip::ks_measurement_t

Definition at line 147 of file OMIPTypeDefs.h.

typedef omip_msgs::KinematicStructureMsg::Ptr omip::ks_state_ros_t

Definition at line 150 of file OMIPTypeDefs.h.

typedef KinematicModel omip::ks_state_t

Definition at line 153 of file OMIPTypeDefs.h.

typedef Eigen::Matrix<double, 6, 6> omip::Matrix6d

Definition at line 234 of file OMIPUtils.h.

typedef pcl::PointCloud<pcl::PointXYZRGB> omip::PointCloudPCL

Definition at line 106 of file OMIPTypeDefs.h.

typedef pcl::PointCloud<PointPCL> omip::PointCloudPCLNoColor

Definition at line 105 of file OMIPTypeDefs.h.

typedef pcl::PointXYZ omip::PointPCL

Definition at line 73 of file OMIPTypeDefs.h.

typedef long int omip::RB_id_t

Definition at line 110 of file OMIPTypeDefs.h.

typedef sensor_msgs::PointCloud2 omip::rbt_measurement_ros_t

Definition at line 132 of file OMIPTypeDefs.h.

typedef FeatureCloudPCLwc::Ptr omip::rbt_measurement_t

Definition at line 135 of file OMIPTypeDefs.h.

typedef omip_msgs::RigidBodyPosesAndVelsMsg omip::rbt_state_ros_t

Definition at line 138 of file OMIPTypeDefs.h.

typedef omip_msgs::RigidBodyPosesAndVelsMsg omip::rbt_state_t

Definition at line 141 of file OMIPTypeDefs.h.

typedef pcl::PointCloud<PointPCL> omip::RigidBodyShape

Definition at line 113 of file OMIPTypeDefs.h.

Enumeration Type Documentation

enum omip::shape_model_selector_t

Enumerator:

BASED_ON_DEPTH
BASED_ON_COLOR
BASED_ON_EXT_DEPTH
BASED_ON_EXT_COLOR
BASED_ON_EXT_DEPTH_AND_COLOR

Definition at line 158 of file OMIPTypeDefs.h.

enum omip::static_environment_tracker_t

Enumerator:

STATIC_ENVIRONMENT_EKF_TRACKER
STATIC_ENVIRONMENT_ICP_TRACKER

Definition at line 167 of file OMIPTypeDefs.h.

Function Documentation

void omip::adjointXcovXadjointT	(	const Eigen::Twistd &	pose_ec,
		const Eigen::Matrix< double, 6, 6 > &	cov,
		Eigen::Matrix< double, 6, 6 > &	transformed_cov_out
	)

adjointXcovXadjointT Transforms a covariance from one reference frame to another

Parameters:

pose_ec	Pose (in exponential coordinates) where we want to have the covariance expressed
cov	Original covariance
transformed_cov	Transformed covariance

Definition at line 436 of file OMIPUtils.cpp.

void omip::adjointXcovXadjointT	(	const Eigen::Displacementd &	pose_disp,
		const Eigen::Matrix< double, 6, 6 > &	cov,
		Eigen::Matrix< double, 6, 6 > &	transformed_cov_out
	)

Definition at line 459 of file OMIPUtils.cpp.

void omip::adjointXinvAdjointXcovXinvAdjointTXadjointT	(	const Eigen::Displacementd &	pose_disp1,
		const Eigen::Displacementd &	pose_disp2,
		const Eigen::Matrix< double, 6, 6 > &	cov,
		Eigen::Matrix< double, 6, 6 > &	transformed_cov_out
	)

Definition at line 469 of file OMIPUtils.cpp.

void omip::computeAdjoint	(	const Eigen::Twistd &	pose_ec,
		Eigen::Matrix< double, 6, 6 > &	adjoint_out
	)

computeAdjoint Returns the adjoint matrix of the given pose. Necessary because the LGSM library represents first rotation and then translation while we represent first translation and then rotation

Parameters:

pose_ec	Pose (in exponential coordinates) to obtain the adjoint for
adjoint	Matrix containing the adjoint. 3 first rows are translation and 3 last rows are rotation

Definition at line 431 of file OMIPUtils.cpp.

void omip::computeAdjoint	(	const Eigen::Displacementd	pose_disp,
		Eigen::Matrix< double, 6, 6 > &	adjoint_out
	)

Same as the functions above but the pose is passed as a displacement

Definition at line 450 of file OMIPUtils.cpp.

void omip::EigenAffine2TranslationAndEulerAngles	(	const Eigen::Affine3d &	t,
		double &	x,
		double &	y,
		double &	z,
		double &	roll,
		double &	pitch,
		double &	yaw
	)

Convert an Eigen::Affine3d matrix into translation and rotation (Euler angles)

Parameters:

t	- Eigen Affine3d (input)
x	- Translation in x (output)
y	- Translation in y (output)
z	- Translation in z (output)
roll	- Rotation roll (output)
pitch	- Rotation pitch (output)
yaw	- Rotation yaw (output)

Definition at line 14 of file OMIPUtils.cpp.

void omip::EigenTwist2GeometryMsgsTwist	(	Eigen::Twistd &	eigen_twist,
		geometry_msgs::Twist &	gm_twist
	)

Convert an Eigen Twist into a ROS geometry message Twist

Parameters:

eigen_twist - Eigen twist

Returns:: - ROS geometry message Twist

Definition at line 212 of file OMIPUtils.cpp.

void omip::GeometryMsgsTwist2EigenTwist	(	const geometry_msgs::Twist &	gm_twist,
		Eigen::Twistd &	eigen_twist
	)

Convert a ROS geometry message Twist into an Eigen Twist

Parameters:

gm_twist - ROS geometry message Twist

Returns:: - Eigen twist

Definition at line 206 of file OMIPUtils.cpp.

void omip::invert3x3Matrix	(	const MatrixWrapper::Matrix &	to_inv,
		MatrixWrapper::Matrix &	inverse
	)

Definition at line 378 of file OMIPUtils.cpp.

void omip::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
	)

Definition at line 395 of file OMIPUtils.cpp.

void omip::invert3x3MatrixEigen2	(	const Eigen::Matrix3d &	to_inv,
		Eigen::Matrix3d &	inverse
	)

Definition at line 413 of file OMIPUtils.cpp.

Eigen::Twistd omip::invertTwist	(	Eigen::Twistd &	current_twist,
		Eigen::Twistd &	previous_twist,
		bool &	inverted
	)

Definition at line 358 of file OMIPUtils.cpp.

bool omip::isFinite ( const Eigen::Matrix4d & transformation )

Checks if all the elements of the Eigen matrix are finite

Parameters:

transformation - Eigen matrix to test

Returns:: - TRUE if all elements of the matrix are finite

Definition at line 222 of file OMIPUtils.cpp.

double omip::L2Distance	(	const Feature::Location &	first,
		const Feature::Location &	second
	)

Compute the L2 distance between two Feature Locations. The Locations can represent the same Feature in two time steps or two different Features

Parameters:

first	- First Feature location
second	- Second Feature location

Returns:: - L2 distance between first and second Feature Locations

Definition at line 95 of file OMIPUtils.cpp.

void omip::Location2PointPCL	(	const Feature::Location &	point_location,
		pcl::PointXYZ &	point_pcl
	)

Definition at line 239 of file OMIPUtils.cpp.

void omip::LocationAndId2FeaturePCL	(	const Feature::Location &	feature_location,
		const Feature::Id &	feature_id,
		pcl::PointXYZL &	feature_pcl
	)

Definition at line 246 of file OMIPUtils.cpp.

void omip::LocationAndId2FeaturePCLwc	(	const Feature::Location &	feature_location,
		const Feature::Id &	feature_id,
		omip::FeaturePCLwc &	feature_pcl
	)

Definition at line 254 of file OMIPUtils.cpp.

void omip::LocationOfFeature2ColumnVector	(	const Feature::Location &	lof,
		MatrixWrapper::ColumnVector &	col_vec
	)

Convert the Location of a Feature to a ColumnVector (3x1)

Parameters:

lof	- Location of a Feature

Returns:: - Column vector (3x1)

Definition at line 165 of file OMIPUtils.cpp.

void omip::LocationOfFeature2ColumnVectorHomogeneous	(	const Feature::Location &	lof,
		MatrixWrapper::ColumnVector &	col_vec
	)

Convert the Location of a Feature to a ColumnVector in homogeneous coordinates (4x1)

Parameters:

lof	- Location of a Feature

Returns:: - Column vector homogeneous (4x1)

Definition at line 172 of file OMIPUtils.cpp.

void omip::LocationOfFeature2EigenVectorHomogeneous	(	const Feature::Location &	lof,
		Eigen::Vector4d &	eig_vec
	)

Definition at line 180 of file OMIPUtils.cpp.

Feature::Location omip::operator+	(	const Feature::Location &	location1,
		const Feature::Location &	location2
	)

Definition at line 197 of file OMIPUtils.cpp.

Feature::Location omip::operator-	(	const Feature::Location &	location1,
		const Feature::Location &	location2
	)

Operator - for two Feature Locations

Parameters:

location1	- First Feature Location
location2	- Second Feature Location

Returns:: - Subtraction of the first Feature Location minus the second Feature Location (it should be a vector but for coherence we return a Feature Location)

Definition at line 188 of file OMIPUtils.cpp.

std::ostream & omip::operator<<	(	std::ostream &	os,
		std::vector< Feature::Id >	vector_ids
	)

Operator << for ostream to add a vector of Feature Ids

Parameters:

os	- Input ostream
vector_ids	- Vector of Feature Ids to add to the ostream

Returns:: - ostream with the added vector

Definition at line 64 of file OMIPUtils.cpp.

std::ostream & omip::operator<<	(	std::ostream &	os,
		Feature::Location	location
	)

Operator << for ostream to add the Location of a Feature

Parameters:

os	- Input ostream
location	- Location of a Feature to add to the ostream

Returns:: - ostream with the added Location

Definition at line 74 of file OMIPUtils.cpp.

std::ostream & omip::operator<<	(	std::ostream &	os,
		Eigen::Twistd	twistd
	)

Definition at line 83 of file OMIPUtils.cpp.

void omip::ROSTwist2EigenTwist	(	const geometry_msgs::Twist &	ros_twist,
		Eigen::Twistd &	eigen_twist
	)

Definition at line 262 of file OMIPUtils.cpp.

double omip::sampleNormal	(	double	mean,
		double	std_dev
	)

Retrieve one sample of a normal (Gaussian) distribution, given its mean and standard deviation

Parameters:

mean	- Mean value of the Gaussian
std_dev	- Std deviation of the Gaussian

Returns:: - One sample of the Gaussian distribution

Definition at line 51 of file OMIPUtils.cpp.

void omip::TransformLocation	(	const Feature::Location &	origin,
		const Eigen::Matrix4d &	transformation,
		Feature::Location &	new_location
	)

Transform the location of a Feature using a rigid body transformation (matrix)

Parameters:

origin	- Original location of the Feature
transformation	- Rigid body transformation (pose)

Returns:: - New location of the Feature

Definition at line 123 of file OMIPUtils.cpp.

void omip::TransformLocation	(	const Feature::Location &	origin,
		const Eigen::Twistd &	twist,
		Feature::Location &	new_location,
		int	feat_id = `0`
	)

Transform the location of a Feature using a rigid body transformation (twist)

Parameters:

origin	- Original location of the Feature
twist	- Rigid body transformation (pose)

Returns:: - New location of the Feature

Definition at line 135 of file OMIPUtils.cpp.

void omip::TransformLocation	(	const Feature::Location &	origin,
		const geometry_msgs::Twist &	twist,
		Feature::Location &	new_location
	)

Transform the location of a Feature using a rigid body transformation (twist)

Parameters:

origin	- Original location of the Feature
twist	- Rigid body transformation (pose)

Returns:: - New location of the Feature

Definition at line 158 of file OMIPUtils.cpp.

void omip::TransformMatrix2Twist	(	const Eigen::Matrix4d &	transformation_matrix,
		Eigen::Twistd &	twist
	)

DEPRECATED! This function is dangerous because it uses internally the function log to convert the transformation matrix into an element of the Lie algebra and this function is discontinuous! DO NOT USE THIS FUNCTION! Convert an Eigen Matrix (4x4) of a homogeneous transformation to an Eigen Twist

Parameters:

transformation_matrix - Eigen matrix of the transformation

Returns:: - Twist of the transformation

Definition at line 103 of file OMIPUtils.cpp.

void omip::TransformMatrix2TwistUnwrapped	(	const Eigen::Matrix4d &	transformation_matrix,
		Eigen::Twistd &	twist,
		const Eigen::Twistd &	twist_previous
	)

Convert an Eigen Matrix (4x4) of a homogeneous transformation to an Eigen Twist

Parameters:

transformation_matrix - Eigen matrix of the transformation

Returns:: - Twist of the transformation

Definition at line 111 of file OMIPUtils.cpp.

void omip::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
	)

Convert translation and rotation (Euler angles) into an Eigen::Affine3d matrix

Parameters:

x	- Translation in x (input)
y	- Translation in x (input)
z	- Translation in x (input)
roll	- Rotation roll (input)
pitch	- Rotation pitch (input)
yaw	- Rotation yaw (input)
t	- Eigen Affine3d (output)

Definition at line 27 of file OMIPUtils.cpp.

void omip::Twist2TransformMatrix	(	const Eigen::Twistd &	transformation_twist,
		Eigen::Matrix4d &	matrix
	)

Convert an Eigen Twist to an Eigen Matrix (4x4) homogeneous transformation

Parameters:

transformation_twist - Eigen twist of the transformation

Returns:: - Eigen Matrix (4x4) homogeneous transformation

Definition at line 117 of file OMIPUtils.cpp.

Eigen::Twistd omip::unwrapTwist	(	Eigen::Twistd &	current_twist,
		Eigen::Displacementd &	current_displacement,
		Eigen::Twistd &	previous_twist,
		bool &	changed
	)

Definition at line 291 of file OMIPUtils.cpp.

Classes

Typedefs

Enumerations

Functions

Typedef Documentation

Enumeration Type Documentation

Function Documentation