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 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 | 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 ¤t_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 ¤t_twist, Eigen::Displacementd ¤t_displacement, Eigen::Twistd &previous_twist, bool &changed) |
Definition at line 107 of file OMIPTypeDefs.h.
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 pcl::PointCloud<pcl::PointXYZRGB> omip::PointCloudPCL |
Definition at line 106 of file OMIPTypeDefs.h.
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.
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.
Enumerator | |
---|---|
STATIC_ENVIRONMENT_EKF_TRACKER | |
STATIC_ENVIRONMENT_ICP_TRACKER |
Definition at line 167 of file OMIPTypeDefs.h.
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)
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 | ||
) |
void omip::GeometryMsgsTwist2EigenTwist | ( | const geometry_msgs::Twist & | gm_twist, |
Eigen::Twistd & | eigen_twist | ||
) |
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
transformation | - Eigen matrix to test |
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
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)
lof | - Location of a Feature |
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)
lof | - Location of a Feature |
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 | ||
) |
std::ostream & omip::operator<< | ( | std::ostream & | os, |
std::vector< Feature::Id > | vector_ids | ||
) |
Operator << for ostream to add a vector of Feature Ids
os | - Input ostream |
vector_ids | - Vector of Feature Ids to add to the ostream |
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
os | - Input ostream |
location | - Location of a Feature to add to the ostream |
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
mean | - Mean value of the Gaussian |
std_dev | - Std deviation of the Gaussian |
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)
origin | - Original location of the Feature |
transformation | - Rigid body transformation (pose) |
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)
origin | - Original location of the Feature |
twist | - Rigid body transformation (pose) |
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)
origin | - Original location of the Feature |
twist | - Rigid body transformation (pose) |
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
transformation_matrix | - Eigen matrix 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 | ||
) |
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
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 | ||
) |
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.