Gaussian.hpp

Go to the documentation of this file.

#ifndef ODOMETRY_GAUSSIAN_HPP__
#define ODOMETRY_GAUSSIAN_HPP__

#include <odometry/Configuration.hpp>
#include <base/Pose.hpp>

#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/normal_distribution.hpp>

namespace odometry
{

base::Pose2D projectPoseDelta( const Eigen::Quaterniond& orientation, const base::Pose& pose );

class GaussianSamplingPose3D
{
    typedef base::Matrix6d Matrix6d;
    typedef base::Vector6d Vector6d;

public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    GaussianSamplingPose3D( const Configuration& config );
    void update( const Vector6d& mean, const Matrix6d& llt );
    Vector6d sample();

public:
    Matrix6d poseCov, poseLT;
    Vector6d poseMean;

protected:
    Configuration config;

    boost::variate_generator<boost::minstd_rand, boost::normal_distribution<> > rand_norm;
};

}
#endif