t3d.hpp

/* $Id$ */

#ifndef GEOM_T3D_HPP
#define GEOM_T3D_HPP

#include <iostream>

#include "kernel/jafarMacro.hpp"
#include "kernel/keyValueFile.hpp"
#include "kernel/dataLog.hpp"

#include "jmath/jblas.hpp"

namespace jafar {
  namespace geom {

    class T3D : public jafar::kernel::KeyValueFileSaveLoad, public jafar::kernel::DataLoggable {

    public:

      enum Type {
  EULER,            
  QUATERNION,       
  ROTATION_VECTOR,  
  IDENTITY          
      };

      enum Standard {
        CALIFE_TO_JAFAR,
        JAFAR_TO_CALIFE  
      };

      virtual ~T3D();

      T3D& operator=(const T3D& t);

      void assign(T3D const& t);

      virtual Type type() const = 0;

      std::size_t size() const {return x.size();}

      jblas::vec const& getX() const {return x;}
      jblas::vec& getX() {p_isMuptodate = false; return x;}

      jblas::sym_mat const& getXCov() const {
  JFR_PRECOND(hasCov(), "T3D::getXCov: t3d covariance is undefined"); 
  return xCov;
      }
      
      jblas::sym_mat& getXCov() {
  JFR_PRECOND(hasCov(), "T3D::getXCov: t3d covariance is undefined"); 
  p_isMuptodate = false;
  return xCov;
      }

      jblas::vec getXStdDev();
      
      const jblas::mat44& getM() const {
  if (!isMuptodate())
    updateM();
  return M;
      }

      jblas::mat33 getR() const {
  if (!isMuptodate())
    updateM();
  return ublas::project(M, ublas::range(0,3), ublas::range(0,3) );
      }

      jblas::vec3 getT() const {
  if (!isMuptodate())
    updateM();
  return ublas::project(ublas::column(M,3), ublas::range(0,3) );
      }

      bool hasCov() const {return p_hasCov;}
      bool is2D() const {return p_is2D;}
      bool isMuptodate() const {return p_isMuptodate;}

      bool isIdentity() const {return type() == IDENTITY;}

      void clear();
      
      void set(const jblas::vec& x_);

      void set(const jblas::mat& M_);

      void set(const jblas::mat& R_, const jblas::vec& t_);

      void set(const jblas::vec& x_, const jblas::vec& xStdDev_);

      void set(const jblas::vec& x_, const jblas::sym_mat& xCov_);

      void set2D(bool is2D=true) {p_is2D = is2D;}

      void rearrange(const Standard& standard = JAFAR_TO_CALIFE);

      static void inv(T3D const& t, T3D& t_inv);

      static void inv(T3D const& t, T3D& t_inv, jblas::mat& J);

      static void compose(T3D const& t1, T3D const& t2, T3D& t3);

      static void composeIncr(T3D& t1, T3D const& t2);

      static void compose(T3D const& t1, T3D const& t2, T3D& t3, jblas::mat& J1, jblas::mat& J2 );
      
      static void convert(T3D const& t1, T3D& t2);

      template<class T3D_res>
      static T3D_res compose(T3D const& t1, T3D const& t2) {
  T3D_res t3;
  compose(t1,t2,t3);
  return t3;
      }

      template<class T3D_res>
      static T3D_res inv(T3D const& t) {
  T3D_res t_inv;
  inv(t,t_inv);
  return t_inv;
      }

      static T3D* create(Type type);

      static T3D* clone(T3D const& t);
    private:
      static void invImpl(T3D const& t, T3D& t_inv);
    protected:

      jblas::vec x;
      jblas::sym_mat xCov;
      mutable jblas::mat44 M; // M is an internal variable, so it is declared "mutable"

      bool p_hasCov;
      bool p_is2D;
      mutable bool p_isMuptodate; // _isMuptodate is an internal variable, so it is declared "mutable"

      T3D(std::size_t size, bool hasCov_);
      T3D(std::size_t size, const jblas::vec& x_, bool hasCov_);
      T3D(std::size_t size, const jblas::vec& x_, const jblas::vec& xStdDev_);
      T3D(std::size_t size, const jblas::vec& x_, const jblas::sym_mat& xCov_);
      T3D(const jblas::mat& M_);
      T3D(const T3D& t_);

      virtual void updateM() const = 0;

      virtual void updateX() = 0;

      void loadKeyValueFile(jafar::kernel::KeyValueFile const& keyValueFile);
      void saveKeyValueFile(jafar::kernel::KeyValueFile& keyValueFile);

      void writeLogHeader(jafar::kernel::DataLogger& log) const;
      void writeLogData(jafar::kernel::DataLogger& log) const;

      friend std::ostream& operator<<(std::ostream& s, const T3D& t3d); 

    }; // class T3D


    std::ostream& operator<<(std::ostream& s, const T3D::Type& type);
    std::istream& operator>>(std::istream& s, T3D::Type& type);

    std::ostream& operator<<(std::ostream& s, const T3D& t3d);


  } // namespace geom
} // namespace jafar

#endif // GEOM_T3D_HPP