Go to the documentation of this file.00001
00002 #ifndef _UNCERTAIN_TRANSFORM_LISTENER_
00003 #define _UNCERTAIN_TRANSFORM_LISTENER_
00004
00005 #include "uncertain_tf/UncertainTransformer.h"
00006 #include "uncertain_tf/utfMessage.h"
00007 #include "uncertain_tf/EigenMultiVariateNormal.hpp"
00008
00009 using namespace tf;
00010 using namespace Eigen;
00011
00012 namespace uncertain_tf {
00013
00014 class UncertainTransformListener : public UncertainTransformer
00015 {
00016
00017 public:
00018
00019 UncertainTransformListener(ros::Duration max_cache_time = ros::Duration(tf::Transformer::DEFAULT_CACHE_TIME), bool spin_thread = true);
00020
00022 void sampleTransform(const std::string& target_frame, const std::string& source_frame,
00023 const ros::Time& time, std::vector<StampedTransform>& output, size_t n);
00024
00026 void sampleTransformGaussianTime(const std::string& target_frame, const std::string& source_frame,
00027 const ros::Time& time_mean, const ros::Duration& time_variance, std::vector<StampedTransform>& output, size_t n);
00028
00030 void sampleTransformUniformTime(const std::string& target_frame, const std::string& source_frame,
00031 const ros::Time& time_start, const ros::Time& time_end, std::vector<StampedTransform>& output, size_t n);
00032
00033 void printFrame(std::string last_frame, std::string current_frame, tf::Transform rel);
00034
00035
00037 MatrixXd sampleFromMeanCov(const VectorXd &mean_, const MatrixXd &cov_, size_t n = 1);
00038
00040 void sampleFromMeanCov(const tf::Transform &mean, const MatrixXd &cov, std::vector<tf::Transform> &output, size_t n = 1);
00041
00043 tf::Transform sampleFromMeanCov(const tf::Transform &mean, const MatrixXd &cov);
00044
00046 VectorXd transformTFToVectorXd(const tf::Transform &transform);
00047
00049 tf::Transform transformVectorXdToTF(const VectorXd &vec);
00050
00052 bool isZero(const MatrixXd mat);
00053
00055 template <typename Derived, typename OtherDerived> void calculateSampleCovariance(const MatrixBase<Derived>& x, const MatrixBase<Derived>& y, MatrixBase<OtherDerived> & C_);
00056
00058 VectorXd calculateSampleMean(const MatrixXd &x);
00059
00061 MatrixXd sampleSetTFtoMatrixXd(std::vector<tf::StampedTransform> sampleset);
00062
00063 private:
00064
00065 void subscription_callback(const uncertain_tf::utfMessageConstPtr& msg);
00066
00067 ros::NodeHandle node_;
00068 ros::Subscriber message_subscriber_utf_;
00069 EigenMultivariateNormal<double, 6> *emn_;
00070 UnivariateNormal<double> *univ_;
00071 };
00072
00073 template <typename Derived, typename OtherDerived>
00074 void UncertainTransformListener::calculateSampleCovariance(const MatrixBase<Derived>& x, const MatrixBase<Derived>& y, MatrixBase<OtherDerived> & C_)
00075 {
00076 typedef typename Derived::Scalar Scalar;
00077 typedef typename internal::plain_row_type<Derived>::type RowVectorType;
00078
00079 const Scalar num_observations = static_cast<Scalar>(x.rows());
00080
00081 const RowVectorType x_mean = x.colwise().sum() / num_observations;
00082 const RowVectorType y_mean = y.colwise().sum() / num_observations;
00083
00084 MatrixBase<OtherDerived>& C = const_cast< MatrixBase<OtherDerived>& >(C_);
00085
00086 C.derived().resize(x.cols(),x.cols());
00087 C = (x.rowwise() - x_mean).transpose() * (y.rowwise() - y_mean) / num_observations;
00088 }
00089
00090
00091 }
00092
00093 #endif