ProbMultiRegCam3D.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_REG3D_H_
#define __PROB_MULTI_REG3D_H_

#define BLACK "\e[2;30m"
#define RED "\e[0;31m"
#define GREEN "\e[0;32m"
#define YELLOW "\e[0;33m"
#define BLUE "\e[0;34m"
#define MAGENTA "\e[0;35m"
#define CYAN "\e[0;36m"
#define WHITE "\e[0;37m"
#define STOPCOLOR "\e[m"

#include <MIPMatrix.h>
#include "time.h"
#include <fstream>
#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <iostream>
#include <R2.h>
#include <S1.h>
#include <SE2.h>
#include "ProbMultiRegCam3DStr.h"
 
using namespace std;
using namespace MipBaselib;
 
namespace kybalg{
 class ProbMultiRegCam3DQuadPar {
 private:
  // Linear tollerance
  Decimal     _linearTollerance;
  // Angular tollerance
  Decimal     _angularTollerance;
  // Projection tollerance
  Decimal     _projTollerance;
  // Zenith tollerance
  Decimal     _zenithTollerance;
  // Expansion method
  int       _expandMethod;
  // Id path
  string     _idPath;
  // Bearing-only path
  string     _bearMeasPath;
  // Minimum weight of intersection of a triangle to be accepted
  Decimal     _minIntWeight;
 
 public:
  //ProbMultiRegCam3DQuadPar();

  ProbMultiRegCam3DQuadPar(Decimal lt=0.2, Decimal at=0.013962634+0.05, Decimal pt=0.1,Decimal zt=0.1,int expandMethod=2,Decimal miw=1.0,string idPath="Ids",string bmPath="UAVMeas");

  ProbMultiRegCam3DQuadPar(const ProbMultiRegCam3DQuadPar &A);
  
  ProbMultiRegCam3DQuadPar& operator=(ProbMultiRegCam3DQuadPar &A);
  
  Decimal getLinearTollerance();
  
  Decimal getAngularTollerance();
  
  Decimal getProjTollerance();
  
  Decimal getZenithTollerance();
  
  int getExpandMethod();
  
  string getIdPath();
  
  string getBearMeasPath();
  
  Decimal getMinIntWeight();
 };
 
 class ProbMultiRegCam3DQuadSol: public MIPObject {
 private:
  vector<ProbPolFusion*> _sols;
  
 public:
  ProbMultiRegCam3DQuadSol();
  
  ProbMultiRegCam3DQuadSol(ProbPolFusion *polFusion);
  
  ~ProbMultiRegCam3DQuadSol();
  
  void insertPol(ProbPolFusion *polFusion);
  
  int getSize();
  
  ProbPolFusion* getProbPolFusion(int pos);
  
  vector<ProbPolFusion*> getActualSol();
  
  ProbMultiRegCam3DQuadSol& operator=(ProbMultiRegCam3DQuadSol &A);
  
  int searchId(int id, vector<Pose3D> *ris, vector<MIPMatrix> *measCov, Decimal *rightProb, vector<Decimal> *weights);
  
  string printSolutions(vector<Pose3D> *ris);
  
  string print(int opt = 0);
  
  string getObjectName() const{
   return "ProbMultiRegCam3DQuadSol";
  }
  
 };
  
 class ProbMultiRegCam3DCam : public MIPObject {
 private:
  // Map that links id with actual belief (from motion update blocks of particle filters)
  map <int, vector<Pose3D> > *likelihood;
  // List of ids of activated filters
  vector <int> activeLikelihoods;
  
 public:
  ProbMultiRegCam3DQuadPar _par;
  
  ProbMultiRegCam3DCam(ProbMultiRegCam3DQuadPar par);
  
  ~ProbMultiRegCam3DCam();
  
  void initExample(vector<AngleBearId> &input);
  
  void print(vector<AngleBearId> input, int option);
  
  void print(vector< vector<Decimal> > input);
  
  void initTriangles(vector<AngleBearId> &input, ProbMultiRegCam3DQuadSol *mrcsols, int myId);
  
  vector <vector<Angle> > createAng(vector<vector<Angle> > input);
  
  void evalTria(vector<AngleBearId> &input,int firstV,int firstF,int firstS,int secondV,int secondF,int secondS,int thirdV,int thirdF,int thirdS,Triangle *triaPointer);
  //void evalTria(vector<AngleId> input, Decimal firstV, Decimal firstF, Decimal firstS, Decimal secondV, Decimal secondF, Decimal secondS, Decimal thirdV, Decimal thirdF, Decimal thirdS, Decimal aSide, Decimal bSide, Decimal cSide, Triangle *triaPointer);
  //void evalTria(vector<AngleId> input,Decimal firstV,Decimal secondV,Decimal thridV,Decimal aSide,Decimal bSide,Decimal cSide,Triangle *triaPointer);
  
  int step(int argc, char** argv);
  
  void eraseDup(Triangle* &triaPointer, Decimal thr);
  
  void eraseDup3(Triangle* &triaPointer, Decimal thr);
  
  void eraseDup2(Triangle* &triaPointer, Decimal thr);
  
  bool eraseDupTria(vector<Triangle*> &trias, Triangle* &tria);
  
  void createPol(vector<Triangle*> triaSols, int myId,ProbMultiRegCam3DQuadSol *mrcsols);
  
  void quickSortTria(vector<Triangle*> &triaSols, Triangle* &tria, int myId);
  
  void selectSubSol(vector<Triangle*> &triaSols, Triangle* &tria, int myId);
  
  void printDataIn(AngleBearId* first,int f1,int f2, AngleBearId* second, int s1, int s2, AngleBearId* third, int t1, int t2,int info1,int info2, int info3, int id1, int id2, int id3);
  
  void setLikelihood(vector<int>& ids, map <int, vector<Pose3D> >* lik);
  
  string getObjectName() const {
   return "ProbMultiRegCam3DCam";
  }
  
  // Variable used to print 
  fstream _logProbMultiRegCam3DCamFile;
  fstream _logProbMultiRegCam3DCamOutput;
  //void searchId(int id, ProbMultiRegCam3DQuadSol *mrcsols,vector<BearId> ris);
  
  //string printSolutions(vector<BearId> ris);
 };
  
}//end of namespace kybalg

#endif