ProbMultiRegCam.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 __PROB_MULTI_REG_CAM_H_
#define __PROB_MULTI_REG_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 <map>
#include "ProbMultiRegCamStr.h"


using namespace std;
using namespace MipBaselib;

namespace MipAlgorithms{

    class ProbMultiRegCamPar {
    private:
        Decimal _linearTollerance;
        Decimal _angularTollerance;
        Decimal _projTollerance;
        int     _expandMethod;

    public:
        ProbMultiRegCamPar();

        ProbMultiRegCamPar(Decimal lt, Decimal at, Decimal pt,int expandMethod);

        ProbMultiRegCamPar(const ProbMultiRegCamPar &A);

        ProbMultiRegCamPar& operator=(ProbMultiRegCamPar &A);


        Decimal getLinearTollerance();


        Decimal getAngularTollerance();

        Decimal getProjTollerance();

        int getExpandMethod();
    };
    
    class ProbMultiRegCamSol {
    private:
        vector<ProbPolFusion*> _sols;

    public:

        ProbMultiRegCamSol();


        ProbMultiRegCamSol(ProbPolFusion *polFusion);

        ~ProbMultiRegCamSol();


        void insertPol(ProbPolFusion *polFusion);


        int getSize();


        ProbPolFusion* getProbPolFusion(int pos);

        vector<ProbPolFusion*> getActualSol();

        ProbMultiRegCamSol& operator=(ProbMultiRegCamSol &A);

        int searchId(int id, vector<Pose> *ris, vector<MIPMatrix> *measCov, Decimal *rightProb, vector<Decimal> *weights);

        string printSolutions(vector<Pose> *ris);


        string print(int opt = 0);

    };

    class ProbMultiRegCam {
    private:
        map <int, vector<Pose> > *likelihood;
 vector <int> activeLikelihoods;

  public:
        ProbMultiRegCamPar _par;

        ProbMultiRegCam(ProbMultiRegCamPar par);

        ~ProbMultiRegCam();

        void initExample(vector<AngleId> &input);

        void print(vector<AngleId> input, int option);

        void print(vector< vector<Decimal> > input);

        void initTriangles(vector<AngleId> &input, ProbMultiRegCamSol *mrcsols, int myId, Pose &myPose);

        // \brief Find the direction seen by a robot
        vector <vector<Angle> > createAng(vector<vector<Angle> > input);

        vector <vector<Decimal> > findFormTot(vector<vector<Angle> > input);

        void evalTria(vector<AngleId> input,int firstV,int firstF,int firstS,int secondV,int secondF,int secondS,int thirdV,int thirdF,int thirdS,Decimal aSide,Decimal bSide,Decimal cSide,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);

        //public:     // !TODO It's private (for testing...)

        void eraseDup(Triangle* &triaPointer, Decimal thr);

        void eraseDup3(Triangle* &triaPointer, Decimal thr);

        bool eraseDupTria(vector<Triangle*> &trias, Triangle* &tria);

        void createPol(vector<Triangle*> triaSols, int myId,ProbMultiRegCamSol *mrcsols,Pose &myPose);

        void quickSortTria(vector<Triangle*> &triaSols, Triangle* &tria, int myId);

        void selectSubSol(vector<Triangle*> &triaSols, Triangle* &tria, int myId);

        void printDataIn(AngleId* first,int f1,int f2, AngleId* second, int s1, int s2, AngleId* 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<Pose> >* lik);

        fstream _logProbMultiRegCamFile;
        fstream _logProbMultiRegCamOutput;
        //void searchId(int id, ProbMultiRegCamSol *mrcsols,vector<BearId> ris);

        //string printSolutions(vector<BearId> ris);

    };

    //Triangle tria1;

}

#endif

// Rep for class ProbPolFusion
/*class ProbPolFusion{ private:
                       vector<Triangle> _triangles;
                       TriaMatrix* _polMatrix;

                    public:
                        ProbPolFusion(){
                            _triangles.clear();
                            _polMatrix=(TriaMatrix*)new TriaMatrix(1,1);
                        }

                        ProbPolFusion(Triangle tria){
                            _triangles.clear();
                            _triangles.push_back(tria);
                            _polMatrix=(TriaMatrix*)new TriaMatrix(1,1);
                        }

                        ProbPolFusion(vector<Triangle> tria){
                            _triangles=tria;
                            _polMatrix=(TriaMatrix*)new TriaMatrix(1,1);
                        }

                        ~ProbPolFusion(){

                        }

                        void insertTriangle(Triangle *tria){
                            //_triangles.push_back(tria);
                            int pos = 0;
                            //Decimal scorr = 0;
                            cout << _triangles.size() << endl;

                            while((pos<_triangles.size())&&(_triangles.at(pos).getInter3Size()>=(*tria).getInter3Size()))
                                    pos++;

                                cout << "pos: " << pos << endl;
                            _triangles.insert(_triangles.begin()+pos,*tria);

                        }

                        string print(){
                           stringstream s;
                           for(int i=0;i<_triangles.size();i++)
                                   s << "Triangle[" << i << "]:" << endl << _triangles.at(i).print();

                           return s.str();
                        }

   };
 */

//Rep for the class ProbMultiRegCamSol:
/*class ProbMultiRegCamSol{ private:
                            vector<ProbPolFusion> _sols;

                        public:
                            ProbMultiRegCamSol(){
                                _sols.clear();
                            }

                            ProbMultiRegCamSol(ProbPolFusion polFusion){
                                _sols.clear();
                                _sols.push_back(polFusion);
                            }

                            ~ProbMultiRegCamSol(){

                            }

                            void insertPol(ProbPolFusion polFusion){
                                _sols.push_back(polFusion);
                            }

                            int getSize(){
                                return _sols.size();
                            }

                            ProbMultiRegCamSol operator= (ProbMultiRegCamSol &A){
                                _sols.clear();
                                _sols=A._sols;
                            }

                            string print(){

                               stringstream s;
                               for(int i=0;i<_sols.size();i++)
                                       s << RED << "Solution[" << i << "]:" << STOPCOLOR << endl << _sols.at(i).print();

                               return s.str();
                            }

   };
 */