ProbMultiRegStereoCam3DStr.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_STEREO_CAM_STR3D_H
#define __PROB_MULTI_REG_STEREO_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>
#include <ProbMultiRegCam3DStr.h>
 
using namespace MipBaselib;
 
namespace kybalg{
 class bearDistStr{
  public:
   Angle azimuth;
   Angle zenith;
   Decimal dist;
   
   bearDistStr();
   
   bearDistStr(Angle az,Angle zn, Decimal d);
   
   bearDistStr(Decimal az,Decimal zn,Decimal d);
   
   ~bearDistStr();
   
   bearDistStr& operator= (const bearDistStr &A);
   
   bool operator==(const bearDistStr &other) const;
   
   string print();
 };
 
 typedef ObjId<bearDistStr> bearDistId;
 
 class bearDistIdTime : public bearDistId {
  private:
   long int _ttSec;
   long int _ttUSec;
  public:
   bearDistIdTime(){};

   bearDistIdTime(Angle az,Angle zn, Decimal d,int id,long int ttSec, long int ttUSec);
   
   bearDistIdTime(bearDistStr bs, int id,long int ttSec, long int ttUSec);

   ~bearDistIdTime();

   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 triangleAssObs : public MIPObject {
 private:
  
 PosId3D    *_firstVertex;
 PosId3D    *_secondVertex;
 PosId3D    *_thirdVertex;
 InterPoints  *_interPoints3;
 InterPoints  *_interPoints2;
 bearDistStr  _oriTwo;
 bearDistStr  _oriThree;
 bearDistStr  _oriTwoOne;
 bearDistStr  _oriTwoThree;
 bearDistStr  _oriThreeOne;
 bearDistStr  _oriThreeTwo;
  bearDistStr _ori2;
  bearDistStr _ori3;
 
 private:
  string printSetOriResult();
 public:

  triangleAssObs();

  triangleAssObs(Position3D xy1, int id1, Position3D xy2, int id2, Position3D xy3, int id3);
  
  ~triangleAssObs();
  
  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(triangleAssObs* tria);
  
  bool setOri(bearDistStr oriTwo, bearDistStr oriThree, bearDistStr oriTwoF, bearDistStr oriTwoS, bearDistStr oriThreeF, bearDistStr oriThreeS, Decimal projToll);
  
  bool setOriOpt(bearDistStr oriTwo, bearDistStr oriThree, bearDistStr oriTwoF, bearDistStr oriTwoS, bearDistStr oriThreeF, bearDistStr oriThreeS, Decimal projToll,Decimal angToll);
  
  bool setOriSing(bearDistStr oriTwo, bearDistStr oriThree, bearDistStr oriTwoF, bearDistStr oriTwoS, bearDistStr oriThreeF, bearDistStr oriThreeS, Decimal projToll,Decimal angToll);
  
  bearDistStr getOriTwo();
  
  bearDistStr getOriThree();
  
  bearDistStr getOriTwoThree();
  
  bearDistStr getOriTwoOne();

  bearDistStr getOriThreeOne();
  
  bearDistStr getOriThreeTwo();
  
  Angle getOriByPos(int pos);
  
  string print();
  
  triangleAssObs& operator=(triangleAssObs &A);
  
  bool operator==(triangleAssObs &A) const;
  
  string getObjectName() const{
   return "triangleAssObs";
  }
 };
 
 class probPolFusionDist : public MIPObject{
  private:
   // Pointers to the triangles that compose the polimorph
   vector<triangleAssObs*>  _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<triangleAssObs*>  _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;
   // Scale factor of the entire formation
   Decimal          _scaleFactForm;
   // Scale factor of each triangle wrt the distance infos
   vector<Decimal>      _scaleFTriangles;
   
  public:
   probPolFusionDist();
   
   probPolFusionDist(triangleAssObs* tria, int expMth);
   
   ~probPolFusionDist();
   
   triangleAssObs* getTriangle(int pos);
   
   void setTriangle(int pos,triangleAssObs* tria);
   
   void deleteTriangle(int pos);
   
   void deleteLastTriangle();
   
   void deleteScaleFTriangles(int pos);
   
   void deleteLastScaleFTriangles();
   
   void setScaleFTriangles(int pos,Decimal &scale);
   
   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(triangleAssObs* 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(triangleAssObs *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(triangleAssObs* tria, int &numInt4);
   
   void insertPointsNoIn(triangleAssObs* 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 expandPolNoIn(int myId, vector<probPolFusionDist*> actualSol, vector<probPolFusionDist*> &bifprobPolFusionDist,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);
   
   probPolFusionDist& operator=(probPolFusionDist &A);
   
   Decimal computeProbability(Pose3D measure, int id, map <int, vector<Pose3D> > * &likelihood);
   
   Decimal computeProbabilityScale(Position3D &measure, Decimal measureYaw,  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 computeProbFixedScale(Decimal &bestProb, map <int, vector<Pose3D> > * &likelihood, vector <int> &activeLikelihoods,vector<int> &idsV);
   
   void setExpandMethod(int expMth);
   
   string getObjectName() const{
    return "probPolFusionDist";
   }
   
   Decimal getScaleFormation();
   
   void updateScaleFactForm(Decimal &d);
   
   void deleteScaleFactForm(Decimal &d);
 };
 
} // End of namespace

#endif