gaussian.hpp

Go to the documentation of this file.

#ifndef GAUSSIAN_HPP_
#define GAUSSIAN_HPP_

#include <string.h>
#include "jmath/jblas.hpp"
#include "jmath/indirectArray.hpp"
#include "rtslam/rtslamException.hpp"

#define ENABLE_REINIT 0

namespace jafar {
  namespace rtslam {

#if ENABLE_REINIT
    /*
     * This functions use really dirty memcpy, but it is the only way to truly reinit a vec_indirect (change its reference),
     * which is necessary if we don't know the size of the state of an object before its construction
     * (which is the case e.g. for a robot when sensors require additional state parameters to be estimated).
     * It works ok because closure_type is a vector_reference that only contain references and no pointer.
     */

    template<class V, class V2>
    void vec_indirect_init(ublas::vector_indirect<V> & vi, V2 & v, jblas::ind_array const & ia)
    {
      typename V::closure_type d(v);
      memcpy(&vi.data(), &d, sizeof(typename V::closure_type));
      vi.indirect() = ia;
    }

    template<class M, class M2>
    void mat_indirect_init(ublas::matrix_indirect<M> & mi, M2 & m, jblas::ind_array const & ia1, jblas::ind_array const & ia2)
    {
      typename M::closure_type d(m);
      memcpy(&mi.data(), &d, sizeof(typename M::closure_type));
      mi.indirect1() = ia1;
      mi.indirect2() = ia2;
    }
#endif

    //    CLASS Gaussian

    class Gaussian {
      public:
        typedef enum {
          REMOTE, 
          LOCAL, 
          UNCHANGED 
        } storage_t;

      private:
        bool hasNullCov_; 
        std::size_t size_; 
        storage_t storage_; 
        jblas::vec x_local; 
        jblas::sym_mat P_local; 
        jblas::ind_array ia_; 
        jblas::vec_indirect x_; 
        jblas::sym_mat_indirect P_; 

      public:

#if ENABLE_REINIT
        void init(Gaussian & g)
        {
          vec_indirect_init(x_, g.x_.data().expression(), g.ia_);
          mat_indirect_init(P_, g.P_.data().expression(), g.ia_, g.ia_);
        }
        void init(jblas::vec & x, jblas::sym_mat & P, jblas::ind_array const & ia)
        {
          vec_indirect_init(x_, x, ia);
          mat_indirect_init(P_, P, ia, ia);
        }
#endif

        Gaussian() :
          hasNullCov_ (false),
          size_       (0),
          storage_    (LOCAL),
          x_local     (0),
          P_local     (0, 0),
          ia_         (0),
          x_          (x_local, ia_.all()),
          P_          (P_local, ia_.all(), ia_.all())
        {}

        inline Gaussian(const size_t _size) :
          hasNullCov_ (false),
          size_       (_size),
          storage_    (LOCAL),
          x_local     (size_),
          P_local     (size_, size_),
          ia_         (size_),
          x_          (x_local, ia_.all()),
          P_          (P_local, ia_.all(), ia_.all())
        {
          clear();
          for (size_t i = 0; i < size_; i++)
            ia_(i) = i;
        }

#if ENABLE_REINIT

        virtual void operator()(const size_t _size)
        {
          hasNullCov_  = false;
          size_        = _size;
          storage_     = LOCAL;
          x_local.resize(size_);
          P_local.resize(size_, size_);
          ia_          = jmath::ublasExtra::ia_set(0, size_);
          init(x_local, P_local, ia_);
          //x_           = jblas::vec_indirect(x_local, ia_.all());
          //P_           = jblas::sym_mat_indirect(P_local, ia_.all(), ia_.all());
          clear();
        }
#endif

        inline Gaussian(const jblas::vec & _x) :
          hasNullCov_ (true),
          size_       (_x.size()),
          storage_    (LOCAL),
          x_local     (_x),
          P_local     (size_, size_),
          ia_         (size_),
          x_          (x_local, ia_.all()),
          P_          (P_local, ia_.all(), ia_.all())
        {
          for (size_t i = 0; i < size_; i++)
            ia_(i) = i;
        }


        inline Gaussian(const jblas::vec& _x, const jblas::sym_mat& _P) :
          hasNullCov_ (false),
          size_       (_x.size()),
          storage_    (LOCAL),
          x_local     (_x),
          P_local     (_P),
          ia_         (size_),
          x_          (x_local, ia_.all()),
          P_          (P_local, ia_.all(), ia_.all())
        {
          JFR_ASSERT(_x.size() == _P.size1(), "gaussian::Gaussian():: x and P sizes do not match.");
          for (size_t i = 0; i < size_; i++)
            ia_(i) = i;
        }


        // Thanks to C. Roussillon for this constructor.
        inline Gaussian(const Gaussian & G, storage_t _storage = UNCHANGED) :
          hasNullCov_ (G.hasNullCov_),
          size_       (G.size_),
          //        # check storage       ?              # is local                                  : # is remote
          storage_(_storage == UNCHANGED  ?  G.storage_                                              : _storage),
          x_local (_storage == LOCAL      ?  G.x_                                                    : G.x_local),
          P_local (_storage == LOCAL      ?  G.P_                                                    : G.P_local),
          ia_     (_storage == LOCAL      ?  jafar::jmath::ublasExtra::ia_set(0, size_)              : G.ia_),
          x_      (storage_ == LOCAL      ?  jblas::vec_indirect     (x_local, ia_.all())            : G.x_),
          P_      (storage_ == LOCAL      ?  jblas::sym_mat_indirect (P_local, ia_.all(), ia_.all()) : G.P_)
        {
        }

#if ENABLE_REINIT

        virtual void operator()(Gaussian & G, storage_t _storage = UNCHANGED)
        {
          hasNullCov_ = G.hasNullCov_;
          size_ = G.size_;
          //       #     check storage       ?              # is local                                  : # is remote
          storage_ = (_storage == UNCHANGED  ?  G.storage_                                              : _storage);
          x_local  = (_storage == LOCAL      ?  G.x_                                                    : G.x_local);
          P_local  = (_storage == LOCAL      ?  G.P_                                                    : G.P_local);
          ia_      = (_storage == LOCAL      ?  jafar::jmath::ublasExtra::ia_set(0, size_)              : G.ia_);
          if         (storage_ == LOCAL)        init(x_local, P_local, ia_.all());                   else init(G);
//          x_       = (storage_ == LOCAL      ?  jblas::vec_indirect     (x_local, ia_.all())            : G.x_);
//          P_       = (storage_ == LOCAL      ?  jblas::sym_mat_indirect (P_local, ia_.all(), ia_.all()) : G.P_);
        }
#endif

        inline Gaussian(Gaussian & G, const jblas::ind_array & _ia) :
          hasNullCov_ (false),
          size_       (_ia.size()),
          storage_    (REMOTE),
          x_local     (0),
          P_local     (0),
          //     # check storage   ?                   # is local                   :             # is remote
          ia_(G.storage_ == LOCAL  ?  _ia                                           : G.ia_.compose(_ia)),
          x_ (G.storage_ == LOCAL  ?  jblas::vec_indirect     (G.x_local, ia_)      : jblas::vec_indirect     (G.x_.data(), ia_, 1)     ),
          P_ (G.storage_ == LOCAL  ?  jblas::sym_mat_indirect (G.P_local, ia_, ia_) : jblas::sym_mat_indirect (G.P_.data(), ia_, ia_, 1))
        {
        }

#if ENABLE_REINIT

        virtual void operator()(Gaussian & G, const jblas::ind_array & _ia)
        {
          hasNullCov_ = false;
          size_       = _ia.size();
          storage_    = REMOTE;
          x_local.resize(0);
          P_local.resize(0);
          //     # check storage   ?                   # is local                   :             # is remote
          ia_ = (G.storage_ == LOCAL  ?  _ia                                           : G.ia_.compose(_ia));
          if    (G.storage_ == LOCAL)    init(G.x_local, G.P_local, ia_);           else init(G.x_.data().expression(), G.P_.data().expression(), ia_);
          //x_  = (G.storage_ == LOCAL  ?  jblas::vec_indirect     (G.x_local, ia_)      : jblas::vec_indirect     (G.x_.data(), ia_, 1)     );
          //P_  = (G.storage_ == LOCAL  ?  jblas::sym_mat_indirect (G.P_local, ia_, ia_) : jblas::sym_mat_indirect (G.P_.data(), ia_, ia_, 1));
        }
#endif


        inline Gaussian(jblas::vec & _x, jblas::sym_mat & _P, const jblas::ind_array& _ia) :
          hasNullCov_ (false),
          size_       (_ia.size()),
          storage_    (REMOTE),
          x_local     (0),
          P_local     (0),
          ia_         (_ia),
          x_          (_x, ia_),
          P_          (_P, ia_, ia_)
        {
          JFR_ASSERT(_x.size() == _P.size1(), "gaussian::Gaussian():: x and P sizes do not match.");
        }

#if ENABLE_REINIT

        virtual void operator()(jblas::vec & _x, jblas::sym_mat & _P, const jblas::ind_array& _ia)
        {
          hasNullCov_ = false;
          size_       = _ia.size();
          storage_    = REMOTE;
          x_local.resize(0);
          P_local.resize(0);
          ia_         = _ia;
          init(_x, _P, ia_);
//          x_          = jblas::vec_indirect    (_x, ia_);
//          P_          = jblas::sym_mat_indirect(_P, ia_, ia_);
          JFR_ASSERT(_x.size() == _P.size1(), "gaussian::Gaussian():: x and P sizes do not match.");
        }
#endif

        void resize(size_t n, bool preserve = true)
        {
          JFR_ASSERT(storage_ == LOCAL, "cannot extend a LOCAL storage gaussian with size_t!");
          size_ = n;
          x_local.resize(size_, preserve);
          P_local.resize(size_, size_, preserve);

          ia_ = jmath::ublasExtra::ia_set(0, size_);
          x_.indirect() = ia_;
          P_.indirect1() = ia_;
          P_.indirect2() = ia_;

        }

        void resize(const jblas::ind_array& _ia)
        {
          JFR_ASSERT(storage_ == REMOTE, "cannot extend a LOCAL storage gaussian with ind_array!");
          ia_ = _ia;
          size_ = ia_.size();
          x_.indirect() = ia_;
          P_.indirect1() = ia_;
          P_.indirect2() = ia_;
        }

        void extend(size_t n)
        {
          resize(size_+n, true);
        }

        void extend(const jblas::ind_array& _ia)
        {
          resize(jmath::ublasExtra::ia_union(ia_, _ia));
        }


        // Getters
        inline bool                      hasNullCov() const    { return hasNullCov_; }
        inline storage_t                 storage() const       { return storage_;    }
        inline size_t                    size() const          { return size_;       }
        inline jblas::ind_array        & ia()                  { return ia_;         }
        inline const jblas::ind_array  & ia() const            { return ia_;         }
        inline jblas::vec_indirect     & x()                   { return x_;          }
        inline const jblas::vec_indirect & x() const           { return x_;          }
        inline jblas::sym_mat_indirect & P()                   { return P_;          }
        inline const jblas::sym_mat_indirect & P() const       { return P_;          }
        inline double                  & x(size_t i)           { return x_(i);       }
        inline double                  & P(size_t i, size_t j) { return P_(i, j);    }

         // Setters
        inline void  storage(const storage_t & _s) {
          storage_ = _s;
          if (_s == REMOTE)
            hasNullCov_ = false;
        }
        inline void  hasNullCov(bool _hasNullCov) {
          hasNullCov_ = _hasNullCov;
        }
        inline void  x(const jblas::vec & _x) {
          JFR_ASSERT(_x.size() == size_, "gaussian.hpp: set_x: size mismatch.");
          x_.assign(_x);
        }
        inline void  P(const jblas::sym_mat & _P) {
          JFR_ASSERT(_P.size1() == size_, "gaussian.hpp: set_P: size mismatch.");
          hasNullCov_ = false;
          P_.assign(_P);
        }

        inline void std(const jblas::vec& _std) {
          JFR_ASSERT(_std.size() == P_.size1(), "gaussian.hpp: set_P: size mismatch.");
          hasNullCov_ = false;
          P_.assign(jblas::zero_mat(size_));
          for (std::size_t i = 0; i < size_; i++) {
            P_(i, i) = _std(i) * _std(i);
          }
        }

        inline void std(double _std) {
          hasNullCov_ = false;
          P_.assign(jblas::zero_mat(size_));
          for (std::size_t i = 0; i < size_; i++) {
            P_(i, i) = _std * _std;
          }
        }

        inline void P(const jblas::mat & M, const jblas::ind_array & ia1, const jblas::ind_array & ia2) {
          hasNullCov_ = false;
          project(P_local, ia1, ia2) = M;
        }

        virtual void clear(void) {
          x_.assign(jblas::zero_vec(size_));
          P_.assign(jblas::zero_mat(size_, size_));
        }


        double probabilityDensity(const jblas::vec& v) const;

        friend std::ostream& operator <<(std::ostream & s, Gaussian const & g_) {

          if (g_.storage() == LOCAL) {
            if (g_.hasNullCov()) {
              s << std::endl << "  .x : " << g_.x_ << std::endl;
            }
            else { // Null covariance
              s << std::endl << "  .x : " << g_.x_ << std::endl;
              s << "  .P : " << g_.P_;
            }
          }
          else { // REMOTE
            s << "  .ia: " << g_.ia_ << std::endl;
            s << "  .x : " << g_.x_ << std::endl;
            s << "  .P : " << g_.P_;
          }
          return s;
        }

    };

  }
}

#endif /* GAUSSIAN_HPP_ */