ProbMultiRegCam3DStr.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$
//
// Copyright 2008, 2009, 2010  Antonio Franchi
//
// This file is part of TeleKyb.
//
// 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
//
// ----------------------------------------------------------------------------
 
 
#ifndef __PROB_MULTI_REG_CAM_STR3D_H
#define __PROB_MULTI_REG_CAM_STR3D_H
 
//Include libraries
#include "time.h"
#include <fstream>
#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <iostream>
#include <vector>
#include <map>
#include <baselib.h>
 
namespace MipBaselib {
 
 class PosId3D : public Association<Position3D,int>{
  public:
   
   PosId3D() : Association<Position3D,int>(){}
   
   PosId3D (Position3D p) : Association<Position3D,int>(p){}
   
   PosId3D (Position3D p, int i) : Association<Position3D,int>(p,i){}
   
   PosId3D (const PosId3D& f) : Association<Position3D,int>(f){};
   
   using Association<Position3D,int>::operator=;
   
   Position3D getPosition(void){
     return getObj();
   }
   int getId(void){
     return getAsso();
   }
   void setId(int a){
     associate(a);
   }
   
   string print(){
     stringstream s;
     if(associated()){
      s << "[" << getPosition().print()  << "," << getId() << "]";
     }else{
      s << "[" << getPosition().print()  << ",na]";
     }
     return s.str();
   }
   
   bool notEqual(PosId3D &A, Decimal toll){
     if ( fabs(getPosition().x() - A.getPosition().x()) > toll ) return true;
     if ( fabs(getPosition().y() - A.getPosition().y()) > toll ) return true;
     if ( fabs(getPosition().z() - A.getPosition().z()) > toll ) return true;
     /*if ( !DoubleUtilities::equal(getPosition().x(), A.getPosition().x(), toll) ) return true;
     if ( !DoubleUtilities::equal(getPosition().y(), A.getPosition().y(), toll) ) return true;
     if ( !DoubleUtilities::equal(getPosition().z(), A.getPosition().z(), toll) ) return true;*/
     if ( getId() != A.getId() )           return true;
     return false;
   }
 };
 
} // end of namespace MipBaselib
 
using namespace MipBaselib;
 
namespace kybalg{
 class bearStr{
  public:
   Angle azimuth;
   Angle zenith;
   
   bearStr();
   
   bearStr(Angle az,Angle zn);
   
   bearStr(Decimal az,Decimal zn);
   
   ~bearStr();
   
   bearStr& operator= (const bearStr &A);
   
   bool operator==(const bearStr &other) const;
   
   string print();
 };
 
 template <class T>
  class ObjId {
   private:
    vector<T> _obj;
    int _id;
   public:
    
    ObjId() {
     _id = -1;
     _obj.clear();
    }
    
    ObjId(T obj, int id) {
     _id = id;
     _obj.push_back(obj);
    }
    
    ObjId(vector<T> obj, int id) {
     _id = id;
     _obj.clear();
     _obj = obj;
    }
    
    ~ObjId() {
     _id = -1;
     _obj.clear();
    }
    
    ObjId & operator=(const ObjId& rhs) {
     if (this != &rhs) { // Not necessary in this case but it is useful to don't forget it
     _id = rhs._id;
     _obj = rhs._obj;
     }
     return (*this);
    }
    
    vector<T> getObj() {
     return _obj;
    }
    
    int getId() {
     return _id;
    }
    
    void setObj(T obj) {
     _obj.push_back(obj);
    }
    
    void setObj(vector<T> obj, int id) {
     _obj = obj;
     _id = id;
    }
    
    void setObj(vector<T> obj) {
     _obj = obj;
    }
    
    void setObj(T obj, int id) {
     _obj.push_back(obj);
     _id = id;
    }
    
    void setSubObj(T obj, int pos){
     if(pos>=0&&pos<_obj.size())
     _obj.at(pos) = obj;
    }
    
    void setId(int id) {
     _id = id;
    }
    
    void clearObj() {
     _obj.clear();
    }
   
  };
 
 // Create typedef structures
 // AngleBearId: feature set of a robot
 // PosId: renaming posifeature
 typedef ObjId<bearStr> AngleBearId;
 typedef PosiFeature PosId;
 
 class BearIdTime : public AngleBearId {
  private:
   long int _ttSec;
   long int _ttUSec;
  public:
   BearIdTime(){};

   BearIdTime(Angle az,Angle zn,int id,long int ttSec, long int ttUSec);
   
   BearIdTime(bearStr bs, int id,long int ttSec, long int ttUSec);

   ~BearIdTime();

   void setTimeSec(long int sec);

   void setTimeUSec(long int usec);

   void setTime(long int sec, long int usec);

   long int getTimeSec();
   
   long int getTimeUSec();

   bool overTime(long int sec, long int usec);
   
   bool underTime(long int sec, long int usec);
   
   Decimal getTime();
   
   bool underTime(Decimal sec);
   
   bool overTime(Decimal sec);
   
   string print();
 }; 
 
 class Pose3DIdTime : public Pose3D {
  private:
   long int _ttSec;
   long int _ttUSec;
   int _id;
   
  public:
   Pose3DIdTime(){};

   Pose3DIdTime(Position3D pos,Orientation3D ori,int id,long int ttSec, long int ttUSec);
   
   ~Pose3DIdTime();

   void setTimeSec(long int sec);

   void setTimeUSec(long int usec);

   void setTime(long int sec, long int usec);

   long int getTimeSec();
   
   long int getTimeUSec();
   
   bool overTime(long int sec, long int usec);
   
   bool underTime(long int sec, long int usec);
   
   Decimal getTime();
   
   bool underTime(Decimal sec);
   
   bool overTime(Decimal sec);
   
   int getId();
   
   void setId(int id);
   
   string print();
 };
 
 class InterPoints {
 private:
  vector<Position3D> _xy;
  
// /// \brief Number of points stored
// int _nPoints;
  
 public:
  InterPoints();
  
  InterPoints(Position3D xy);
  
  InterPoints(vector<Position3D> xy);
  
  InterPoints(Decimal x, Decimal y,Decimal z);
  
  ~InterPoints();
  
  int getSize();
  
  InterPoints& operator=(const InterPoints& rhs);
  
  void insertPoint(Position3D xy);
  
  void deletePoint(int pos);
  
  void deleteLastPoint();
  
  void insertPoints(vector<Position3D> xy);
  
  void insertPoint(Position3D ip,int pos);
  
  Position3D getPoint(int pos);
  
  Position3D getLastPoint();
  
  string print();
 };
 
 class Triangle : public MIPObject {
 private:
  
 PosId3D    *_firstVertex;
 PosId3D    *_secondVertex;
 PosId3D    *_thirdVertex;
 InterPoints  *_interPoints3;
 InterPoints  *_interPoints2;
 bearStr    _oriTwo;
 bearStr    _oriThree;
 bearStr    _oriTwoOne;
 bearStr    _oriTwoThree;
 bearStr    _oriThreeOne;
 bearStr    _oriThreeTwo;
    bearStr _ori2;
    bearStr _ori3;
 
 private:
  string printSetOriResult();
 public:

  Triangle();

  Triangle(Position3D xy1, int id1, Position3D xy2, int id2, Position3D xy3, int id3);
  
  ~Triangle();
  
  vector<PosId3D> getVertex();
  
  PosId3D* getFirstVertex();
  
  PosId3D* getSecondVertex();
  
  PosId3D* getThirdVertex();
  
  PosId3D* getVertexByPos(int pos);
  
  PosId3D* getVertexByID(int id);
  
  void setMyIdVertex(int myId, Decimal &aSide, Decimal &bSide, Decimal &cSide);
  
  bool isAVertex(Position3D pos, Decimal thr);
  
  void getIds(vector<int> &ris);
  
  vector<int> getIds();
  
  InterPoints* getInter3();
  
  InterPoints* setInter3();
  
  Position3D getInter3At(int i);
  
  int getInter3Size();
  
  InterPoints* getInter2();
  
  int getInter2Size();
  
  void deleteDupPoint2(int pos);
  
  void deleteDupPoint3(int pos);
  
  void insertPoints3(vector<Position3D> iPoints);
  
  void insertPoint3(Position3D iPoint);
  
  void insertPoints2(vector<Position3D> iPoints);
  
  void insertPoint2(Position3D iPoint);
  
  bool checkId(int myId);
  
  bool sameIds(Triangle* tria);
  
  bool setOri(bearStr oriTwo, bearStr oriThree, bearStr oriTwoF, bearStr oriTwoS, bearStr oriThreeF, bearStr oriThreeS, Decimal projToll);
  
  bool setOriOpt(bearStr oriTwo, bearStr oriThree, bearStr oriTwoF, bearStr oriTwoS, bearStr oriThreeF, bearStr oriThreeS, Decimal projToll,Decimal angToll);
  
  bool setOriSing(bearStr oriTwo, bearStr oriThree, bearStr oriTwoF, bearStr oriTwoS, bearStr oriThreeF, bearStr oriThreeS, Decimal projToll,Decimal angToll);
  
  bearStr getOriTwo();
  
  bearStr getOriThree();
  
  bearStr getOriTwoThree();
  
  bearStr getOriTwoOne();

  bearStr getOriThreeOne();
  
  bearStr getOriThreeTwo();
  
  Angle getOriByPos(int pos);
  
  string print();
  
  Triangle& operator=(Triangle &A);
  
  bool operator==(Triangle &A) const;
  
  string getObjectName() const{
   return "Triangle";
  }
  /*Triangle & operator=(const Triangle& rhs) {
  if (this != &rhs) { // Not necessary in this case but it is useful to don't forget it
   _firstVertex = rhs._firstVertex;
   _secondVertex = rhs._secondVertex;
   _thirdVertex = rhs._thirdVertex;
   _interPoints2 = rhs._interPoints2;
   _interPoints3 = rhs._interPoints3;
  }
  return (*this);
  }*/
 };
 
 class PolFusion {
 private:
  vector<Triangle*> _triangles;
  vector<PosId3D>  _vertex;
  vector<Angle>   _ori;
  InterPoints    _intThree;
  vector<Triangle*> _triangleQueue;
  vector<MIPMatrix> _rotoTrasl;
  vector<Decimal>  _scaleF;
 
 public:
  PolFusion();
  
  PolFusion(Triangle* tria);
  
  ~PolFusion();
  
  Triangle* getTriangle(int pos);
  
  void setTriangle(int pos,Triangle* tria);
  
  void deleteTriangle(int pos);
  
  void deleteLastTriangle();
  
  void deleteBifConflicts(int pos,int posAt, int idNew);
  
  int getSize();
  
  void insertVertex(PosId3D vertex);
  
  PosId3D getVertex(int pos);
  
  void setVertex(int pos,PosId3D vertex);
  
  void setQueue(Triangle* queue);
  
  int queueSize();
  
  void deleteQueueTriangle(int pos);
  
  void deleteQueueLastTriangle();
  
  void setScale(int i, Decimal scale);
  
  void setScale(Decimal scale);
  
  Decimal getScale(int i);
  
  void deleteScaleF(int pos);
  
  void deleteLastScaleF();
  
  void setRotoTrasl(int pos, MIPMatrix rt);
  
  void deleteRotoTrasl(int pos);
  
  void deleteLastRotoTrasl();
  
  void insertOri(Decimal ori);
  
  Decimal getOri(int pos);
  
  void setOri(int pos,Angle ori);
  
  void deleteOri(int pos);
  
  void deleteLastOri();
  
  void insertTriangle(Triangle *tria, int myId, int id1, int id2);
  
  PosId3D* searchVertex(int id);
  
  PosId3D* searchVertex(int id,int &posAt);
  
  void deleteVertex(int pos);
  
  void deleteLastVertex();
  
  int searchBif(PosId3D vertex,int &posAt,Decimal thr);
  
  void insertPoints(Triangle* tria);
  
  int getNInThree(int pos);
  
  Position3D getIntThree(int pos);
  
  Position3D getLastIntThree();
  
  void deleteIntThree(int pos);
  
  void deleteLastIntThree();
  
  void insertPointIntThree(Position3D ip,int pos);
  
  bool expandPol(int myId, vector<PolFusion*> actualSol, vector<PolFusion*> &bifPolFusion,Decimal linThr);
  
  bool checkBif(PosId3D vertex,Decimal thr,int myId);
  
  string print(int opt=0);
  
  PolFusion& operator=(PolFusion &A);
  
 };
 
 class ProbPolFusion : public MIPObject{
  private:
   // Pointers to the triangles that compose the polimorph
   vector<Triangle*>  _triangles;
   // List of vertex
   vector<PosId3D>   _vertex;
   // Orientation of the robots 
   vector<Angle>    _ori;
   // Intersection by three
   InterPoints     _intThree;
   // List of triangle candidates to enter in the polimorph
   vector<Triangle*>  _triangleQueue;
   // Rototranslation
   vector<MIPMatrix>  _rotoTrasl;
   // Scale factor
   vector<Decimal>   _scaleF;
   // Rating based on the number of verteces 
   int         _ratingVertex;
   // Rating based on the number of triple intersections
   int         _ratingInThree;
   // Number of intersection by four ordered by the triangle index inserted 
   vector<int>     _numIntFour;
   // Kind of expansion method used
   int         _expandMethod;
  
  public:
   ProbPolFusion();
   
   ProbPolFusion(Triangle* tria, int expMth);
   
   ~ProbPolFusion();
   
   Triangle* getTriangle(int pos);
   
   void setTriangle(int pos,Triangle* tria);
   
   void deleteTriangle(int pos);
   
   void deleteLastTriangle();
   
   void deleteBifConflicts(int pos,int posAt, int idNew);
   
   void deleteBifConflictsOpt(int pos, int idNew);
   
   int getSize();
   
   int getQueueSize();
   
   void getIds(vector<int> &ids);
   
   void insertVertex(PosId3D vertex);
   
   PosId3D getVertex(int pos);
   
   void setVertex(int pos,PosId3D vertex);
   
   void setQueue(Triangle* queue);
   
   int queueSize();
   
   void deleteQueueTriangle(int pos);
   
   void deleteQueueLastTriangle();
   
   void setScale(int i, Decimal scale);
   
   void setScale(Decimal scale);
   
   Decimal getScale(int i);
   
   void deleteScaleF(int pos);
   
   void deleteLastScaleF();
   
   void setRotoTrasl(int pos, MIPMatrix rt);
   
   MIPMatrix getRotoTrasl(int pos);
   
   void deleteRotoTrasl(int pos);
   
   void deleteLastRotoTrasl();
   
   void insertOri(Decimal ori);
   
   Decimal getOri(int pos);
   
   void setOri(int pos,Angle ori);
   
   void deleteOri(int pos);
   
   void deleteLastOri();
   
   void insertTriangle(Triangle *tria, int myId, int id1, int id2);
   
   PosId3D* searchVertex(int id);
   
   PosId3D* searchVertex(int id,int &posAt);
   
   void deleteVertex(int pos);
   
   void deleteLastVertex();
   
   int searchBif(PosId3D vertex,int &posAt,Decimal thr);
   
   void insertPoints(Triangle* tria, int &numInt4);
   
   void insertPointsNoIn(Triangle* tria, int &numInt4, MIPMatrix* &matrixPointer, Decimal actualScale, vector<Position3D> &intToInsert, int &incrRatInThree);
   
   int getNInThree(int pos);
   
   Position3D getIntThree(int pos);
   
   Position3D getLastIntThree();
   
   void deleteIntThree(int pos);
   
   void deleteLastIntThree();
   
   void insertPointIntThree(Position3D ip,int pos);
   
   bool expandPol(int myId, vector<ProbPolFusion*> actualSol, vector<ProbPolFusion*> &bifProbPolFusion,Decimal linThr, map <int, vector<Pose3D> > *likelihood, vector <int> activeLikelihoods);
   
   bool expandPolOpt(int myId, vector<ProbPolFusion*> actualSol, vector<ProbPolFusion*> &bifProbPolFusion,Decimal linThr, map <int, vector<Pose3D> > *likelihood, vector <int> activeLikelihoods,int &maxBifSize);
   
   bool expandPolNoIn(int myId, vector<ProbPolFusion*> actualSol, vector<ProbPolFusion*> &bifProbPolFusion,Decimal linThr, map <int, vector<Pose3D> > *likelihood, vector <int> activeLikelihoods,int &maxBifSize);
   
   bool checkBif(PosId3D vertex,Decimal thr,int myId);
   
   string print(int opt=0);
   
   ProbPolFusion& operator=(ProbPolFusion &A);
   
   Decimal computeProbability(Pose3D measure, int id, map <int, vector<Pose3D> > * &likelihood);
   
   void setRatingVertex(int rating);
   
   int getRatingVertex();
   
   void growRatingVertex();
   
   void decreaseRatingVertex();
   
   void setRatingInThree(int rating);
   
   int getRatingInThree();
   
   void growRatingInThree();
   
   void decreaseRatingInThree();
   
   void setIntFourNum(int pos, int val);
   
   int getIntFourNum(int pos);
   
   int getLastIntFourNum();
   
   void deleteLastIntFourNum();
   
   void selectScale(Decimal &bestScale, Decimal &bestProb, map <int, vector<Pose3D> > * &likelihood, vector <int> &activeLikelihoods,vector<int> &idsV);
   
   void setExpandMethod(int expMth);
   
   string getObjectName() const{
    return "ProbPolFusion";
   }
 };
} // End of namespace

#endif