OMIPUtils.h

Go to the documentation of this file.

/*
 * OMIPUtils.h
 *
 *      Author: roberto
 *
 * This is a modified implementation of the method for online estimation of kinematic structures described in our paper
 * "Online Interactive Perception of Articulated Objects with Multi-Level Recursive Estimation Based on Task-Specific Priors"
 * (Martín-Martín and Brock, 2014).
 * This implementation can be used to reproduce the results of the paper and to be applied to new research.
 * The implementation allows also to be extended to perceive different information/models or to use additional sources of information.
 * A detail explanation of the method and the system can be found in our paper.
 *
 * If you are using this implementation in your research, please consider citing our work:
 *
@inproceedings{martinmartin_ip_iros_2014,
Title = {Online Interactive Perception of Articulated Objects with Multi-Level Recursive Estimation Based on Task-Specific Priors},
Author = {Roberto {Mart\'in-Mart\'in} and Oliver Brock},
Booktitle = {Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems},
Pages = {2494-2501},
Year = {2014},
Location = {Chicago, Illinois, USA},
Note = {http://www.robotics.tu-berlin.de/fileadmin/fg170/Publikationen_pdf/martinmartin_ip_iros_2014.pdf},
Url = {http://www.robotics.tu-berlin.de/fileadmin/fg170/Publikationen_pdf/martinmartin_ip_iros_2014.pdf},
Projectname = {Interactive Perception}
}
 * If you have questions or suggestions, contact us:
 * roberto.martinmartin@tu-berlin.de
 *
 * Enjoy!
 */

#ifndef OMIP_UTILS_H_
#define OMIP_UTILS_H_

#include <pcl/point_types.h>
#include <Eigen/Geometry>
#include <lgsm/Lgsm>
#include <boost/shared_ptr.hpp>

#include "omip_common/Feature.h"

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

#include "wrappers/matrix/matrix_wrapper.h"

#include "omip_common/OMIPTypeDefs.h"

namespace omip
{

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

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

double sampleNormal(double mean, double std_dev);

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

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

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 Twist2TransformMatrix( const Eigen::Twistd& transformation_twist, Eigen::Matrix4d& matrix);

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 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);
void GeometryMsgsTwist2EigenTwist(const geometry_msgs::Twist& gm_twist, Eigen::Twistd& eigen_twist);

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

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

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 ROSTwist2EigenTwist(const geometry_msgs::Twist& ros_twist, Eigen::Twistd &eigen_twist);

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

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

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

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

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

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

void computeAdjoint(const Eigen::Displacementd pose_disp, Eigen::Matrix<double, 6, 6> &adjoint_out);
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);

}

#endif /* OMIP_UTILS_H_ */