R3Arma.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$
//
// Copyright 2008, 2009, 2010, 2011, 2012  Antonio Franchi and Paolo Stegagno    
//
// This file is part of MIP. 
//
// MIP is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// MIP is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with MIP. If not, see <http://www.gnu.org/licenses/>.
//
// Contact info: antonio.franchi@tuebingen.mpg.de stegagno@diag.uniroma1.it
//
// ----------------------------------------------------------------------------


#ifndef __R3_ARMA_H_ 
#define __R3_ARMA_H_

#include <armadillo>
#include <S1.h>
#include <S3.h>
#include <SE3.h>
#include <Time.h>
#include <R2Arma.h>
// using namespace arma;
using namespace std;

namespace MipBaselib{
typedef enum {
 PIXEL_CAMERA_ARMA_MATRIX,
 METRIC_CAMERA_ARMA_MATRIX,
 NUM_CAMERA_ARMA_MATRIX
} CanonicalCameraArmaMatrix;

static const char* CanonicalCameraArmaMatrixName[NUM_CAMERA_ARMA_MATRIX] ={
 "PIXEL_ARMA_MATRIX",
 "METRIC_ARMA_MATRIX"
};

class Roto3DArma : public arma::Mat<Decimal>::fixed<3,3>{
 public:
  Roto3DArma(CanonicalArmaMatrix init=ZERO_ARMA_MATRIX);
  
  Roto3DArma(const Decimal &a11,const Decimal &a12, const Decimal &a13, const Decimal &a21, const Decimal &a22, const Decimal &a23, const Decimal &a31, const Decimal &a32, const Decimal &a33);
  
  Roto3DArma(const Decimal &roll, const Decimal &pitch, const Decimal &yaw);
  
  Roto3DArma(const Angle &ro, const Angle &pit, const Angle &ya);
  
  Roto3DArma(const Roto3DArma &A);
  
  Roto3DArma(const vector<Decimal> &vec);
  
  Roto3DArma(const arma::Mat<Decimal>::fixed<3,3>& rhs);
  
  Roto3DArma(const Orientation3D &ori);
  
  Roto3DArma& operator=(const Roto3DArma& rhs);
  
  Roto3DArma& operator=(const arma::Mat<Decimal>::fixed<3,3>& rhs);
  
  Roto3DArma& operator+=(const Roto3DArma &pos);
  
  /*const */Roto3DArma operator+(Roto3DArma &other) const;
  
  /*const */Roto3DArma operator-(Roto3DArma &other) const;
  
  Roto3DArma& operator-=(const Roto3DArma &pos); 
  
  Roto3DArma& operator*=(const Roto3DArma &pos); 
  
  Roto3DArma& operator*=(const Decimal& scalar);
  
  /*const */Roto3DArma operator*(Roto3DArma &other) const;
  
  Roto3DArma& operator/=(const Roto3DArma &pos); 
  
  Roto3DArma& operator/=(const Decimal& scalar);
    
  /*const */Roto3DArma operator/(Roto3DArma &other) const;
  
  string print();
  
  arma::Row<Decimal>::fixed<3> getFirstRow() const;
  
  arma::Row<Decimal>::fixed<3> getSecondRow() const;
  
  arma::Row<Decimal>::fixed<3> getThirdRow() const;
  
  arma::Col<Decimal>::fixed<3> getFirstCol() const;
  
  arma::Col<Decimal>::fixed<3> getSecondCol() const;
  
  arma::Col<Decimal>::fixed<3> getThirdCol() const;
  
  
  Roto3DArma invMatrix();
  
  arma::Mat<Decimal> subMat(int r, int c, int R, int C);
  
  Decimal getElem(int r,int c) const;
  
  void setElem(int r,int c,const Decimal &item);
  
  arma::Mat<Decimal>::fixed<3,3>* getMat();
  
  Decimal getRoll() const;
  
  Angle getRollAngle() const;
  
  Decimal getPitch() const;
  
  Angle getPitchAngle() const;
  
  Decimal getYaw() const;
  
  Angle getYawAngle() const;
  
  void setYaw(const Angle &ya);
  
  void setYaw(const Decimal &ya);
  
  void setRoll(const Angle &ro);
  
  void setRoll(const Decimal &ro);
  
  void setPitch(const Angle &pit);
  
  void setPitch(const Decimal &pit);
};

class Position3DArma: public arma::Col<Decimal>::fixed<3>{
 public:
  Position3DArma();
  
   Position3DArma(const Decimal &x,const Decimal &y,const Decimal &z);
  
   Position3DArma(const Position2DArma &xy,const Decimal &z);
  
  Position3DArma(const Position3D& A);
  
  Position3DArma(const Position3DArma& A);
  
  Position3DArma(const arma::Mat<Decimal>& A);
  
  Position3DArma& operator=(const Position3D& rhs);
  
  Position3DArma& operator=(const Position3DArma& rhs);
  
  Position3DArma& operator=(const arma::Mat<Decimal>& rhs);
  
  Position3DArma& operator+=(const Position3DArma &pos);
  
  Position3DArma& operator+=(const arma::Col<Decimal>::fixed<3> &pos); 
  
  Position3DArma& operator+=(const Decimal& scalar);
    
  /*const */Position3DArma operator-(Position3DArma &other) const;
  
  Position3DArma& operator-=(const Position3DArma &pos); 
  
  Position3DArma& operator-=(const Decimal& scalar);
  
  Position3DArma& operator-=(const arma::Col<Decimal>::fixed<3> &pos); 
    
  /*const */Position3DArma operator+(Position3DArma &other) const;
  
  Position3DArma& operator*=(const Position3DArma &pos); 
  
  Position3DArma& operator*=(const Decimal& scalar);
  
  Position3DArma& operator*=(const arma::Mat<Decimal>::fixed<3,3> &mat);
  
  Position3DArma& operator*=(const arma::Col<Decimal>::fixed<3> &vec);
  
  Position3DArma& operator*=(const Roto3DArma &mat);
  
  Position3DArma& operator*=(const arma::Mat<Decimal>::fixed<4,4> &mat);
  
  /*const */Position3DArma operator*(Position3DArma &other) const;
  
  Position3DArma operator*(const arma::Col<Decimal>::fixed<3> &vec);
  
  Position3DArma& operator/=(const Position3DArma &pos); 
  
  Position3DArma& operator/=(const Decimal& scalar);
  
  Position3DArma& operator/=(const arma::Col<Decimal>::fixed<3> &vec);
  
  /*const */Position3DArma operator/(const arma::Col<Decimal>::fixed<3> &vec) const;
  
  /*const */Position3DArma operator/(Position3DArma &other) const;
  
  bool operator==(const Position3DArma &other) const;
  
  Position3DArma matrixProd(const arma::Mat<Decimal>::fixed<3,3> &mat);
  
  Position3DArma matrixHomoProd(const arma::Mat<Decimal>::fixed<4,4> &mat);
  
  Position3DArma matrixProd(const Roto3DArma &mat);
  
  Decimal x() const;
  
  Decimal y() const;
  
  Decimal z() const;
  
  void setX(const Decimal &p);
  
  void setY(const Decimal &p);
  
  void setZ(const Decimal &p);
  
  Angle azimuth() const;
  
  Angle zenith() const;
  
  string print();
  
  Decimal norm(int p=2) const;
  
  Decimal dist( Position3DArma& other, int p=2) const;
  
  Position2DArma getPosition2D();
};

class Roto3DHomoArma : public arma::Mat<Decimal>::fixed<4,4>{
 public:
  Roto3DHomoArma(CanonicalArmaMatrix init=ZERO_ARMA_MATRIX);
  
  Roto3DHomoArma(const Decimal &a11,const Decimal &a12, const Decimal &a13, const Decimal &a21, const Decimal &a22, const Decimal &a23, const Decimal &a31, const Decimal &a32, const Decimal &a33);
  
  Roto3DHomoArma(const Decimal &a11,const Decimal &a12, const Decimal &a13, const Decimal &a21, const Decimal &a22, const Decimal &a23, const Decimal &a31, const Decimal &a32, const Decimal &a33,const Decimal &x,const Decimal &y,const Decimal &z);
  
  Roto3DHomoArma(const Decimal &a11,const Decimal &a12, const Decimal &a13, const Decimal &a21, const Decimal &a22, const Decimal &a23, const Decimal &a31, const Decimal &a32, const Decimal &a33, const arma::Col<Decimal>::fixed<3> &trasl);
  
  Roto3DHomoArma(const Decimal &roll, const Decimal &pitch, const Decimal &yaw);
  
  Roto3DHomoArma(const Decimal &roll, const Decimal &pitch, const Decimal &yaw, const Decimal &x, const Decimal &y, const Decimal &z);
  
  Roto3DHomoArma(const Roto3DHomoArma &A);
  
  Roto3DHomoArma(const arma::Mat<Decimal>::fixed<4,4>& rhs);
  
  Roto3DHomoArma(const Roto3DArma& other);
  
  Roto3DHomoArma(const Roto3DArma& other, const Decimal &x, const Decimal &y, const Decimal &z);
  
  Roto3DHomoArma(const Roto3DArma& other, const arma::Col<Decimal>::fixed<3> &trasl);
  
  Roto3DHomoArma& operator=(const Roto3DHomoArma& rhs);
  
  Roto3DHomoArma& operator=(const arma::Mat<Decimal>::fixed<4,4>& rhs);
  
  Roto3DHomoArma& operator+=(const Roto3DHomoArma &pos);
  
  /*const */Roto3DHomoArma operator+(Roto3DHomoArma &other) const;
  
  /*const */Roto3DHomoArma operator-(Roto3DHomoArma &other) const;
  
  Roto3DHomoArma& operator-=(const Roto3DHomoArma &pos); 
  
  Roto3DHomoArma& operator*=(const Roto3DHomoArma &pos); 
  
  Roto3DHomoArma& operator*=(const Decimal& scalar);
    
  /*const */Roto3DHomoArma operator*(Roto3DHomoArma &other) const;
  
  Roto3DHomoArma& operator/=(const Roto3DHomoArma &pos); 
  
  Roto3DHomoArma& operator/=(const Decimal& scalar);
    
  /*const */Roto3DHomoArma operator/(Roto3DHomoArma &other) const;
  
  string print();
  
  Roto3DHomoArma invMatrix();
  
  Decimal getElem(int r,int c) const;
  
  void setElem(int r,int c,const Decimal &item);
  
  void setRotation(const Roto3DArma &rhs);
  
  void setPosition(const Position3DArma &rhs);
  
  arma::Col<Decimal>::fixed<3> getTraslation();
  
  Position3DArma getTraslation3DArma() const;
  
  arma::Mat<Decimal>::fixed<3,3> getRotation();
  
  Roto3DArma getRotation3DArma() const;
  
  Decimal getRoll() const;
  
  Angle getRollAngle() const;
  
  Decimal getPitch() const;
  
  Angle getPitchAngle() const;
  
  Decimal getYaw() const;
  
  Angle getYawAngle() const;
};

class Velocity3DArma : public Position3DArma{
 public:
  
  Velocity3DArma() : Position3DArma() {}
  
  Velocity3DArma(const Decimal &x_in,const Decimal &y_in,const Decimal &z_in) : Position3DArma(x_in,y_in,z_in) {}
  
  Velocity3DArma(const Velocity3DArma& p);
  
  Velocity3DArma(Position3DArma p) : Position3DArma(p) {}
  
  Velocity3DArma(const Position3D &pos) : Position3DArma(pos){};
  
  Velocity3DArma(const arma::Mat<Decimal>& A) : Position3DArma(A){};
  
    Velocity3DArma& operator=(const Velocity3DArma& rhs);
  
    Velocity3DArma& operator=(const arma::Mat<Decimal>& rhs);
  
  bool operator==(const Velocity3DArma &other) const;
  
  Velocity2DArma getVelocity2D();
  
  string print(PrintTypes type = PRINT_TYPE_WITH_UNITS) const;
}; 

class Acceleration3DArma : public Position3DArma{
 public:
  
  Acceleration3DArma() : Position3DArma() {}
  
  Acceleration3DArma(const Decimal &x_in,const Decimal &y_in,const Decimal &z_in) : Position3DArma(x_in,y_in,z_in) {}
  
  Acceleration3DArma(const Acceleration3DArma& p);
  
  Acceleration3DArma(Position3DArma p) : Position3DArma(p) {}
  
  Acceleration3DArma(const Position3D &pos) : Position3DArma(pos){};
  
  Acceleration3DArma(const arma::Mat<Decimal>& A) : Position3DArma(A){};
  
    Acceleration3DArma& operator=(const Acceleration3DArma& rhs);
  
    Acceleration3DArma& operator=(const arma::Mat<Decimal>& rhs);
  
  bool operator==(const Acceleration3DArma &other) const;
  
  Acceleration2DArma getAcceleration2D();
  
  string print(PrintTypes type = PRINT_TYPE_WITH_UNITS) const;
}; 

class Pose3DArma{
 private:
  Orientation3D  _ori;
  Position3DArma _pos;
  
 public:
  
  Pose3DArma();
  
  Pose3DArma(const Decimal &x, const Decimal &y, const Decimal &z, const Angle &roll, const Angle &pitch, const Angle &yaw);
  
  Pose3DArma(const Position3DArma &pos, const Orientation3D &ori);
  
  Pose3DArma(const Position3DArma &pos, const Angle &roll, const Angle &pitch, const Angle &yaw);
  
  Pose3DArma(const Decimal &x, const Decimal &y, const Decimal &z, const Orientation3D &ori);
  
  Pose3DArma(Pose3D &pos);
  
  Pose3DArma(const Pose3DArma &pos);
  
  Pose3DArma& operator=(Pose3D& rhs);
  
  Pose3DArma& operator=(const Pose3DArma& rhs);
  
  Pose3DArma& operator+=(const Pose3DArma& a);
  
  const Pose3DArma operator+(const Pose3DArma &other) const;
  
  Pose3DArma& operator-=(const Pose3DArma& a);
  
  const Pose3DArma operator-(const Pose3DArma &other) const;
  
  bool operator==(const Pose3DArma &other) const;
  
  bool operator!=(const Pose3DArma &other) const;
  
  Pose3DArma& operator*=(const arma::Mat<Decimal>::fixed<3,3>& rhs);
  
  /*const*/ Pose3DArma operator*(const arma::Mat<Decimal>::fixed<3,3>& rhs) const;
  
  Pose3DArma& operator*=(const arma::Mat<Decimal>::fixed<4,4>& rhs);
  
  /*const*/ Pose3DArma operator*(const arma::Mat<Decimal>::fixed<4,4>& rhs) const;
  
  Position3DArma getPos();
  
  Decimal getX() const;
  
  Decimal getY() const;
  
  Decimal getZ() const;
  
  Angle getRollAngle() const;
  
  Angle getPitchAngle() const;
  
  Angle getYawAngle() const;
  
  Decimal getRoll() const;
  
  Decimal getPitch() const;
  
  Decimal getYaw() const;
  
  Decimal getAzimuth() const;
  
  Angle getAzimuthAngle() const;
  
  Decimal getZenith() const;
  
  Angle getZenithAngle() const;
  
  Decimal norm(int p=2) const;
  
  Orientation3D getOri();
  
  void setPos(const Position3DArma &pos);
  
  void setX(const Decimal &x);
  
  void setY(const Decimal &y);
  
  void setZ(const Decimal &z);
  
  void setOri(const Orientation3D& ori);
  
  void setRoll(const Angle& roll);
  
  void setRoll(const Decimal& roll);
  
  void setPitch(const Angle& pitch);
  
  void setPitch(const Decimal& pitch);
  
  void setYaw(const Angle& yaw);
  
  void setYaw(const Decimal& yaw);
  
  Pose2DArma getPose2D();
  
  string print();
};

class IMU3DArma : public arma::Col<Decimal>::fixed<6>{
 public:
  
  IMU3DArma();
  
  IMU3DArma(const Decimal &accX, const Decimal &accY, const Decimal &accZ, const Decimal &velAngX, const Decimal &velAngY, const Decimal &velAngZ);
  
  IMU3DArma(const Decimal &accX, const Decimal &accY, const Decimal &accZ, const Velocity3DArma &angVel);
  
  IMU3DArma(const Acceleration3DArma &acc, const Decimal &velAngX, const Decimal &velAngY, const Decimal &velAngZ);
  
  IMU3DArma(const Acceleration3DArma &acc, const Velocity3DArma &angVel);
  
  IMU3DArma& operator=(const IMU3DArma& rhs);
  
  IMU3DArma& operator=(const arma::Mat<Decimal>& rhs);
  
  IMU3DArma& operator+=(const IMU3DArma &pos); 
  
  IMU3DArma& operator+=(const Decimal& scalar);
    
  /*const */IMU3DArma operator-(IMU3DArma &other) const;
  
  IMU3DArma& operator-=(const IMU3DArma &pos); 
  
  IMU3DArma& operator-=(const Decimal& scalar);
    
  /*const */IMU3DArma operator+(IMU3DArma &other) const;
  
  IMU3DArma& operator*=(const IMU3DArma &pos); 
  
  IMU3DArma& operator*=(const Decimal& scalar);
    
  /*const */IMU3DArma operator*(IMU3DArma &other) const;
  
  IMU3DArma& operator/=(const IMU3DArma &pos); 
  
  IMU3DArma& operator/=(const Decimal& scalar);
    
  /*const */IMU3DArma operator/(IMU3DArma &other) const;
  
  bool operator==(const IMU3DArma &other) const;
  
  Decimal getAccX() const;
  
  Decimal getAccY() const;
  
  Decimal getAccZ() const;
  
  Acceleration3DArma getAcceleration() const;
  
  Decimal getAngVelX() const;
  
  Decimal getAngVelY() const;
  
  Decimal getAngVelZ() const;
  
  Velocity3DArma getAngularVelocity() const;
  
  void setAccX(const Decimal &x);
  
  void setAccY(const Decimal &y);
  
  void setAccZ(const Decimal &z);
  
  void setAcceleration(const Acceleration3DArma &acc);
  
  void setAngVelX(const Decimal &x);
  
  void setAngVelY(const Decimal &y);
  
  void setAngVelZ(const Decimal &z);
  
  void setAngularVelocity(const Velocity3DArma &angVel);
  
  IMU3DArma& matrixProd(const arma::Mat<Decimal>::fixed<3,3> &in);
  
  IMU3DArma& matrixProd(const Roto3DArma &in);
  
  IMU3DArma& matrixProdAcc(const arma::Mat<Decimal>::fixed<3,3> &in);
  
  IMU3DArma& matrixProdAngVel(const arma::Mat<Decimal>::fixed<3,3> &in);
  
  IMU3DArma& matrixProdAcc(const Roto3DArma &in);
  
  IMU3DArma& matrixProdAngVel(const Roto3DArma &in);
  
  string print();
};

class Odometry3DArma : public arma::Col<Decimal>::fixed<6>{
 public:
  
  Odometry3DArma();
  
  Odometry3DArma(const Decimal &linVelX, const Decimal &linVelY, const Decimal &linVelZ, const Decimal &velAngX, const Decimal &velAngY, const Decimal &velAngZ);
  
  Odometry3DArma(const Decimal &linVelX, const Decimal &linVelY, const Decimal &linVelZ, const Velocity3DArma &angVel);
  
  Odometry3DArma(const Velocity3DArma &linVel, const Decimal &velAngX, const Decimal &velAngY, const Decimal &velAngZ);
  
  Odometry3DArma(const Velocity3DArma &linVel, const Velocity3DArma &angVel);
  
  Odometry3DArma& operator=(const Odometry3DArma& rhs);
  
  Odometry3DArma& operator=(const arma::Mat<Decimal>& rhs);
  
  Odometry3DArma& operator+=(const Odometry3DArma &pos); 
  
  Odometry3DArma& operator+=(const Decimal& scalar);
    
  /*const */Odometry3DArma operator-(Odometry3DArma &other) const;
  
  Odometry3DArma& operator-=(const Odometry3DArma &pos); 
  
  Odometry3DArma& operator-=(const Decimal& scalar);
    
  /*const */Odometry3DArma operator+(Odometry3DArma &other) const;
  
  Odometry3DArma& operator*=(const Odometry3DArma &pos); 
  
  Odometry3DArma& operator*=(const Decimal& scalar);
    
  /*const */Odometry3DArma operator*(Odometry3DArma &other) const;
  
  Odometry3DArma& operator/=(const Odometry3DArma &pos); 
  
  Odometry3DArma& operator/=(const Decimal& scalar);
    
  /*const */Odometry3DArma operator/(Odometry3DArma &other) const;
  
  bool operator==(const Odometry3DArma &other) const;
  
  Decimal getLinVelX() const;
  
  Decimal getLinVelY() const;
  
  Decimal getLinVelZ() const;
  
  Velocity3DArma getLinearVelocity() const;
  
  Decimal getAngVelX() const;
  
  Decimal getAngVelY() const;
  
  Decimal getAngVelZ() const;
  
  Velocity3DArma getAngularVelocity() const;
  
  void setLinVelX(const Decimal &x);
  
  void setLinVelY(const Decimal &y);
  
  void setLinVelZ(const Decimal &z);
  
  void setLinearVelocity(const Velocity3DArma &acc);
  
  void setAngVelX(const Decimal &x);
  
  void setAngVelY(const Decimal &y);
  
  void setAngVelZ(const Decimal &z);
  
  void setAngularVelocity(const Velocity3DArma &angVel);
  
  Odometry3DArma& matrixProd(const arma::Mat<Decimal>::fixed<3,3> &in);
  
  Odometry3DArma& matrixProd(const Roto3DArma &in);
  
  Odometry3DArma& matrixProdLinVel(const arma::Mat<Decimal>::fixed<3,3> &in);
  
  Odometry3DArma& matrixProdAngVel(const arma::Mat<Decimal>::fixed<3,3> &in);
  
  Odometry3DArma& matrixProdLinVel(const Roto3DArma &in);
  
  Odometry3DArma& matrixProdAngVel(const Roto3DArma &in);
  
  Odometry2DArma getOdometry2D() const;
  
  string print();
};
 
class Odometry3DTimeArma{
 private: 
  Odometry3DArma _odo;
  Time      _time;
 public:
  
  Odometry3DTimeArma();
  
  Odometry3DTimeArma(const Odometry3DArma &odo, const Time &time);
  
  Odometry3DTimeArma(const Decimal &linVelX, const Decimal &linVelY, const Decimal &linVelZ, const Decimal &velAngX, const Decimal &velAngY, const Decimal &velAngZ, const Time &time);
  
  Odometry3DTimeArma(const Decimal &linVelX, const Decimal &linVelY, const Decimal &linVelZ, const Velocity3DArma &angVel, const Time &time);
  
  Odometry3DTimeArma(const Velocity3DArma &linVel, const Decimal &velAngX, const Decimal &velAngY, const Decimal &velAngZ,const Time &time);
  
  Odometry3DTimeArma(const Velocity3DArma &linVel, const Velocity3DArma &angVel,const Time &time);
  
  Odometry3DTimeArma(const Decimal &linVelX, const Decimal &linVelY, const Decimal &linVelZ, const Decimal &velAngX, const Decimal &velAngY, const Decimal &velAngZ, const long int &sec, const long int &usec); 
  
  Odometry3DTimeArma(const Decimal &linVelX, const Decimal &linVelY, const Decimal &linVelZ, const Velocity3DArma &angVel, const long int &sec, const long int &usec);
  
  Odometry3DTimeArma(const Velocity3DArma &linVel, const Decimal &velAngX, const Decimal &velAngY, const Decimal &velAngZ, const long int &sec, const long int &usec);
  
  Odometry3DTimeArma(const Velocity3DArma &linVel, const Velocity3DArma &angVel, const long int &sec, const long int &usec);
  
  Odometry3DTimeArma(const Decimal &linVelX, const Decimal &linVelY, const Decimal &linVelZ, const Decimal &velAngX, const Decimal &velAngY, const Decimal &velAngZ, const Decimal &time);
  
  Odometry3DTimeArma(const Decimal &linVelX, const Decimal &linVelY, const Decimal &linVelZ, const Velocity3DArma &angVel, const Decimal &time);
  
  Odometry3DTimeArma(const Velocity3DArma &linVel, const Decimal &velAngX, const Decimal &velAngY, const Decimal &velAngZ,const Decimal &time);
  
  Odometry3DTimeArma(const Velocity3DArma &linVel, const Velocity3DArma &angVel,const Decimal &time);
  
  Odometry3DTimeArma& operator=(const Odometry3DTimeArma& rhs);
  
  Odometry2DTimeArma getOdometry2DTime() const;
  
  Time getTime() const;
  
  Decimal getTimeDCast();
  
  Odometry3DArma getOdo() const;
  
  Decimal getLinVelX() const;
  
  Decimal getLinVelY() const;
  
  Decimal getLinVelZ() const;
  
  Velocity3DArma getLinearVelocity() const;
  
  Decimal getAngVelX() const;
  
  Decimal getAngVelY() const;
  
  Decimal getAngVelZ() const;
  
  Velocity3DArma getAngularVelocity() const;
  
  void setOdom(const Odometry3DArma &odo);
  
  void setLinVelX(const Decimal &x);
  
  void setLinVelY(const Decimal &y);
  
  void setLinVelZ(const Decimal &z);
  
  void setLinearVelocity(const Velocity3DArma &acc);
  
  void setAngVelX(const Decimal &x);
  
  void setAngVelY(const Decimal &y);
  
  void setAngVelZ(const Decimal &z);
  
  void setAngularVelocity(const Velocity3DArma &angVel);
  
  void setTime(const Time &time);
  
  void setTime(Decimal time);
  
  void setTime(const long int &sec, const long int &usec);
  
  Odometry3DArma* getPointerOdo();
  
  Time* getPointerTime();
  
  string print();
  
};

class Position3DTimeArma{
 private:
    Position3DArma _pos;
  Time      _time;
 public:
  
  Position3DTimeArma();
  
  Position3DTimeArma(const Position3DArma &odo, const Time &time);
  
  Position3DTimeArma(const Decimal &x, const Decimal &y, const Decimal &z, const Time &time);
  
  Position3DTimeArma(const Decimal &x,const Decimal &y,const Decimal &z,const long int &sec,const long int &usec);
  
  Position3DTimeArma(const Decimal &x,const Decimal &y,const Decimal &z,const Decimal &time);
  
  Position3DTimeArma(const Position3D& A, const Time &time);
  
  Position3DTimeArma(const Position3D& A, const long int &sec,const long int &usec);
  
  Position3DTimeArma(const Position3D& A, const Decimal &time);
  
  Position3DTimeArma(const arma::Mat<Decimal>& A, const Time &time);
  
  Position3DTimeArma(const arma::Mat<Decimal>& A, const long int &sec,const long int &usec);
  
  Position3DTimeArma(const arma::Mat<Decimal>& A, const Decimal &time);
  
  Position3DTimeArma& operator=(const Position3DTimeArma& rhs);
  
  Position3DArma getPos() const;
  
  Time getTime() const;
  
  Decimal getTimeDCast();
  
  Position3DArma* getPointerPos();
  
  Time* getPointerTime();
  
  void setPos(const Position3DArma &pos);
  
  void setTime(const Time &time);
  
  void setTime(const Decimal in);
  
  Decimal posX() const;
  
  Decimal posY() const;
  
  Decimal posZ() const;
  
  void setPosX(const Decimal &p);
  
  void setPosY(const Decimal &p);
  
  void setPosZ(const Decimal &p);
  
  string print();
};
 
class PosiFeatures3DTimeArma{
 private:
  Time _time;
  vector<Position3DArma> _vecPos;
 public:
  
  PosiFeatures3DTimeArma();
  
  PosiFeatures3DTimeArma(const Time &time);
  
  PosiFeatures3DTimeArma(const int &sec, const int &usec);
  
  PosiFeatures3DTimeArma(const Decimal &time);
  
  PosiFeatures3DTimeArma(const vector<Position3DArma> &vec, const Time &time);
  
  PosiFeatures3DTimeArma(const vector<Position3DArma> &vec,const int &sec, const int &usec);
  
  PosiFeatures3DTimeArma(const vector<Position3DArma> &vec,const Decimal &time);
  
  PosiFeatures3DTimeArma(const Position3DArma &pos, const Time &time);
  
  PosiFeatures3DTimeArma(const Position3DArma &pos,const int &sec, const int &usec);
  
  PosiFeatures3DTimeArma(const Position3DArma &pos,const Decimal &time);
  
  PosiFeatures3DTimeArma(const PosiFeatures3DTimeArma &pos);
  
  PosiFeatures3DTimeArma& operator=(const PosiFeatures3DTimeArma& rhs);
  
  vector<Position3DArma> getPositionVector() const;
  
  Position3DArma getPositionAt(int index) const;
  
  Time getTime() const;
  
  Decimal getTimeDCast();
  
  void setTime(const Time &time);
  
  void setTime(const Decimal &time);
  
  void setTime(const long int &sec, const long int &usec);
  
  void addPosition(const Position3DArma &pos);
  
  void popLast();
  
  void setPosition(Position3DArma &pos,int index);
  
  Position3DArma getLastPosition();
  
  int getPositionSize() const;
  
  void insertPosition(Position3DArma &pos, int index);
  
  void freeVector();
  
  string print();
};

class Observation3DTimeArma{
 private:
  PosiFeatures3DTimeArma _vecTimePos;
  int _id;
 public:
  
  Observation3DTimeArma(int id=-1);
  
  Observation3DTimeArma(const Time &time,int id);
  
  Observation3DTimeArma(const int &sec, const int &usec,int id);
  
  Observation3DTimeArma(const Decimal &time,int id);
  
  Observation3DTimeArma(const vector<Position3DArma> &vec, const Time &time,int id);
  
  Observation3DTimeArma(const vector<Position3DArma> &vec,const int &sec, const int &usec,int id);
  
  Observation3DTimeArma(const vector<Position3DArma> &vec,const Decimal &time,int id);
  
  Observation3DTimeArma(const Position3DArma &pos, const Time &time,int id);
  
  Observation3DTimeArma(const Position3DArma &pos,const int &sec, const int &usec,int id);
  
  Observation3DTimeArma(const Position3DArma &pos,const Decimal &time,int id);
  
  Observation3DTimeArma(const PosiFeatures3DTimeArma &pos,const Decimal &time,int id);
  
  Observation3DTimeArma(const Observation3DTimeArma &pos);
  
  Observation3DTimeArma& operator=(const Observation3DTimeArma& rhs);
  
  vector<Position3DArma> getPositionVector() const;
  
  Position3DArma getPositionAt(int index) const;
  
  Time getTime() const;
  
  Decimal getTimeDCast();
  
  void setTime(const Time &time);
  
  void setTime(const Decimal &time);
  
  void setTime(const long int &sec, const long int &usec);
  
  void addPosition(const Position3DArma &pos);
  
  void popLast();
  
  void setPosition(Position3DArma &pos,int index);
  
  Position3DArma getLastPosition();
  
  int getPositionSize() const;
  
  void insertPosition(Position3DArma &pos, int index);
  
  void freeVector();
  
  int getId() const;
  
  void setId(int id);
  
  string print();
};


} // END OF NAMESPACE
#endif