Implémentation OpenGL

ou bien : Hp→ hl(x, y, θ)∈ R3 ⇐⇒ (r, φ) ∈ R2tel que Hp= ∂r ∂x ∂r ∂y ∂φ ∂x ∂φ ∂y ∂r ∂θ ∂φ ∂θ !

Donc le poids d’importance est donné par :

ω = ¹

p|2πZ|^exp h

−0.5YTLuYⁱ (7.46)

A la fin de cette étape, on obtient pour chaque particule un poids d’importance qui représente l’adéquation entre la particule, sa carte et les observations correspondantes. Si plus d’un amer est apparié le poids résultant (7.47) pour chaque particule est le produit des poids calculés pour chaque amer apparié. (N est le nombre des amers appariés).

ω_total= N Y i=1 ω_i (7.47)

7.4.4 Rééchantillonnage

Admettons que la densité de probabilité postérieure p(s|zt, u_t)qu’on veut représenter est celle des-sinée avec les deux sommets en gras “ Fig.7.18-(a) ”. A l’instant t = 0 la position du robot est inconnue, donc la densité probabiliste est une distribution uniforme. La densité uniforme est échantillonnée pour chaque particule à travers le modèle d’évolution, ce qui donne une nouvelle densité représentée dans la “ Fig.7.18-(b) ”. Puis pour chaque particule de la nouvelle densité on calcule le poids d’importance. Pour que la densité résultante soit similaire à celle qu’on souhaite obtenir il faut que la densité des particules dans n’importe quelle tranche de l’axe du temps soit proportionnel à la probabilité de cette tranche, autrement dit dans les régions où la probabilité est grande, la densité des particules qui leur correspond doit être grande aussi et ainsi de suite. Contrairement au “ Fig.7.18-(b) ” les échantillons sont espacés de manière équidistante. Pour les ajuster de manière appropriée, il s’avère indispensable de les rééchantillonner [146] [147] [148]. Le principe consiste à augmenter le nombre des particules dont le poids est supérieur et de diminuer celles dont le poids est minimum, ou négligeable, tout en gardant constant le nombre total des particules dans le filtre. Le résultat ressemble à la troisième courbe “ Fig.7.18-(c) ” dont les particules pondérées reconstituent bien la densité envisagée.

Après avoir calculé le poids des particules, les particules dont la probabilité est trop faible sont supprimées. En supprimant la particule, on la remplace par une copie de la particule ayant le poids le plus élevé. Enfin, on met à jour la carte de la nouvelle particule.

7.4.5 Implémentation OpenGL

7.4.5.1 Implémentation OpenGL FB1

L’opération de prédiction par le multipass est décrite dans l’algorithme 6. s est un tableau des texture qui stock l’identificateur des deux texture de ping-pong (sold, snew). L’option de ces deux tex-tures est changée à au sein de la boucle par la fonction swap(). glDrawbuffer détermine les textex-tures utilisée pour l’écriture. Les deux fonctions glActiveTexture et glBindTexture déterminent les tex-tures utilisées pour la lecture. N est le nombre des données odométriques acquises dans une itération,

Figure 7.18: La forme de la densité probabiliste après le rééchantillonnage

drawQuad() est une fonction qui déclenche le calcul par la génération d’un rectangle convenable au type de parallélisation qu’on souhaite réaliser. La matrice de covariance initial Pm est calculée à la même manière en utilisant la technique de multipass.

Algorithme 6 : Prediction using multiple rendering pass

attachement[0] = GL_COLOR_ATTACHMENT0; attachement[1] = GL_COLOR_ATTACHMENT1; read = 1, write = 0;

Attach s[0] to attachement[0], and s[1] to attachement[1]; Transfer particles poses to s[1];

glActiveTexture(GL_TEXTURE0);

glBindTexture(GL_TEXTURE_RECTANGLE, u); for m ← 1 to N do

Transfer noisy encoders data to texture u; glDrawBuffer(attachement[write]); glActiveTexture(GL_TEXTURE1); glBindTexture(GL_TEXTURE_RECTANGLE, s[read]); drawQuad(); swap(read,write); end 7.4.5.2 Implémentation OpenGL FB3

L’implémentation parallèle de la procédure de mise à jour est décrite dans l’algorithme 7. Huit textures dont l’identificateur est stockées dans le tableau pingpongTexID sont attachées au frame buffer FBO. On détermine ensuite les textures à lecture-seule inchangées LdmkTexID qui vont stocker les paramètres des amers appariés. La première boucle de l’algorithme transfère les paramètres initiale de la distribution ^µ^m,t−1_old , Σ^m,t−1_old ^ vers les quatre textures déjà attachées au FBO. La deuxième boucle est la boucle principale qui calcule la nouvelle distribution probabiliste en utilisant le multipass, avec N est le nombre des amers appariés. On transfère tous à chaque itération de la boucle de ’For’ les paramètres des amers appariés vers les textures dont l’identificateur est stocké dans le tableau LdmkTexID. La fonction glDrawBuffers détermine les textures à écriture seule qui vont stocker les résultats de calcul d’un pass µm,t−1

new , Σm,t−1 new

. La boucle suivante détermine les textures à lecture seule qui vont stocker l’ancien paramètres de la distribution^µ^m,t−1_old , Σ^m,t−1_old . Dans le prochain pass , on échange l’option des huit textures pour une nouvelle opération de mise à jour. Ceci est répété

7.4 Annexe D 178 jusqu’à la construction complète de la nouvelle distribution probabiliste.

Algorithme 7 : GLSL Gaussian Construction

Attach eight ping-pong textures pingpongTexID[0..7] to FBO; Set the four readable textures LdmkTexID[0..3];

for i ← 0 to 3 do

Transfer the initial particles Gaussian state µ^m,t−1_old , Σ^m,t−1_old to textures pingpongTexID[i];

end

for n ← 1 to N do

Transfer the n landmark state to textures LdmkTexID; glDrawBuffers(4,pingpongTexID);

for i ← 0 to 3 do

sets readable textures in pingpongTexID; end

drawQuad(); swap(); end

7.4.5.3 Implémentation OpenGL FB4

L’algorithme 8 décrit la procédure de parallélisation du bloc d’estimation FB4 sur le GPU. Chaque itération de la boucle ’For’ corrige les paramètre d’un seul amer de la carte pour chaque particule en parallèle. Si plusieurs amer sont appariés, plusieurs itération seront nécessaire pour les corriger. Les résultats (paramètres mis à jour) sont transférer vers le CPU afin de permettre leur classification dans l’arbre binaire.

Algorithme 8 : GLSL estimation

for n = 1 to N do

Compute in parallel for each particle the updated state of the n landmarks : — Transfer the matched landmark state

(u, v, x0, y0, ρ, θ, ϕ, Ct) to four read-only

input textures;

— Draw a filled rectangle to trigger the computation

— Download the corrected landmark state from four write-only output texutres to CPU end

7.4.5.4 Implémentation OpenGL FB5

Pour exemplifier, la structure interne d’un shader de fragment, l’algorithme 9 décrit le source code du shader du fragment utilisé pour l’initialisation par inverse de profondeur.

sampler2Drect est un pointeur spécifique à l’unité texture activée. Donc, partPose point ver la texture qui stocke la position des particules. La première ligne du code récupère la position des particules dans la texture et les stocke dans un vecteur de quatre dimension Pose_Particle . Les deux lignes suivantes, calcule respectivement, la position de la camera et la position de l’amer dans le repère du monde. La position de la camera lors de la première observation (xi, yi), l’azimuthe θi et l’élévation φi sont stocké dans une variable spécifique appelé gl_FragColor.

Algorithme 9 :Fragment shader Inverse depth initialization uniform sampler2DRect partPose;

uniform vec3 trans, uniform vec2 uv, uniform mat3 rot vec3 pos_cam, pos_lmk, vec4 pos_particle;

void main(void) {

pos_particle = texture2DRect(partPose, gl_TexCoord[0].st); pos_came = rot*trans;

pos_lmk = compute_world_Ldmk_pose(uv); gl_FragColor.x = pos_cam.x + pos_particle.x; gl_FragColor.y = pos_cam.y + pos_particle.y;

gl_FragColor.z= - atan ((pos_cam.x - pos_lmk.x)/(pos_cam.z - pos_lmk.z));

gl_FragColor.w = atan (pos_lmk.y/sqrt (pow (pos_lmk.x pos_cam.x,2) + pow (pos_lmk.z -pos_cam.z,2)));

}