SE3.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 __SE3_H_
#define __SE3_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 <R3.h>
#include <S1.h>
#include <S3.h>

using namespace std;

class Pose3D {
 public:
  Pose3D() {
   _valid = false;
  };
  Pose3D(Decimal x, Decimal y, Decimal z, Angle roll, Angle pitch, Angle yaw) {
   _pos = Position3D(x,y,z);
   _ori = Orientation3D(roll, pitch, yaw);
   _valid = true;
  };
  Pose3D(Position3D pos,Orientation3D ori) {
   _pos = pos;
   _ori = ori;
   _valid = true;
  };
  Pose3D(Decimal x, Decimal y, Decimal z,Orientation3D ori) {
   _pos = Position3D(x,y,z);
   _ori = ori;
   _valid = true;
  };
  Pose3D(Position3D pos,Angle roll, Angle pitch, Angle yaw) {
   _pos = pos;
   _ori = Orientation3D(roll, pitch, yaw);
   _valid = true;
  };
  Pose3D(const Pose3D& p) {
   _pos = p._pos;
   _ori = p._ori;
   _valid =  p._valid;
  };
  Pose3D& operator=(const Pose3D& rhs){
   if (this != &rhs){      // Not necessary in this case but it is useful to don't forget it
    _ori = rhs._ori;
    _pos = rhs._pos;
    _valid =  rhs._valid;
   }
   return *this;
  }
  Pose3D& operator+=(const Pose3D& a) {
   _ori += a._ori;
   _pos += a._pos;
   return *this;
  }
  Pose3D& operator-=(const Pose3D& a) {
   _ori -= a._ori;
   _pos -= a._pos;
   return *this;
  }
  const Pose3D operator+(const Pose3D &other) const {
   return Pose3D(*this) += other;
  }
  const Pose3D operator-(const Pose3D &other) const {
   return Pose3D(*this) -= other;
  }
  
  const Position3D operator*(Position3D p) {
   // BUG manca termine di traslazione!!!!
   /*TODO*/ Decimal x = cos(_ori.yaw().dCast2Pi())*cos(_ori.pitch().dCast2Pi())*p.x() +
          (cos(_ori.yaw().dCast2Pi())*sin(_ori.pitch().dCast2Pi())*sin(_ori.roll().dCast2Pi())
          -sin(_ori.yaw().dCast2Pi())*cos(_ori.roll().dCast2Pi()))*p.y() +
          (cos(_ori.yaw().dCast2Pi())*sin(_ori.pitch().dCast2Pi())*cos(_ori.roll().dCast2Pi())
          -sin(_ori.yaw().dCast2Pi())*sin(_ori.roll().dCast2Pi()))*p.z();
   /*TODO*/ Decimal y = sin(_ori.yaw().dCast2Pi())*cos(_ori.pitch().dCast2Pi())*p.x() +
          (sin(_ori.yaw().dCast2Pi())*sin(_ori.pitch().dCast2Pi())*sin(_ori.roll().dCast2Pi())
          -cos(_ori.yaw().dCast2Pi())*cos(_ori.roll().dCast2Pi()))*p.y() +
          (sin(_ori.yaw().dCast2Pi())*sin(_ori.pitch().dCast2Pi())*cos(_ori.roll().dCast2Pi())
          -cos(_ori.yaw().dCast2Pi())*sin(_ori.roll().dCast2Pi()))*p.z();
   /*TODO*/ Decimal z = -sin(_ori.pitch().dCast2Pi())*p.x() +
          cos(_ori.pitch().dCast2Pi())*sin(_ori.roll().dCast2Pi())*p.y() +
          cos(_ori.pitch().dCast2Pi())*cos(_ori.roll().dCast2Pi())*p.z();
   return Position3D(x,y,z);
  }
  bool operator==(const Pose3D &other) const {
   if( (_pos == other._pos) && (_ori == other._ori) ) 
    return true;
   else return false;
  }
  bool operator!=(const Pose3D &other) const {
   return !(*this == other);
  }
  Position3D pos() {
   return _pos;
  }
  Orientation3D ori () {
   return _ori;
  }
  bool valid () {
   return _valid;
  }
  string print(){
   stringstream s;
   s << "(" << _pos.print() << "," << _ori.print() << ")" ;
   return s.str();
  }
  void directComposition(Pose3D &t){
   //TODO
/*   Decimal cosTh = cos( t.ori().dCast() );
   Decimal sinTh = sin( t.ori().dCast() );
   
   Position pos0(pos());
   Angle ori0 = ori();
 
   (*this) = Pose3D(cosTh*pos0.x()-sinTh*pos0.y()+t.pos().x(), sinTh*pos0.x()+cosTh*pos0.y()+t.pos().y(), ori()+t.ori());*/
  }
  void inverseComposition(Pose3D &t){
   //TODO
/*   Decimal cosAth = cos(t.ori().dCast());
   Decimal sinAth = sin(t.ori().dCast());
   Pose3D temp(-t.pos().x()*cosAth-t.pos().y()*sinAth, +t.pos().x()*sinAth-t.pos().y()*cosAth, Angle(-t.ori().dCast()) );
   directComposition(temp);*/
  }
  /*TODO implementare le composizioni di pose*/
  /*TODO implementare le inversioni di pose*/
  
  void directCompositionYaw(Pose3D &t){
        Decimal cosTh = cos( t.ori().yaw().dCast2Pi() );
        Decimal sinTh = sin( t.ori().yaw().dCast2Pi() );
        
        Position3D pos0(pos());
        Orientation3D ori0 = ori();

        (*this) = Pose3D(cosTh*pos0.x()-sinTh*pos0.y()+t.pos().x(), sinTh*pos0.x()+cosTh*pos0.y()+t.pos().y(),t.pos().z()+pos0.z(), ori()+t.ori());
  }
  void inverseCompositionYaw(Pose3D &t){
        Decimal cosAth = cos(t.ori().yaw().dCast2Pi());
        Decimal sinAth = sin(t.ori().yaw().dCast2Pi());
        Pose3D temp(-t.pos().x()*cosAth-t.pos().y()*sinAth, +t.pos().x()*sinAth-t.pos().y()*cosAth, t.pos().z(), Orientation3D(0.0,0.0,-t.ori().yaw().dCast2Pi()) );
        directCompositionYaw(temp);
  }  
  
  void setPos(Position3D &p){
   _pos = p;
  }
  
  void setOri(Orientation3D &o){
   _ori = o;
  }
  
  bool fromString(const string& s){
   string comma=",",xs,ys,zs,s2,s3,s4,s5,rs,ps,yaws;
   int commaPos;
   if((commaPos=s.find(comma))!=s.npos){
    xs = s.substr(0,commaPos);
    s2 = s.substr(commaPos+1,s.size()-commaPos-1);
    if((commaPos=s2.find(comma))!=s2.npos){
     ys = s2.substr(0,commaPos);
     s3 = s2.substr(commaPos+1,s2.size()-commaPos-1);
     if((commaPos=s3.find(comma))!=s3.npos){
      zs = s3.substr(0,commaPos);
      s4 = s3.substr(commaPos+1,s3.size()-commaPos-1);
      if((commaPos=s4.find(comma))!=s4.npos){
       rs = s4.substr(0,commaPos);
       s5 = s4.substr(commaPos+1,s4.size()-commaPos-1);
       if((commaPos=s5.find(comma))!=s5.npos){
        ps = s5.substr(0,commaPos);
        yaws = s5.substr(commaPos+1,s5.size()-commaPos-1);
       }else{
   //      MIP_STATIC_WARNING("Position3D","Second comma not present in Position3DOption.");
        return false;
       }
      }else{
  //      MIP_STATIC_WARNING("Position3D","Second comma not present in Position3DOption.");
       return false;
      }
     }else{
 //      MIP_STATIC_WARNING("Position3D","Second comma not present in Position3DOption.");
      return false;
     }
    }else{
//      MIP_STATIC_WARNING("Position3D","Second comma not present in Position3DOption.");
     return false;
    }
    
    Decimal x, y, z, ro, pit, yaw;
    istringstream ssx(xs);
    ssx >> x;
    istringstream ssy(ys);
    ssy >> y;
    istringstream ssz(zs);
    ssz >> z;
    istringstream ssr(rs);
    ssx >> ro;
    istringstream ssp(ps);
    ssy >> pit;
    istringstream ssyaw(yaws);
    ssz >> yaw;
    
    _pos.setX(x);
    _pos.setY(y);
    _pos.setZ(z);
    
    _ori.setRoll(ro);
    _ori.setPitch(pit);
    _ori.setYaw(yaw);
   }else{
//     MIP_STATIC_WARNING("Position3D","First comma not present in Position3DOption.");
    return false;
   }
   
   return true;
  }
  
  string toString() const{
   stringstream ss;
   ss << _pos.toString() << "," << _ori.toString();
   return ss.str();
  }
  
 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
  
    Position3D    _pos;
    Orientation3D _ori;
    bool   _valid; 
};

class PoseFeature3D : public Association<Pose3D,int>{
 private:
  
//   friend class boost::serialization::access;
//   template<class Archive>
//     void serialize(Archive & ar, const unsigned int version){
//    ar & _pos;
//    ar & _ori;
//    ar & _valid;
//   }

 public:
  
  
  PoseFeature3D() : Association<Pose3D,int>(){}
  
  PoseFeature3D (Pose3D p) : Association<Pose3D,int>(p){}
  
  PoseFeature3D (Pose3D p, int i) : Association<Pose3D,int>(p,i){}

  PoseFeature3D (const PoseFeature3D& f) : Association<Pose3D,int>(f){};
  
  using Association<Pose3D,int>::operator=;
  
  Pose3D getPose(void){
   return getObj();
  }
  int getId(void){
   return getAsso();
  }
  void setId(int a){
   associate(a);
  }
  
  string print(){
   stringstream s;
   if(associated()){
    s << "[" << getPose().print()  << "," << getId() << "]";
   }else{
    s << "[" << getPose().print()  << ",na]";
   }
   return s.str();
  }

  bool notEqual(PoseFeature3D &pose, Decimal toll){
   // Variable TEMP not used
//    Decimal temp = getPose().pos().x() - pose.getPose().pos().x();
   if ( !DecimalUtilities::equal(getPose().pos().x(), pose.getPose().pos().x(), toll) ) return true;
   if ( !DecimalUtilities::equal(getPose().pos().y(), pose.getPose().pos().y(), toll) ) return true;
   if ( !DecimalUtilities::equal(getPose().pos().z(), pose.getPose().pos().z(), toll) ) return true;
   if ( !DecimalUtilities::equal(getPose().ori().roll().dCast2Pi(), pose.getPose().ori().roll().dCast2Pi(), toll) ) return true;
   if ( !DecimalUtilities::equal(getPose().ori().pitch().dCast2Pi(), pose.getPose().ori().pitch().dCast2Pi(), toll) ) return true;
   if ( !DecimalUtilities::equal(getPose().ori().yaw().dCast2Pi(), pose.getPose().ori().yaw().dCast2Pi(), toll) ) return true;
   if ( getId() != pose.getId() )           return true;
   return false;
  }
};


#endif