MultiRegCam.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 __MULTI_REG_CAM_H_
#define __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 "MultiRegCamStr.h"


using namespace std;
using namespace MipBaselib;

namespace MipAlgorithms{

    class MultiRegCamPar {
    private:
        Decimal _linearTollerance;
        Decimal _angularTollerance;
        Decimal _projTollerance;

    public:
        MultiRegCamPar();

        MultiRegCamPar(Decimal lt, Decimal at, Decimal pt);

        MultiRegCamPar(const MultiRegCamPar &A);

        MultiRegCamPar& operator=(MultiRegCamPar &A);


        Decimal getLinearTollerance();


        Decimal getAngularTollerance();

        Decimal getProjTollerance();
    };
    
    class MultiRegCamSol {
    private:
        vector<PolFusion*> _sols;

    public:

        MultiRegCamSol();


        MultiRegCamSol(PolFusion *polFusion);

        ~MultiRegCamSol();


        void insertPol(PolFusion *polFusion);


        int getSize();


        PolFusion* getPolFusion(int pos);

        vector<PolFusion*> getActualSol();

        MultiRegCamSol& operator=(MultiRegCamSol &A);

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

        string printSolutions(vector<Pose> *ris);


        string print(int opt = 0);

    };

    class MultiRegCam {
    public:
        MultiRegCamPar _par;

        MultiRegCam(MultiRegCamPar par);

        ~MultiRegCam();

        void initExample(vector<AngleId> &input);

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

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

        void initTriangles(vector<AngleId> input, MultiRegCamSol *mrcsols, int myId);

        // \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,MultiRegCamSol *mrcsols);

        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);

        fstream _logMultiRegCamFile;
        //void searchId(int id, MultiRegCamSol *mrcsols,vector<BearId> ris);

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

    };

    //Triangle tria1;

}

#endif

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

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

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

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

                        ~PolFusion(){

                        }

                        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 MultiRegCamSol:
/*class MultiRegCamSol{ private:
                            vector<PolFusion> _sols;

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

                            MultiRegCamSol(PolFusion polFusion){
                                _sols.clear();
                                _sols.push_back(polFusion);
                            }

                            ~MultiRegCamSol(){

                            }

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

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

                            MultiRegCamSol operator= (MultiRegCamSol &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();
                            }

   };
 */