ProbMultiRegStereoCam3D.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_STEREO_CAM_H_
#define __PROB_MULTI_REG3D_STEREO_CAM_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 "ProbMultiRegStereoCam3DStr.h"
#include "ProbMultiRegCam3D.h"
 
using namespace std;
using namespace MipBaselib;
 
namespace kybalg{
 
 class probMultiRegStereoCam3DQuadSol: public MIPObject {
 private:
  vector<probPolFusionDist*> _sols;
  
 public:
  probMultiRegStereoCam3DQuadSol();
  
  probMultiRegStereoCam3DQuadSol(probPolFusionDist *polFusion);
  
  ~probMultiRegStereoCam3DQuadSol();
  
  void insertPol(probPolFusionDist *polFusion);
  
  int getSize();
  
  probPolFusionDist* getProbPolFusion(int pos);
  
  vector<probPolFusionDist*> getActualSol();
  
  probMultiRegStereoCam3DQuadSol& operator=(probMultiRegStereoCam3DQuadSol &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 "probMultiRegStereoCam3DQuadSol";
  }
  
 };
 
 class probMultiRegStereoCam3DCam : 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;
  
  probMultiRegStereoCam3DCam(ProbMultiRegCam3DQuadPar par);
  
  ~probMultiRegStereoCam3DCam();
  
  void initExample(vector<bearDistId> &input);
  
  void print(vector<bearDistId> input, int option);
  
  void print(vector< vector<Decimal> > input);
  
  void initTriangles(vector<bearDistId> &input, probMultiRegStereoCam3DQuadSol *mrcsols, int myId);
  
  vector <vector<Angle> > createAng(vector<vector<Angle> > input);
  
  void evalTria(vector<bearDistId> &input,int firstV,int firstF,int firstS,int secondV,int secondF,int secondS,int thirdV,int thirdF,int thirdS,triangleAssObs *triaPointer);
  //void evalTria(vector<bearDistId> 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<bearDistId> input,Decimal firstV,Decimal secondV,Decimal thridV,Decimal aSide,Decimal bSide,Decimal cSide,Triangle *triaPointer);
  
  int step(int argc, char** argv);
  
  void eraseDup(triangleAssObs* triaPointer, Decimal thr);
  
  void eraseDup3(triangleAssObs* triaPointer, Decimal thr);
  
  void eraseDup2(triangleAssObs* triaPointer, Decimal thr);
  
  bool eraseDupTria(vector<triangleAssObs*> &trias, triangleAssObs* &tria);
  
  void createPol(vector<triangleAssObs*> triaSols, int myId,probMultiRegStereoCam3DQuadSol *mrcsols);
  
  void quickSortTria(vector<triangleAssObs*> &triaSols, triangleAssObs* &tria, int myId);
  
  void selectSubSol(vector<triangleAssObs*> &triaSols, triangleAssObs* &tria, int myId);
  
  void printDataIn(bearDistId* first,int f1,int f2, bearDistId* second, int s1, int s2, bearDistId* 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 "probMultiRegStereoCam3DCam";
  }
  
  // Variable used to print 
  fstream _logprobMultiRegStereoCam3DCamFile;
  fstream _logprobMultiRegStereoCam3DCamOutput;
 };
}//end of namespace kybalg

#endif