R2Arma.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 __R2_ARMA_H_
#define __R2_ARMA_H_

//BOOST SERIALIZATION
#ifdef FOUND_BOOST_SER
 #include <boost/serialization/vector.hpp>
 #include <boost/archive/text_iarchive.hpp>
 #include <boost/archive/text_oarchive.hpp>
#endif

  
#include <armadillo>
#include <S1.h>
#include <SE2.h>
#include <Time.h>
// using namespace arma;
using namespace std;
namespace MipBaselib{  
typedef enum {
 IDENTITY_ARMA_MATRIX,
 ZERO_ARMA_MATRIX,
 NUM_CANONICAL_ARMA_MATRIX
} CanonicalArmaMatrix;

static const char* CanonicalArmaMatrixName[NUM_CANONICAL_ARMA_MATRIX] ={
 "IDENTITY_ARMA_MATRIX",
 "ZERO_ARMA_MATRIX"
};

class Roto2DArma : public arma::Mat<Decimal>::fixed<2,2>{
 public:
  Roto2DArma(CanonicalArmaMatrix init=ZERO_ARMA_MATRIX);
  
  Roto2DArma(const Decimal &a11,const Decimal &a12,const Decimal &a21, const Decimal &a22);
  
  Roto2DArma(const Decimal &yaw);
  
  Roto2DArma(const Roto2DArma &A);
  
  Roto2DArma(const arma::Mat<Decimal>::fixed<2,2>& rhs);
  
  Roto2DArma(const vector<Decimal> &vec);
  
  Roto2DArma& operator=(const Roto2DArma& rhs);
  
  Roto2DArma& operator=(const arma::Mat<Decimal>::fixed<2,2>& rhs);
  
  Roto2DArma& operator+=(const Roto2DArma &pos);
  
  /*const */Roto2DArma operator+(Roto2DArma &other) const;
  
  /*const */Roto2DArma operator-(Roto2DArma &other) const;
  
  Roto2DArma& operator-=(const Roto2DArma &pos); 
  
  Roto2DArma& operator*=(const Roto2DArma &pos); 
  
  Roto2DArma& operator*=(const Decimal& scalar);
  
  /*const */Roto2DArma operator*(Roto2DArma &other) const;
  
  Roto2DArma& operator/=(const Roto2DArma &pos); 
  
  Roto2DArma& operator/=(const Decimal& scalar);
    
  /*const */Roto2DArma operator/(Roto2DArma &other) const;
  
  string print();
  
  Roto2DArma invMatrix();
  
  arma::Mat<Decimal> subMat(int r, int c, int R, int C);
  
  arma::Row<Decimal>::fixed<2> getFirstRow() const;
  
  arma::Row<Decimal>::fixed<2> getSecondRow() const;
  
  arma::Col<Decimal>::fixed<2> getFirstCol() const;
  
  arma::Col<Decimal>::fixed<2> getSecondCol() const;
  
  Decimal getElem(int r,int c) const;
  
  void setElem(int r,int c,const Decimal &item);
  
  Decimal getYaw() const;
  
  Angle getYawAngle() const;
  
  arma::Mat<Decimal>::fixed<2,2>* getMat();
};

class Position2DArma: public arma::Col<Decimal>::fixed<2>{
 
  private:
#ifdef FOUND_BOOST_SER
  friend class boost::serialization::access;
  template<class Archive>
    void serialize(Archive & ar, const unsigned int version){
   ar & (*this)(0);
   ar & (*this)(1);
    }
#endif
 
 public:
  Position2DArma();
  
  Position2DArma(const Decimal &x,const Decimal &y);
  
  Position2DArma(const Angle& theta);
  
  Position2DArma(const Decimal &d,const Angle& theta);
  
  Position2DArma(const Position2DArma& A);
  
  Position2DArma(const Position &pos);
  
  Position2DArma(const arma::Mat<Decimal>& A);
  
  Position2DArma(stringstream& ss);
  
  Position2DArma& operator=(const Position& pos);
  
  Position2DArma& operator=(const Position2DArma& rhs);
  
  Position2DArma& operator=(const arma::Mat<Decimal>& rhs);
  
  bool operator==(const Position2DArma &other) const;
  
  Position2DArma& operator+=(const Position2DArma &pos);
  
  Position2DArma& operator+=(const Position &pos);
  
  Position2DArma& operator+=(const arma::Col<Decimal>::fixed<2> &pos); 
  
  Position2DArma& operator+=(const Decimal& scalar);
    
  /*const */Position2DArma operator-(Position2DArma &other) const;
  
  Position2DArma& operator-=(const Position2DArma &pos); 
  
  Position2DArma& operator-=(const Decimal& scalar);
  
  Position2DArma& operator-=(const arma::Col<Decimal>::fixed<2> &pos); 
  
  /*const */Position2DArma operator+(Position2DArma &other) const;
  
  Position2DArma& operator*=(const Position2DArma &pos); 
  
  Position2DArma& operator*=(const Decimal& scalar);
  
  Position2DArma& operator*=(const arma::Mat<Decimal>::fixed<2,2> &mat);
  
  Position2DArma& operator*=(const arma::Col<Decimal>::fixed<2> &pos); 
  
  Position2DArma& operator*=(const Roto2DArma &mat);
  
  Position2DArma& operator*=(const arma::Mat<Decimal>::fixed<3,3> &mat);
  
  Position2DArma operator*(const arma::Col<Decimal>::fixed<2> &pos) const;
  
  /*const */Position2DArma operator*(Position2DArma &other) const;
  
  Position2DArma& operator/=(const Position2DArma &pos); 
  
  Position2DArma& operator/=(const Decimal& scalar);
  
  Position2DArma& operator/=(const arma::Col<Decimal>::fixed<2> &pos);
    
  /*const */Position2DArma operator/(Position2DArma &other) const;
  
  /*const */Position2DArma operator/(const arma::Col<Decimal>::fixed<2> &pos) const;
  
  Position2DArma matrixProd(const arma::Mat<Decimal>::fixed<2,2> &mat);
  
  Position2DArma matrixHomoProd(const arma::Mat<Decimal>::fixed<3,3> &mat);
  
  Position2DArma matrixProd(Roto2DArma &mat);
  
  Position2DArma minimum(const Position2DArma& p) const;
  
  Position2DArma maximum(const Position2DArma& p) const;
  
  Decimal x() const;
  
  Decimal y() const;
  
  void setX(const Decimal &p);
  
  void setY(const Decimal &p);
  
  Angle azimuthAngle() const;
  
  Decimal azimuth() const;
  
  Angle bearing() const;
  
  Decimal bearingInv() const;
  
  Decimal scalar(const Position2DArma& p) const;
  
  vector<Decimal> vectorize() const;
  
  string print(int precision=4) const;
  
  Decimal norm(int p=2) const;
  
  Decimal squareNorm(int p=2) const;
  
  Decimal dist( Position2DArma& other, int p=2) const;
  
  string exportAsString() const;
  
    // String getter & setter (Syntax "x,y")
  bool fromString(const string& s);
  string toString() const;
};

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

class Acceleration2DArma : public Position2DArma{
 public:
  
  Acceleration2DArma() : Position2DArma() {}
  
  Acceleration2DArma(const Decimal &x_in,const Decimal &y_in) : Position2DArma(x_in,y_in) {}
  
  Acceleration2DArma(const Acceleration2DArma& p);
  
  Acceleration2DArma(Position2DArma p) : Position2DArma(p) {}
  
  Acceleration2DArma(const Position &pos) : Position2DArma(pos){};
  
  Acceleration2DArma(const arma::Mat<Decimal>& A) : Position2DArma(A){};
  
    Acceleration2DArma& operator=(const Acceleration2DArma& rhs);
  
  bool operator==(const Acceleration2DArma &other) const;
  
  string print(PrintTypes type = PRINT_TYPE_WITH_UNITS) const;
}; 
 
class Roto2DHomoArma : public arma::Mat<Decimal>::fixed<3,3>{
 public:
  Roto2DHomoArma(CanonicalArmaMatrix init=ZERO_ARMA_MATRIX);
  
  Roto2DHomoArma(const Decimal &a11,const Decimal &a12, const Decimal &a21, const Decimal &a22);
  
  Roto2DHomoArma(const Decimal &a11,const Decimal &a12, const Decimal &a21, const Decimal &a22,const Decimal &x,const Decimal &y);
  
  Roto2DHomoArma(const Decimal &a11,const Decimal &a12, const Decimal &a21, const Decimal &a22, const arma::Col<Decimal>::fixed<2> &trasl);
  
  Roto2DHomoArma(const Decimal &yaw);
  
  Roto2DHomoArma(const Decimal &yaw, const Decimal &x, const Decimal &y);
  
  Roto2DHomoArma(const Roto2DHomoArma &A);
  
  Roto2DHomoArma(const arma::Mat<Decimal>::fixed<3,3>& rhs);
  
  Roto2DHomoArma(const Roto2DArma& other);
  
  Roto2DHomoArma(const Roto2DArma& other, const Decimal &x, const Decimal &y);
  
  Roto2DHomoArma(const Roto2DArma& other, const arma::Col<Decimal>::fixed<2> &trasl);
  
  Roto2DHomoArma& operator=(const Roto2DHomoArma& rhs);
  
  Roto2DHomoArma& operator=(const arma::Mat<Decimal>::fixed<3,3>& rhs);
  
  Roto2DHomoArma& operator+=(const Roto2DHomoArma &pos);
  
  /*const */Roto2DHomoArma operator+(Roto2DHomoArma &other) const;
  
  /*const */Roto2DHomoArma operator-(Roto2DHomoArma &other) const;
  
  Roto2DHomoArma& operator-=(const Roto2DHomoArma &pos); 
  
  Roto2DHomoArma& operator*=(const Roto2DHomoArma &pos); 
  
  Roto2DHomoArma& operator*=(const Decimal& scalar);
    
  /*const */Roto2DHomoArma operator*(Roto2DHomoArma &other) const;
  
  Roto2DHomoArma& operator/=(const Roto2DHomoArma &pos); 
  
  Roto2DHomoArma& operator/=(const Decimal& scalar);
    
  /*const */Roto2DHomoArma operator/(Roto2DHomoArma &other) const;
  
  string print();
  
  Roto2DHomoArma invMatrix();
  
  Decimal getElem(int r,int c) const;
  
  void setElem(int r,int c,const Decimal &item);
  
  void setRotation(Roto2DArma &rhs);
  
  Decimal getYaw() const;
  
  Angle getYawAngle() const;
  
  arma::Col<Decimal>::fixed<2> getTraslation();
  
  arma::Mat<Decimal>::fixed<2,2> getRotation();
  
  Position2DArma getTraslation2DArma();
  
  Roto2DArma getRotation2DArma();
};
 
class Pose2DArma{
 private:
  Angle      _yaw;
  Position2DArma _pos;
 public:
  
  Pose2DArma();
  
  Pose2DArma(const Decimal &x, const Decimal &y, const Angle &yaw);
  
  Pose2DArma(const Decimal& x, const Decimal &y, const Decimal &yaw);
  
  Pose2DArma(const Position2DArma &pos, const Angle &ori);
  
  Pose2DArma(const Position2DArma &pos, const Decimal &ori);
  
  Pose2DArma(const arma::Mat<Decimal>::fixed<2,1> &pos, const Decimal &ori);
  
  Pose2DArma(const arma::Mat<Decimal>::fixed<2,1> &pos, const Angle &ori);
  
  Pose2DArma(const Pose &pos);
  
  Pose2DArma& operator*=(const arma::Mat<Decimal>::fixed<3,3>& rhs);
  
  /*const*/ Pose2DArma operator*(const arma::Mat<Decimal>::fixed<3,3>& rhs) const;
  
  Pose2DArma& operator*=(const arma::Mat<Decimal>::fixed<4,4>& rhs);
  
  /*const*/ Pose2DArma operator*(const arma::Mat<Decimal>::fixed<4,4>& rhs) const;
  
  Pose2DArma& operator=(const Pose& rhs);
  
  Pose2DArma& operator=(const Pose2DArma& rhs);
  
  Pose2DArma& operator+=(const Pose2DArma& a);
  
  const Pose2DArma operator+(const Pose2DArma &other) const;
  
  Pose2DArma& operator-=(const Pose2DArma& a);
  
  const Pose2DArma operator-(const Pose2DArma &other) const;
  
  bool operator==(const Pose2DArma &other) const;
  
  bool operator!=(const Pose2DArma &other) const;
  
  Position2DArma getPos() const;
  
  Decimal getX() const;
  
  Decimal getY() const;
  
  Angle getOriAngle();
  
  Decimal getOri();
  
  void setPos(const Position2DArma &pos);
  
  void setX(const Decimal &x);
  
  void setY(const Decimal &y);
  
  void setOri(const Angle& ori);
  
  void setOri(const Decimal& ori);
  
  Decimal norm(int p=2) const;
  
  Decimal azimuth() const;
  
  Angle azimuthAngle() const;
  
  Pose2DArma& relativeCoord(const Pose2DArma& other);
  
  void directComposition(Pose2DArma &t);
  
  void inverseComposition(Pose2DArma &t);
  
  string print();
};

class Odometry2DArma : public arma::Col<Decimal>::fixed<3>{
 public:
  
  Odometry2DArma();
  
  Odometry2DArma(const Decimal &xVel, const Decimal &yVel, const Decimal &zOmega);
  
  Odometry2DArma(const Position2DArma &A, const Decimal &zOmega);
  
  Odometry2DArma(const Odometry2DArma &A);
  
  Odometry2DArma& operator=(const Odometry2DArma& rhs);
  
  Odometry2DArma& operator=(const arma::Mat<Decimal>& rhs);
  
  Odometry2DArma& operator+=(const Odometry2DArma& A);
  
  Odometry2DArma& operator+=(const Decimal& scalar);
  
  /*const */Odometry2DArma operator+(Odometry2DArma &other) const;
  
  Odometry2DArma& operator-=(const Odometry2DArma& A);
  
  Odometry2DArma& operator-=(const Decimal& scalar);
  
  /*const */Odometry2DArma operator-(Odometry2DArma &other) const;
  
  Odometry2DArma& operator*=(const Odometry2DArma& A);
  
  Odometry2DArma& operator*=(const Decimal& scalar);
  
  /*const */Odometry2DArma operator*(Odometry2DArma &other) const;
  
  Odometry2DArma& operator/=(const Odometry2DArma& A);
  
  Odometry2DArma& operator/=(const Decimal& scalar);
  
  /*const */Odometry2DArma operator/(Odometry2DArma &other) const;
  
  bool operator==(const Odometry2DArma &other) const;
  
  bool operator!= ( const Odometry2DArma& other ) const;
  
  Decimal getVelX() const;
  
  Decimal getVelY() const;
  
  Decimal getOmegaZ() const;
  
  Velocity2DArma getVelocity() const;
  
  void setVelX(const Decimal &xVel);
  
  void setVelY(const Decimal &yVel);
  
  void setVelocity(const Velocity2DArma &vel);
  
  void setOmegaZ(const Decimal &zOmega);
  
  string print();
};

class Odometry2DTimeArma{
 private:
  Odometry2DArma _odo;
  Time      _time;
 public:
  
  Odometry2DTimeArma();
  
  Odometry2DTimeArma(const Decimal &xVel, const Decimal &yVel, const Decimal &zOmega, const Time &time);
  
  Odometry2DTimeArma(const Position2DArma &A, const Decimal &zOmega, const Time &time);
  
  Odometry2DTimeArma(const Odometry2DArma &A, const Time &time);
  
  Odometry2DTimeArma(const Decimal &xVel, const Decimal &yVel, const Decimal &zOmega, const long int &sec, const long int &usec);
  
  Odometry2DTimeArma(const Position2DArma &A, const Decimal &zOmega, const long int &sec, const long int &usec);
  
  Odometry2DTimeArma(const Odometry2DArma &A, const long int &sec, const long int &usec);
  
  Odometry2DTimeArma(const Decimal &xVel, const Decimal &yVel, const Decimal &zOmega, const Decimal &time);
  
  Odometry2DTimeArma(const Position2DArma &A, const Decimal &zOmega, const Decimal &time);
  
  Odometry2DTimeArma(const Odometry2DArma &A, const Decimal &time);
  
  Odometry2DTimeArma& operator=(const Odometry2DTimeArma& rhs);
  
  Odometry2DArma getOdo() const;
  
  Time getTime() const;
  
  Decimal getTimeDCast();
  
  Decimal getOdoVelX() const;
  
  Decimal getOdoVelY() const;
  
  Decimal getOdoOmegaZ() const;
  
  Velocity2DArma getOdoVelocity() const;
  
  void setOdometry(const Odometry2DArma &odo);
  
  void setTime(const Time &time);
  
  void setTime(const Decimal time);
  
  void setTime(const long int &sec,const long int &usec);
  
  void setOdoVelX(const Decimal &xVel);
  
  void setOdoVelY(const Decimal &yVel);
  
  void setOdoVelocity(const Velocity2DArma &vel);
  
  void setOdoOmegaZ(const Decimal &zOmega);
  
  string print();
};

class Position2DTimeArma{
 private:
  Position2DArma _pos;
  Time      _time;
 public:
  
  Position2DTimeArma();
  
  Position2DTimeArma(const Position2DArma &odo, const Time &time);
  
  Position2DTimeArma(const Decimal &x, const Decimal &y, const Time &time);
  
  Position2DTimeArma(const Decimal &x,const Decimal &y,const long int &sec,const long int &usec);
  
  Position2DTimeArma(const Decimal &x,const Decimal &y,const Decimal &time);
  
  Position2DTimeArma(const Position& A, const Time &time);
  
  Position2DTimeArma(const Position& A, const long int &sec,const long int &usec);
  
  Position2DTimeArma(const Position& A, const Decimal &time);
  
  Position2DTimeArma(const arma::Mat<Decimal>& A, const Time &time);
  
  Position2DTimeArma(const arma::Mat<Decimal>& A, const long int &sec,const long int &usec);
  
  Position2DTimeArma(const arma::Mat<Decimal>& A, const Decimal &time);
  
  Position2DTimeArma& operator=(const Position2DTimeArma& rhs);
  
  Position2DArma getPos() const;
  
  Time getTime() const;
  
  Decimal getTimeDCast();
  
  void setPos(const Position2DArma &pos);
  
  void setTime(const Time &time);
  
  void setTime(const Decimal time);
  
  void setTime(const long int &sec,const long int &usec);
  
  Decimal posX() const;
  
  Decimal posY() const;
  
  void setPosX(const Decimal &p);
  
  void setPosY(const Decimal &p);
  
  
  string print();
};

class PosiFeatures2DTimeArma{
 private:
  Time _time;
  vector<Position2DArma> _vecPos;
 public:
  
  PosiFeatures2DTimeArma();
  
  PosiFeatures2DTimeArma(const Time &time);
  
  PosiFeatures2DTimeArma(const int &sec, const int &usec);
  
  PosiFeatures2DTimeArma(const Decimal &time);
  
  PosiFeatures2DTimeArma(const vector<Position2DArma> &vec, const Time &time);
  
  PosiFeatures2DTimeArma(const vector<Position2DArma> &vec,const int &sec, const int &usec);
  
  PosiFeatures2DTimeArma(const vector<Position2DArma> &vec,const Decimal &time);
  
  PosiFeatures2DTimeArma(const Position2DArma &pos, const Time &time);
  
  PosiFeatures2DTimeArma(const Position2DArma &pos,const int &sec, const int &usec);
  
  PosiFeatures2DTimeArma(const Position2DArma &pos,const Decimal &time);
  
  PosiFeatures2DTimeArma(const PosiFeatures2DTimeArma &pos);
  
  PosiFeatures2DTimeArma& operator=(const PosiFeatures2DTimeArma& rhs);
  
  vector<Position2DArma> getPositionVector() const;
  
  Position2DArma 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 Position2DArma &pos);
  
  void popLast();
  
  void setPosition(Position2DArma &pos,int index);
  
  Position2DArma getLastPosition();
  
  int getPositionSize() const;
  
  void insertPosition(Position2DArma &pos, int index);
  
  void freeVector();
  
  string print();
};

class Observation2DTimeArma{
 private:
  PosiFeatures2DTimeArma _vecTimePos;
  int _id;
 public:
  
  Observation2DTimeArma(int id=-1);
  
  Observation2DTimeArma(const Time &time,int id);
  
  Observation2DTimeArma(const int &sec, const int &usec,int id);
  
  Observation2DTimeArma(const Decimal &time,int id);
  
  Observation2DTimeArma(const vector<Position2DArma> &vec, const Time &time,int id);
  
  Observation2DTimeArma(const vector<Position2DArma> &vec,const int &sec, const int &usec,int id);
  
  Observation2DTimeArma(const vector<Position2DArma> &vec,const Decimal &time,int id);
  
  Observation2DTimeArma(const Position2DArma &pos, const Time &time,int id);
  
  Observation2DTimeArma(const Position2DArma &pos,const int &sec, const int &usec,int id);
  
  Observation2DTimeArma(const Position2DArma &pos,const Decimal &time,int id);
  
  Observation2DTimeArma(const Observation2DTimeArma &pos);
  
  Observation2DTimeArma(const PosiFeatures2DTimeArma &pos,int id);
  
  Observation2DTimeArma& operator=(const Observation2DTimeArma& rhs);
  
  vector<Position2DArma> getPositionVector() const;
  
  Position2DArma 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 Position2DArma &pos);
  
  void popLast();
  
  void setPosition(Position2DArma &pos,int index);
  
  Position2DArma getLastPosition();
  
  int getPositionSize() const;
  
  void insertPosition(Position2DArma &pos, int index);
  
  void freeVector();
  
  int getId() const;
  
  void setId(int id);
  
  string print();
};
} // End of namespace
#endif