UncertainTransformListener.h

Go to the documentation of this file.

#ifndef _UNCERTAIN_TRANSFORM_LISTENER_
#define _UNCERTAIN_TRANSFORM_LISTENER_

#include "uncertain_tf/UncertainTransformer.h"
#include "uncertain_tf/utfMessage.h"
#include "uncertain_tf/EigenMultiVariateNormal.hpp"

using namespace tf;
using namespace Eigen;

namespace uncertain_tf {


//TODO: Most of this functionality should move to UncertainTransformer!

class UncertainTransformListener : public UncertainTransformer
{

public:

    UncertainTransformListener(ros::Duration max_cache_time = ros::Duration(tf::Transformer::DEFAULT_CACHE_TIME), bool spin_thread = true);

    void sampleTransform(const std::string& target_frame, const std::string& source_frame,
                         const ros::Time& time, std::vector<StampedTransform>& output, size_t n);

    void sampleTransform(const std::string& target_frame, const ros::Time& target_time,
                          const std::string& source_frame, const ros::Time& time,
                          const std::string& fixed_frame,
                          std::vector<StampedTransform>& output, size_t n);

    void sampleTransformGaussianTime(const std::string& target_frame, const std::string& source_frame,
                         const ros::Time& time_mean, const ros::Duration& time_variance, std::vector<StampedTransform>& output, size_t n);

    void sampleTransformUniformTime(const std::string& target_frame, const std::string& source_frame,
                         const ros::Time& time_start, const ros::Time& time_end, std::vector<StampedTransform>& output, size_t n);

    void printFrame(std::string last_frame, std::string current_frame, tf::Transform rel);


    MatrixXd sampleFromMeanCov(const VectorXd &mean_, const MatrixXd &cov_, size_t n = 1);

    void sampleFromMeanCov(const tf::Transform &mean, const MatrixXd &cov, std::vector<tf::Transform> &output, size_t n = 1);

    tf::Transform sampleFromMeanCov(const tf::Transform &mean, const MatrixXd &cov);

    VectorXd transformTFToVectorXd(const tf::Transform &transform);

    tf::Transform transformVectorXdToTF(const VectorXd &vec);

    bool isZero(const MatrixXd mat);

    template <typename Derived, typename OtherDerived> void calculateSampleCovariance(const MatrixBase<Derived>& x, const MatrixBase<Derived>& y, MatrixBase<OtherDerived> & C_);

    VectorXd calculateSampleMean(const MatrixXd &x);

    MatrixXd sampleSetTFtoMatrixXd(std::vector<tf::StampedTransform> sampleset);

private:

    void subscription_callback(const uncertain_tf::utfMessageConstPtr& msg);

    ros::NodeHandle node_;
    ros::Subscriber message_subscriber_utf_;
    EigenMultivariateNormal<double, 6> *emn_;
    UnivariateNormal<double> *univ_;
};

template <typename Derived, typename OtherDerived>
void UncertainTransformListener::calculateSampleCovariance(const MatrixBase<Derived>& x, const MatrixBase<Derived>& y, MatrixBase<OtherDerived> & C_)
{
    typedef typename Derived::Scalar Scalar;
    typedef typename internal::plain_row_type<Derived>::type RowVectorType;

    const Scalar num_observations = static_cast<Scalar>(x.rows());

    const RowVectorType x_mean = x.colwise().sum() / num_observations;
    const RowVectorType y_mean = y.colwise().sum() / num_observations;

    MatrixBase<OtherDerived>& C = const_cast< MatrixBase<OtherDerived>& >(C_);

    C.derived().resize(x.cols(),x.cols()); // resize the derived object
    C = (x.rowwise() - x_mean).transpose() * (y.rowwise() - y_mean) / num_observations;
}


}

#endif