SE2.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 __SE2_H_
#define __SE2_H_

#ifdef MIP_HOST_APPLE
#include <applePatch.h>
#endif

#include <stdio.h>
#include <math.h>
#include <vector>
#include <iostream>
#include <string>
#include <sstream>
#include <sys/time.h>
#include <sys/select.h>
#include <assert.h>

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

#include <Types.h>

#include <R2.h>
#include <S1.h>
#include <Time.h>

using namespace std;

class Pose {
 public:
  Pose();
  
  Pose(Decimal x, Decimal y,Angle theta);
  
  Pose(Position pos,Angle ori);
    
  Pose(const Pose& p);
    
  Pose& operator=(const Pose& rhs);
    
  Pose& operator+=(const Pose& a);
    
  Pose& operator-=(const Pose& a);
    
  const Pose operator+(const Pose &other) const;
  
  const Pose operator-(const Pose &other) const;
  
  const Position operator*(Position p);
  
  bool operator==(const Pose &other) const;
  
  bool operator!=(const Pose &other) const;

  Decimal x() const;
  
  Decimal y() const;

  Position pos() const;

  Angle ori() const;
  
  void setX(Decimal);
  
  void setY(Decimal);
  
  void setOri(Angle);
  
  bool valid();
  
  string print(int linPrecision=4,int angPrecision=2) const;
  
  // String getter & setter (Syntax "x,y,theta")
  bool fromString(const string& s);
  string toString() const;

  void directComposition(Pose &t);
  
  void inverseComposition(Pose &t);
  
  void uRotateFrame(Angle &t);
  
  void uInverseRotateFrame(Angle &t);


 private:
#ifdef FOUND_BOOST_SER
  friend class boost::serialization::access;
  template<class Archive>
    void serialize(Archive & ar, const unsigned int version){
   ar & _pos; 
   ar & _ori;
   ar & _valid;
    }
#endif

  Position _pos;
  Angle   _ori;
  bool   _valid; 
};







#endif



/* @} */