#ifndef LOCALISATIONGRAPH_H
#define LOCALISATIONGRAPH_H
/*********************************************
*
* Cedric Pradalier
* DEA 2000/2001
* INRIA Rhones Alpes
* http://cedric.pradalier.free.fr/index.html
* mail : http://cedric.pradalier.free.fr/mail.html
*
* *******************************************/
#include <stdio.h>
#include <Object.h>
#include "VisibilityPolygon.h"
#include "Vector.h"
#include "Polygon.h"
#include <map.h>
#include <pair.h>
#include <multimap.h>
// Classe représentant un graphe de localisation tel que décrit dans
// mon mémoire de DEA.
// J'ai utilisé un tel graphe pour décrire l'environnement d'un robot équipé
// d'un capteur télémétrique laser à balayage. Avec un tel capteur et des
// amers (landmarks), le robot peut connaître sa position dans l'environnement
// dès qu'il "voit" au moins deux amers.
// Le graphe de localisation permet d'obtenir 3 résultats :
// + Pour un environnement donné (espace de travail et amers), il permet de
// déterminer quelles sont les configurations où la localisation est
// possible.
// + Pour un environnement donné, on peut utiliser ces informations pour
// planifier une trajectoire garantissant des capacités optimales de
// localisation (pour un robot Nomade équipé d'une caméra par exemple).
// + Pour un espace de travail et un nombre d'amers donnés, il permet de mettre
// en place des techniques d'optimisations pour placer les amers de façon
// optimales.
// Au niveau mathématique, l'espace de localisation est un sous ensemble de
// l'espace des configurations (ensemble des positions possibles (x,y,theta)
// d'un robot).
class LocalisationGraph
{
private :
class Node;
typedef map< int, Node*, less<int> > address_mapper;
// Epaisseur des couches en RADIANS
double theta_step;
// Espace de travail utilisé pour la construction
Polygon * workSpace;
// Stockage des couches (class Layer)
Vector * graph;
// Nombre de parties connexes. -1 tant que le calcul n'a pas
// été fait
int ConnexParts;
// Cellule du graphe de localisation : polygone tel que pour
// l'orientation theta du robot, l'ensemble de balises
// visibles soit le même en tout point du polygone et de
// taille maximale.
class Node : public Object
{
public :
// Polygone et balises visibles
VisibilityPolygon * poly;
// Nodes "en contact" avec ce Node
Vector connection;
// Identifiant de la partie connexe a laquelle
// appartient ce Node.
int connId;
// Orientation du robot sur cette cellule.
// redondant avec l'info dans le Layer qui
// contient la cellule car dans un chemin,
// les noeuds sont liés entre eux, sans
// information d'appartenance à une couche.
double theta;
// Données pour Dijkstra
int score;
Node * predecessorInPath;
public :
Node() {poly=NULL;connId=-1;theta=0;}
// Constructeur par lecture dans un fichier.
// Le fichier est SUPPOSE syntaxiquement
// correct. amap permet de stocker la
// correspondance entre identifiants et
// pointeurs
Node(FILE * fp,address_mapper* amap);
Node(VisibilityPolygon * P,double t)
{
poly=P;connId=-1;theta=t;
}
~Node() {delete poly;}
// Manipulation de connId
void setId(int Id) {connId = Id;}
int getId() {return connId;}
// Ecriture dans un fichier (peut etre relu par
// le constructeur ci-dessus). Les adresses des
// Node sont utilisés comme identifiant
void Print(FILE * fp);
// Ecriture dans un fichier : on considère le
// noeud comme élément d'un chemin
void PrintConfiguration(FILE * fp);
// Construction de la partie connexe contenant
// ce Node. Exploration en largeur(?) et
// marquage au fur et a mesure
void exploreAndMark(int mark);
// Utilisation de amap pour mettre à jour les
// connexion dans le graphe. (appelé lorsque
// tous les noeuds du graphe ont été lu)
void updateLinks(address_mapper* amap);
// Triangulation du polygone de visibilité
// Utile pour déterminer si un point appartient
// au polygone...
void buildTriStrips();
// Utilise la triangulation (pre-requise) de
// poly pour déterminer si (x,y) appartient à
// la cellule.
bool contains(double x,double y)
{
return poly->contains(x,y);
}
// Initialise les données score et
// predecessorInPath pour Dijkstra
void resetPath();
TriangleStrip* getTriStrip()
{
return poly->getTriStrip();
}
};
typedef pair<int, Node*> HeapElt;
typedef multimap<int, Node*, less<double> > Heap;
public :
// Types abstraits pour les résultats de la recherche
// de chemins
typedef Node* Configuration;
typedef Vector* Path;
private :
// Classe décrivant une couche du graphe de localisation,
// ensemble des cellules décrivant la même orientation theta
// du robot
class Layer : public Object
{
private :
Vector * nodes;
public :
double theta; // en RADIANS
Layer(double t){theta=t;nodes=new Vector();}
// appel constructeur de Node autant de fois
// que nécessaire
Layer(FILE*fp,address_mapper* amap);
~Layer() {nodes->deleteAll();delete nodes;}
// Affiche tous les Nodes de nodes
void Print(FILE *fp);
// Pour pouvoir trier les couches, si elles
// sont insérées dans le desordre...
virtual double Value() {return theta;}
void addNode(Node * n) {nodes->addElement(n);}
// Construit les connexions internes au Layer
// (liens entre les Nodes connexes)
void buildLinks();
// appel de Node::updateLinks
void updateLinks(address_mapper* amap);
// appel de Node::buildTriStrips
void buildTriStrips();
int size() {return nodes->size();}
// appel de Node::resetPath()
void resetPath();
Node* getNode(int i)
{
return (Node*)(nodes->elementAt(i));
}
// Renvoie la cellule contenant le point (x,y).
// Null si aucune cellule ne le contient.
Configuration getConfiguration(double x,
double y);
};
// Equivalent à buildLinks mais entre les couches indexNew et
// indexRef. Voir mon mémoire pour plus de détail.
void computeGraphInterLayer(int indexNew,int indexRef);
// Etant donné une liste de balises, partitionne l'espace de
// travail en fonction des balises visibles
Vector * partitionne(Vector *balisesList,Polygon * workSpace);
// Agit sur le résultat de partitionne : supprime les parties
// de l'espace de travail où il est impossible de se localiser.
// Supprime aussi les parties trop petites
void filterPartition(Vector * partition,Vector * usable);
public :
// tstep : epaisseur des couches en radian
// ws : espace de travail décrit par le graphe
LocalisationGraph(Polygon* ws,double tstep);
// Construction par lecture dans fp.
LocalisationGraph(FILE *fp);
~LocalisationGraph();
// Construit une nouvelle couche de l'espace de localisation
void addLayer(Vector *balisesList,
double theta,Polygon* workSpace);
// Construit l'ensemble des positions de l'espace de travail où
// il est possible de se localiser quelle que soit
// l'orientation.
void buildSecureArea(Polygon * workSpace,char * filename);
// Construit les liens de connexité entre les cellules du
// graphe de localisation
void buildGraphLinks();
// Marque les parties connexes du graphe. Renvoie le nb de
// partie connexe trouvée et met a jour ConnexParts
int markConnexPart();
// Renvoie le nb de parties connexes dans le graphe. -1 si le
// calcul n'a pas encore été fait.
int getNbConnexParts() {return ConnexParts;}
// Calcule le volume de l'espace de localisation
double computeVolume();
// Sauvegarde le graphe dans fp (peut etre volumineux)
void Print(FILE * fp);
// Triangulation de toutes les cellules du graphe
void buildTriStrips();
// Recherche la cellule qui contient (x,y,theta)
// theta DOIT etre en radian entre 0 et 2Pi
// Renvoie NULL si aucune cellule ne convient.
Configuration getConfiguration(double x,double y,double theta);
// Libere la mémoire attribuée à une configuration par
// getConfiguration.
void cleanConfiguration(Configuration c);
// Initialise les données des Nodes, pour Dijkstra
void resetPath();
// Trouve un chemin (suite de configuration) entre init et end
Path findPath(Configuration init,Configuration end);
// Renvoie la longueur du chemin
unsigned int getPathSize(Path path) {return path->size();}
// Renvoie la i-ieme configuration du chemin trouvé par
// findPath.
Configuration getStep(Path path,unsigned int i)
{
return (Configuration)(path->elementAt(i));
}
// Affiche une configuration
void printConfiguration(Configuration conf,FILE *fp)
{
conf->PrintConfiguration(fp);
}
// Affiche un chemin, resultat de findPath.
void printPath(FILE *fp,Path path);
// Libère les ressources attribuées par findPath.
void cleanPath(Path path);
// Ecrit le graphe et l'environnement sous forme de
// PolyData pour Vtk. Ces fichiers peuvent ensuite
// être lus par polyPlot.
// Le graphe est écrit dans mainFile, l'environnement dans
// wsFile. xScale,yScale et zScale sont des facteurs
// d'échelle multiplicatif (généralement 0.3 0.3 et 10)
void VtkPrint(FILE * mainFile,FILE * wsFile,
double xScale,double yScale,double zScale);
};
#endif // LOCALISATIONGRAPH_H