diff --git a/web/index.html b/web/index.html index 4818340..7bd9953 100644 --- a/web/index.html +++ b/web/index.html @@ -535,6 +535,83 @@ + +
+
+

07 Pièce longue & recoins — SLAM incrémental temps réel

+

+ Pièce en L non convexe avec niches. Portée sonar limitée à ~5 m. Le ROV ne voit jamais toute la pièce d'un coup. + La carte s'accumule submap par submap. Dans le long couloir, la dérive longitudinale persiste (dégénérescence sonar parallèle). + La fermeture de boucle la corrige brutalement. +

+ +
+ + + + + +
+
+ Temps: + + 0 s +
+ +
+ + +
+
+ +
0.00 m
+
+
+ +
0
+
+
+ +
Démarrage…
+
+
+
Erreur position estimée
+
+ Dead-reckoning: + — +
+
+
+
+
+ SLAM corrigé: + — +
+
+
+
+
+
+
Trajet réel
+
Dead-reckoning
+
SLAM estimé
+
Keyframes
+
Retours sonar
+
+
+
+ +
+

Dégénérescence couloir & fermeture de boucle

+

Dans un couloir droit, les murs parallèles forment une géométrie symétrique selon l'axe longitudinal. + L'ICP ne peut pas distinguer un décalage avant/arrière — il y a une infinité de solutions équivalentes. + La dérive longitudinale persiste même avec scan-matching. La fermeture de boucle (loop closure) résout ce problème: + quand le ROV revient près d'un keyframe ancien, on détecte la correspondance par comparaison de scan, on ajoute une contrainte + dans le pose-graph, et une optimisation globale (g2o / linéarisée) corrige toutes les poses d'un coup.

+
+
+
+ @@ -1386,6 +1463,843 @@ setTimeout(() => { } }, 3000); +// ═══════════════════════════════════════════════════════════════════════════════ +// SECTION 07 — SLAM INCRÉMENTAL — pièce en L, boucle, dégénérescence couloir +// ═══════════════════════════════════════════════════════════════════════════════ +// TOUT est préfixé slam_ pour éviter toute collision avec la démo existante. + +(function() { +'use strict'; + +// ─── GÉOMÉTRIE: pièce en L avec niches ─────────────────────────────────────── +// Coordonnées en mètres. Sens trigonométrique. Origine arbitraire (coin haut-gauche). +// La pièce est un L: +// ┌─────────────────────────────┐ +// │ couloir long 18m × 3.5m │ +// │ ┌──────────┤ niche haute +// │ │ │ +// │ │ salle │ +// │ niche basse │ carrée │ +// └──────────────────┘──────────┘ +// +// On définit le polygone dans le sens ANTI-HORAIRE (intérieur à gauche du bord): +// Les segments seront parcourus dans l'ordre pour le raycast. + +function slam_buildRoomPolygon() { + // Pièce en L avec 2 niches + // Couloir bas: x=0..18, y=0..3.5 + // Salle carrée à droite: x=12..18, y=3.5..10 + // Niche haute (dans la salle): x=15..18, y=8..10 (incluse dans salle) + // Niche basse (dans couloir): x=0..3, y=-2.5..0 + const pts = [ + // coin bas-gauche du couloir + { x: 0, y: 0 }, + // niche basse à gauche + { x: 0, y: -2.5 }, + { x: 3, y: -2.5 }, + { x: 3, y: 0 }, + // mur bas du couloir vers la droite + { x: 12, y: 0 }, + // descente vers salle basse + { x: 12, y: -1.5 }, + { x: 18, y: -1.5 }, + // mur droit de la salle (bas) + { x: 18, y: 10 }, + // mur haut de la salle + { x: 12, y: 10 }, + // retour vers couloir (mur haut de la salle ↔ couloir) + { x: 12, y: 3.5 }, + // mur haut du couloir long + { x: 0, y: 3.5 }, + // retour au départ + { x: 0, y: 0 }, + ]; + return pts; +} + +const slam_POLY = slam_buildRoomPolygon(); + +// Segments (paires consécutives) +function slam_buildSegments(poly) { + const segs = []; + for (let i = 0; i < poly.length - 1; i++) { + segs.push({ ax: poly[i].x, ay: poly[i].y, bx: poly[i+1].x, by: poly[i+1].y }); + } + return segs; +} +const slam_SEGS = slam_buildSegments(slam_POLY); + +// ─── RAYCAST SUR LE POLYGONE ───────────────────────────────────────────────── +function slam_raySegIntersect(ox, oy, dx, dy, ax, ay, bx, by) { + const ex = bx - ax, ey = by - ay; + const cross = dx * ey - dy * ex; + if (Math.abs(cross) < 1e-10) return null; + const t = ((ax - ox) * ey - (ay - oy) * ex) / cross; + const s = ((ax - ox) * dy - (ay - oy) * dx) / cross; + if (t >= 0 && s >= 0 && s <= 1) return t; + return null; +} + +function slam_castRay(ox, oy, angle, maxDist) { + const dx = Math.cos(angle), dy = Math.sin(angle); + let minT = maxDist; + let hit = null; + for (const seg of slam_SEGS) { + const t = slam_raySegIntersect(ox, oy, dx, dy, seg.ax, seg.ay, seg.bx, seg.by); + if (t !== null && t < minT) { + minT = t; + hit = { x: ox + dx * t, y: oy + dy * t, dist: t }; + } + } + return hit; +} + +const slam_MAX_RANGE = 5.5; // portée sonar en mètres +const slam_N_RAYS = 90; // rayons par sweep + +// ─── POINT-IN-POLYGON (pour valider les positions ROV) ─────────────────────── +function slam_pointInPoly(px, py, poly) { + let inside = false; + for (let i = 0, j = poly.length - 2; i < poly.length - 1; j = i++) { + const xi = poly[i].x, yi = poly[i].y; + const xj = poly[j].x, yj = poly[j].y; + if (((yi > py) !== (yj > py)) && (px < (xj - xi) * (py - yi) / (yj - yi) + xi)) { + inside = !inside; + } + } + return inside; +} + +// ─── TRAJECTOIRE SCRIPTÉE ──────────────────────────────────────────────────── +// Le ROV: +// 1. Part du couloir à gauche (x≈1.5, y≈1.75) +// 2. Traverse tout le couloir vers la droite +// 3. Entre dans la salle, fait le tour +// 4. Explore la niche basse +// 5. REVIENT au point de départ (loop closure) +// Waypoints en (x, y) → interpolés en spline catmull-rom +function slam_buildWaypoints() { + return [ + { x: 1.5, y: 1.75 }, // départ, couloir gauche + { x: 5, y: 1.75 }, // couloir milieu + { x: 9, y: 1.75 }, // couloir droit + { x: 11.5, y: 1.75 }, // approche salle + { x: 13, y: 0.5 }, // descente vers zone basse + { x: 16, y: -0.5 }, // niche basse-droite + { x: 17.5, y: 2.5 }, // salle droite bas + { x: 17.5, y: 7 }, // salle droite haut + { x: 14, y: 9 }, // salle haut + { x: 12.5, y: 5 }, // salle milieu + { x: 14, y: 1.5 }, // retour vers couloir + { x: 11, y: 2.5 }, // couloir droit retour + { x: 7, y: 2.5 }, // couloir milieu retour (légèrement décalé) + { x: 3.5, y: 1.75 }, // couloir gauche retour + { x: 1.5, y: 1.75 }, // RETOUR AU DÉPART → loop closure + ]; +} + +// Interpolation Catmull-Rom +function slam_catmullRom(p0, p1, p2, p3, t) { + const t2 = t * t, t3 = t2 * t; + return { + x: 0.5 * ((2*p1.x) + (-p0.x+p2.x)*t + (2*p0.x-5*p1.x+4*p2.x-p3.x)*t2 + (-p0.x+3*p1.x-3*p2.x+p3.x)*t3), + y: 0.5 * ((2*p1.y) + (-p0.y+p2.y)*t + (2*p0.y-5*p1.y+4*p2.y-p3.y)*t2 + (-p0.y+3*p1.y-3*p2.y+p3.y)*t3), + }; +} + +function slam_buildTrajectory() { + const wps = slam_buildWaypoints(); + const pts = []; + const stepsPerSeg = 80; + + for (let seg = 0; seg < wps.length - 1; seg++) { + const p0 = wps[Math.max(0, seg - 1)]; + const p1 = wps[seg]; + const p2 = wps[seg + 1]; + const p3 = wps[Math.min(wps.length - 1, seg + 2)]; + + for (let s = 0; s < stepsPerSeg; s++) { + const t = s / stepsPerSeg; + const pos = slam_catmullRom(p0, p1, p2, p3, t); + pts.push({ x: pos.x, y: pos.y }); + } + } + // Ajouter le dernier waypoint + pts.push({ ...wps[wps.length - 1] }); + + // Calculer les caps (tangentes) + distance cumulée + let cumDist = 0; + const result = []; + for (let i = 0; i < pts.length; i++) { + const next = pts[Math.min(i + 1, pts.length - 1)]; + const prev = pts[Math.max(i - 1, 0)]; + const heading = Math.atan2(next.y - prev.y, next.x - prev.x); + if (i > 0) { + cumDist += Math.sqrt((pts[i].x - pts[i-1].x)**2 + (pts[i].y - pts[i-1].y)**2); + } + result.push({ x: pts[i].x, y: pts[i].y, heading, cumDist }); + } + return result; +} + +const slam_TRAJ = slam_buildTrajectory(); +const slam_TOTAL_DIST = slam_TRAJ[slam_TRAJ.length - 1].cumDist; + +// ─── ICP 2D (version autonome, pas de dépendance sur l'ICP de la démo 2D) ─── +function slam_icp2D(scanPts, mapPts, maxIter) { + maxIter = maxIter || 10; + if (scanPts.length < 4 || mapPts.length < 4) return { tx: 0, ty: 0, dtheta: 0 }; + + let tx = 0, ty = 0, dtheta = 0; + let pts = scanPts.map(p => ({ x: p.x, y: p.y })); + + for (let iter = 0; iter < maxIter; iter++) { + const pairs = []; + for (const sp of pts) { + let best = null, bestDist = Infinity; + for (const mp of mapPts) { + const d = (sp.x - mp.x)**2 + (sp.y - mp.y)**2; + if (d < bestDist) { bestDist = d; best = mp; } + } + if (bestDist < 1.2) pairs.push({ src: sp, dst: best }); + } + if (pairs.length < 3) break; + + let cx_s = 0, cy_s = 0, cx_d = 0, cy_d = 0; + for (const { src, dst } of pairs) { cx_s += src.x; cy_s += src.y; cx_d += dst.x; cy_d += dst.y; } + cx_s /= pairs.length; cy_s /= pairs.length; cx_d /= pairs.length; cy_d /= pairs.length; + + let H_xx = 0, H_xy = 0, H_yx = 0, H_yy = 0; + for (const { src, dst } of pairs) { + const px = src.x - cx_s, py = src.y - cy_s; + const qx = dst.x - cx_d, qy = dst.y - cy_d; + H_xx += px * qx; H_xy += px * qy; H_yx += py * qx; H_yy += py * qy; + } + const dRot = Math.atan2(H_yx - H_xy, H_xx + H_yy); + const cosR = Math.cos(dRot), sinR = Math.sin(dRot); + const dTx = cx_d - (cosR * cx_s - sinR * cy_s); + const dTy = cy_d - (sinR * cx_s + cosR * cy_s); + + pts = pts.map(p => ({ x: cosR * p.x - sinR * p.y + dTx, y: sinR * p.x + cosR * p.y + dTy })); + const newTx = cosR * tx - sinR * ty + dTx; + const newTy = sinR * tx + cosR * ty + dTy; + tx = newTx; ty = newTy; dtheta += dRot; + + if (Math.abs(dRot) < 5e-5 && Math.abs(dTx) < 0.001 && Math.abs(dTy) < 0.001) break; + } + return { tx, ty, dtheta }; +} + +// ─── ÉTAT DE LA SIMULATION SLAM ────────────────────────────────────────────── +const slam_s = { + step: 0, + playing: true, + showMap: true, + showGraph: true, + voEnabled: false, // odométrie visuelle + + // Pose vraie + trueX: 0, trueY: 0, trueH: 0, + + // Dead-reckoning (dérive longitudinale cumulée + bruit) + drX: 0, drY: 0, drH: 0, + drDriftLong: 0, // dérive longitudinale pure (couloir) + drNoiseCum: 0, // bruit latéral cumulé + + // SLAM estimé + slamX: 0, slamY: 0, slamH: 0, + + // Carte accumulée + mapPts: [], + + // Sweep sonar courant + sweepRayIdx: 0, + lastSweepPts: [], + + // Keyframes: { x, y, h, scanPts: [] } + keyframes: [], + KF_DIST: 1.5, // distance entre keyframes (m) + lastKfDist: 0, + + // Traînées + trailTrue: [], + trailDR: [], + trailSLAM: [], + + // Erreurs + errDR: 0, + errSLAM: 0, + + // Loop closure + loopClosed: false, + loopFlashFrames: 0, + loopClosedAt: -1, // step où la boucle a été fermée + + // Correction globale appliquée à la traînée SLAM lors de la boucle + globalCorrection: { tx: 0, ty: 0 }, + + // Dégénérescence couloir: dans le couloir droit, on accumule dérive longitudinale + inCorridor: false, + corridorDriftRate: 0, +}; + +// Détecter si on est dans le couloir (zone x=[0..12], y=[0..3.5]) +function slam_isInCorridor(x, y) { + return x > 0 && x < 12 && y > 0 && y < 3.5; +} + +function slam_initSim() { + const t0 = slam_TRAJ[0]; + Object.assign(slam_s, { + step: 0, loopClosed: false, loopFlashFrames: 0, loopClosedAt: -1, + trueX: t0.x, trueY: t0.y, trueH: t0.heading, + drX: t0.x, drY: t0.y, drH: t0.heading, + slamX: t0.x, slamY: t0.y, slamH: t0.heading, + drDriftLong: 0, drNoiseCum: 0, + mapPts: [], lastSweepPts: [], sweepRayIdx: 0, + keyframes: [], lastKfDist: 0, + trailTrue: [{ x: t0.x, y: t0.y }], + trailDR: [{ x: t0.x, y: t0.y }], + trailSLAM: [{ x: t0.x, y: t0.y }], + errDR: 0, errSLAM: 0, + globalCorrection: { tx: 0, ty: 0 }, + corridorDriftRate: (Math.random() - 0.5) * 0.006 + 0.004, // dérive longitudinale positive + }); +} + +function slam_stepSim() { + if (slam_s.step >= slam_TRAJ.length - 1) return; + + slam_s.step++; + const tp = slam_TRAJ[slam_s.step]; + + // Pose vraie + slam_s.trueX = tp.x; + slam_s.trueY = tp.y; + slam_s.trueH = tp.heading; + + // ── Dead-reckoning avec modèle de dérive ────────────────────────────────── + // Le dead-reckoning suit grossièrement la trajectoire vraie mais dérive. + // Dans le couloir: dérive principalement LONGITUDINALE (le sonar ne voit pas de coin → ICP glisse). + // Hors couloir: bruit plus isotrope. + const inCorridor = slam_isInCorridor(slam_s.trueX, slam_s.trueY); + slam_s.inCorridor = inCorridor; + + // Bruit isotrope de base + const baseNoise = slam_s.voEnabled ? 0.0008 : 0.0018; + const noiseX = (Math.random() - 0.5) * baseNoise; + const noiseY = (Math.random() - 0.5) * baseNoise; + const noiseH = (Math.random() - 0.5) * 0.0005; + + // Dérive longitudinale supplémentaire dans le couloir + let longDrift = 0; + if (inCorridor && !slam_s.voEnabled) { + // Le couloir axe X → dérive selon X si on se déplace dans cette direction + longDrift = slam_s.corridorDriftRate * Math.abs(Math.cos(slam_s.trueH)); + } else if (inCorridor && slam_s.voEnabled) { + // VO réduit la dérive longitudinale à ~30% + longDrift = slam_s.corridorDriftRate * 0.3 * Math.abs(Math.cos(slam_s.trueH)); + } + + slam_s.drDriftLong += longDrift; + + slam_s.drX = tp.x + slam_s.drDriftLong + noiseX * slam_s.step * 0.08; + slam_s.drY = tp.y + noiseY * slam_s.step * 0.08; + slam_s.drH = tp.heading + noiseH * slam_s.step * 0.08; + + // ── Sweep sonar (un rayon par step) ──────────────────────────────────────── + const rayAngle = (slam_s.sweepRayIdx / slam_N_RAYS) * 2 * Math.PI + slam_s.trueH; + const hit = slam_castRay(slam_s.trueX, slam_s.trueY, rayAngle, slam_MAX_RANGE); + if (hit) { + const noise = (Math.random() - 0.5) * 0.06; + const nx = hit.x + noise * Math.cos(rayAngle + Math.PI/2); + const ny = hit.y + noise * Math.sin(rayAngle + Math.PI/2); + slam_s.lastSweepPts.push({ x: nx, y: ny }); + slam_s.mapPts.push({ x: nx, y: ny }); + if (slam_s.mapPts.length > 12000) slam_s.mapPts.splice(0, 300); + } + slam_s.sweepRayIdx++; + + // ── Sweep complet → ICP scan-to-map ─────────────────────────────────────── + if (slam_s.sweepRayIdx >= slam_N_RAYS) { + slam_s.sweepRayIdx = 0; + const currentSweep = slam_s.lastSweepPts; + slam_s.lastSweepPts = []; + + if (slam_s.mapPts.length > 30 && currentSweep.length > 6) { + const mapSample = slam_s.mapPts.slice(-3000); + const icpRes = slam_icp2D(currentSweep, mapSample); + + // Correction bornée — mais dans le couloir, la correction longitudinale est limitée + // (on simule la dégénérescence: ICP ne corrige pas bien le long de l'axe du couloir) + let corrFactor = 0.9; + if (inCorridor) { + // ICP peut corriger latéral (Y) mais PAS longitudinal (X) en couloir droit + // → On brique la correction X dans le couloir + corrFactor = 0.2; // mauvaise correction longitudinale + } + + const boundT = 0.15; + const boundR = 0.04; + const cx = Math.max(-boundT, Math.min(boundT, icpRes.tx * corrFactor)); + const cy = Math.max(-boundT, Math.min(boundT, icpRes.ty)); // latéral OK + const cR = Math.max(-boundR, Math.min(boundR, icpRes.dtheta)); + + slam_s.slamX = slam_s.trueX + cx; + slam_s.slamY = slam_s.trueY + cy; + slam_s.slamH = slam_s.trueH + cR; + } else { + slam_s.slamX = slam_s.trueX; + slam_s.slamY = slam_s.trueY; + slam_s.slamH = slam_s.trueH; + } + + // ── Keyframe ───────────────────────────────────────────────────────────── + const cumDist = tp.cumDist; + if (cumDist - slam_s.lastKfDist > slam_s.KF_DIST) { + slam_s.lastKfDist = cumDist; + // Échantillon de scan pour matching futur + const scanSnapshot = currentSweep.slice(); + slam_s.keyframes.push({ + x: slam_s.trueX, y: slam_s.trueY, h: slam_s.trueH, + slamX: slam_s.slamX, slamY: slam_s.slamY, + scan: scanSnapshot, + step: slam_s.step, + }); + } + + // ── Détection loop closure ──────────────────────────────────────────────── + // On cherche si on est revenu près d'un keyframe *ancien* (>5 keyframes en arrière) + if (!slam_s.loopClosed && slam_s.keyframes.length > 6 && currentSweep.length > 10) { + const minKfAge = 6; // ignorer les keyframes récents + for (let ki = 0; ki < slam_s.keyframes.length - minKfAge; ki++) { + const kf = slam_s.keyframes[ki]; + const d = Math.sqrt((slam_s.trueX - kf.x)**2 + (slam_s.trueY - kf.y)**2); + if (d < 1.2 && kf.scan.length > 5) { + // Correspondance confirmée par ICP entre scans + const lcRes = slam_icp2D(currentSweep, kf.scan); + const lcErr = Math.sqrt(lcRes.tx**2 + lcRes.ty**2); + if (lcErr < 0.5) { + // FERMETURE DE BOUCLE DÉTECTÉE + slam_s.loopClosed = true; + slam_s.loopClosedAt = slam_s.step; + slam_s.loopFlashFrames = 80; + + // Correction globale: on estime l'erreur accumulée de la pose SLAM + // et on la redistribue linéairement sur toute la traînée + const errX = slam_s.trueX - slam_s.slamX; + const errY = slam_s.trueY - slam_s.slamY; + slam_s.globalCorrection = { tx: errX, ty: errY }; + + // Corriger toutes les poses de la traînée SLAM rétroactivement + const n = slam_s.trailSLAM.length; + for (let ti = 0; ti < n; ti++) { + const frac = ti / n; // correction progressive (0 au départ, 1 maintenant) + slam_s.trailSLAM[ti].x += frac * errX; + slam_s.trailSLAM[ti].y += frac * errY; + } + // Réinitialiser la dérive dead-reckoning aussi + slam_s.drDriftLong = 0; + break; + } + } + } + } + } + + // Après fermeture de boucle, SLAM estimé est verrouillé sur la vraie position (bornée) + if (slam_s.loopClosed) { + slam_s.slamX = slam_s.trueX + (Math.random() - 0.5) * 0.04; + slam_s.slamY = slam_s.trueY + (Math.random() - 0.5) * 0.04; + slam_s.drDriftLong *= 0.98; // la dérive se résorbe après boucle + } + + // ── Erreurs ─────────────────────────────────────────────────────────────── + slam_s.errDR = Math.sqrt((slam_s.drX - slam_s.trueX)**2 + (slam_s.drY - slam_s.trueY)**2); + slam_s.errSLAM = Math.sqrt((slam_s.slamX - slam_s.trueX)**2 + (slam_s.slamY - slam_s.trueY)**2); + + // ── Traînées ────────────────────────────────────────────────────────────── + const TRAIL_MAX = 800; + slam_s.trailTrue.push({ x: slam_s.trueX, y: slam_s.trueY }); + slam_s.trailDR.push({ x: slam_s.drX, y: slam_s.drY }); + slam_s.trailSLAM.push({ x: slam_s.slamX, y: slam_s.slamY }); + if (slam_s.trailTrue.length > TRAIL_MAX) { slam_s.trailTrue.shift(); slam_s.trailDR.shift(); slam_s.trailSLAM.shift(); } + + if (slam_s.loopFlashFrames > 0) slam_s.loopFlashFrames--; +} + +// ─── CANVAS & RENDU ────────────────────────────────────────────────────────── +const slam_canvas = document.getElementById('slam_canvas'); +const slam_ctx = slam_canvas.getContext('2d'); + +// Bounding box de la pièce + marge +const slam_BBOX = { minX: -1, maxX: 19.5, minY: -3.5, maxY: 11 }; + +function slam_world2px(x, y) { + const W = slam_canvas.width, H = slam_canvas.height; + const margin = 24; + const scaleX = (W - 2 * margin) / (slam_BBOX.maxX - slam_BBOX.minX); + const scaleY = (H - 2 * margin) / (slam_BBOX.maxY - slam_BBOX.minY); + const s = Math.min(scaleX, scaleY); + // Centre la pièce + const offX = margin + (W - 2*margin - s*(slam_BBOX.maxX - slam_BBOX.minX)) / 2; + const offY = margin + (H - 2*margin - s*(slam_BBOX.maxY - slam_BBOX.minY)) / 2; + return { + x: offX + (x - slam_BBOX.minX) * s, + y: H - offY - (y - slam_BBOX.minY) * s, // Y inversé canvas + s, + }; +} + +function slam_drawRoom() { + const ctx = slam_ctx; + ctx.beginPath(); + for (let i = 0; i < slam_POLY.length; i++) { + const p = slam_world2px(slam_POLY[i].x, slam_POLY[i].y); + i === 0 ? ctx.moveTo(p.x, p.y) : ctx.lineTo(p.x, p.y); + } + ctx.closePath(); + ctx.fillStyle = 'rgba(18,26,38,0.97)'; + ctx.fill(); + ctx.strokeStyle = '#5a6476'; + ctx.lineWidth = 2.5; + ctx.stroke(); + + // Label couloir + const lc = slam_world2px(6, 1.75); + ctx.fillStyle = 'rgba(139,148,158,0.45)'; + ctx.font = '11px system-ui'; + ctx.textAlign = 'center'; + ctx.fillText('couloir', lc.x, lc.y); + + // Label salle + const ls = slam_world2px(15, 4.5); + ctx.fillText('salle', ls.x, ls.y); + + // Label niche basse + const ln = slam_world2px(1.5, -1.25); + ctx.fillText('niche', ln.x, ln.y); +} + +function slam_drawMap() { + if (!slam_s.showMap || slam_s.mapPts.length === 0) return; + const ctx = slam_ctx; + const step = Math.max(1, Math.floor(slam_s.mapPts.length / 3000)); + for (let i = 0; i < slam_s.mapPts.length; i += step) { + const pt = slam_s.mapPts[i]; + const p = slam_world2px(pt.x, pt.y); + ctx.beginPath(); + ctx.arc(p.x, p.y, 1.5, 0, Math.PI * 2); + ctx.fillStyle = 'rgba(255,60,60,0.55)'; + ctx.fill(); + } +} + +function slam_drawSweepCone() { + const ctx = slam_ctx; + const rp = slam_world2px(slam_s.trueX, slam_s.trueY); + const s = rp.s; + const rangePx = s * slam_MAX_RANGE; + + // Cercle de portée + ctx.save(); + ctx.globalAlpha = 0.07; + ctx.beginPath(); + ctx.arc(rp.x, rp.y, rangePx, 0, Math.PI * 2); + ctx.fillStyle = '#00bfff'; + ctx.fill(); + ctx.restore(); + + // Arc du sweep en cours + const sweepFrac = slam_s.sweepRayIdx / slam_N_RAYS; + if (sweepFrac > 0) { + ctx.save(); + ctx.globalAlpha = 0.12; + ctx.beginPath(); + ctx.moveTo(rp.x, rp.y); + const startAngle = slam_s.trueH - Math.PI / 2; // canvas Y inversé + const endAngle = startAngle - sweepFrac * 2 * Math.PI; + ctx.arc(rp.x, rp.y, rangePx, startAngle, endAngle, true); + ctx.closePath(); + ctx.fillStyle = '#00bfff'; + ctx.fill(); + ctx.restore(); + } + + // Points sonar courants + for (const pt of slam_s.lastSweepPts) { + const p = slam_world2px(pt.x, pt.y); + ctx.beginPath(); + ctx.arc(p.x, p.y, 2, 0, Math.PI * 2); + ctx.fillStyle = '#ff5050'; + ctx.fill(); + } +} + +function slam_drawTrail(trail, color, dash, lineWidth) { + if (trail.length < 2) return; + const ctx = slam_ctx; + ctx.beginPath(); + ctx.setLineDash(dash || []); + ctx.strokeStyle = color; + ctx.lineWidth = lineWidth || 1.5; + for (let i = 0; i < trail.length; i++) { + const p = slam_world2px(trail[i].x, trail[i].y); + i === 0 ? ctx.moveTo(p.x, p.y) : ctx.lineTo(p.x, p.y); + } + ctx.stroke(); + ctx.setLineDash([]); +} + +function slam_drawROV(x, y, heading, color, alpha) { + const ctx = slam_ctx; + const p = slam_world2px(x, y); + const r = Math.max(5, p.s * 0.2); + ctx.save(); + ctx.globalAlpha = alpha || 1; + ctx.translate(p.x, p.y); + ctx.rotate(-heading); // canvas Y inversé → négatif + ctx.beginPath(); + ctx.arc(0, 0, r, 0, Math.PI * 2); + ctx.fillStyle = color; + ctx.fill(); + ctx.strokeStyle = 'rgba(255,255,255,0.25)'; + ctx.lineWidth = 1; + ctx.stroke(); + ctx.beginPath(); + ctx.moveTo(0, 0); + ctx.lineTo(r * 2.2, 0); + ctx.strokeStyle = color; + ctx.lineWidth = 2; + ctx.stroke(); + ctx.restore(); +} + +function slam_drawKeyframes() { + const ctx = slam_ctx; + const kfs = slam_s.keyframes; + if (!slam_s.showGraph || kfs.length === 0) return; + + // Lignes du pose-graph + ctx.beginPath(); + ctx.strokeStyle = 'rgba(163,113,247,0.4)'; + ctx.lineWidth = 1; + ctx.setLineDash([3, 5]); + for (let i = 0; i < kfs.length; i++) { + const p = slam_world2px(kfs[i].x, kfs[i].y); + i === 0 ? ctx.moveTo(p.x, p.y) : ctx.lineTo(p.x, p.y); + } + ctx.stroke(); + ctx.setLineDash([]); + + // Marqueurs keyframe + for (let i = 0; i < kfs.length; i++) { + const p = slam_world2px(kfs[i].x, kfs[i].y); + ctx.beginPath(); + ctx.arc(p.x, p.y, 4, 0, Math.PI * 2); + ctx.fillStyle = '#a371f7'; + ctx.fill(); + } +} + +function slam_drawDegeneracyLabel() { + if (!slam_s.inCorridor || slam_s.loopClosed) return; + const ctx = slam_ctx; + // Label dégénérescence dans le couloir + const lp = slam_world2px(6, 3.9); + ctx.save(); + ctx.fillStyle = 'rgba(210,153,34,0.9)'; + ctx.font = 'bold 11px system-ui'; + ctx.textAlign = 'center'; + ctx.fillText('⚠ dégén. couloir', lp.x, lp.y); + ctx.restore(); +} + +function slam_drawLoopClosureEvent() { + const ctx = slam_ctx; + if (!slam_s.loopClosed) return; + + const flash = slam_s.loopFlashFrames; + if (flash > 0) { + // Flash global + ctx.save(); + ctx.globalAlpha = (flash / 80) * 0.22; + ctx.fillStyle = '#56d364'; + ctx.fillRect(0, 0, slam_canvas.width, slam_canvas.height); + ctx.restore(); + + // Cercle pulsé au point de fermeture + const lp = slam_world2px(slam_s.trueX, slam_s.trueY); + const r = lp.s * 2.5 * (1 - flash / 80) + 10; + ctx.save(); + ctx.globalAlpha = (flash / 80) * 0.7; + ctx.beginPath(); + ctx.arc(lp.x, lp.y, r, 0, Math.PI * 2); + ctx.strokeStyle = '#56d364'; + ctx.lineWidth = 3; + ctx.stroke(); + ctx.restore(); + } + + // Label permanent + const W = slam_canvas.width, H = slam_canvas.height; + ctx.save(); + ctx.fillStyle = 'rgba(86,211,100,0.95)'; + ctx.font = 'bold 14px system-ui'; + ctx.textAlign = 'center'; + ctx.fillText('BOUCLE FERMEE ✓', W / 2, 22); + ctx.restore(); +} + +function slam_drawScale() { + const ctx = slam_ctx; + const W = slam_canvas.width, H = slam_canvas.height; + const p0 = slam_world2px(0, slam_BBOX.minY); + const p1 = slam_world2px(2, slam_BBOX.minY); + const s = p1.x - p0.x; // 2m en px + const sx = W - 12, sy = H - 10; + ctx.save(); + ctx.strokeStyle = '#8b949e'; ctx.lineWidth = 1.5; + ctx.beginPath(); ctx.moveTo(sx - s, sy); ctx.lineTo(sx, sy); ctx.stroke(); + ctx.beginPath(); ctx.moveTo(sx - s, sy - 4); ctx.lineTo(sx - s, sy + 4); ctx.stroke(); + ctx.beginPath(); ctx.moveTo(sx, sy - 4); ctx.lineTo(sx, sy + 4); ctx.stroke(); + ctx.fillStyle = '#8b949e'; ctx.font = '11px system-ui'; ctx.textAlign = 'center'; + ctx.fillText('2 m', sx - s / 2, sy - 6); + ctx.restore(); +} + +function slam_renderFrame() { + const W = slam_canvas.width, H = slam_canvas.height; + slam_ctx.clearRect(0, 0, W, H); + slam_ctx.fillStyle = '#0a0e14'; + slam_ctx.fillRect(0, 0, W, H); + + slam_drawRoom(); + slam_drawMap(); + slam_drawKeyframes(); + slam_drawTrail(slam_s.trailTrue, '#00bfff', [], 2); + slam_drawTrail(slam_s.trailDR, 'rgba(255,107,107,0.7)', [5, 4], 1.5); + slam_drawTrail(slam_s.trailSLAM, 'rgba(86,211,100,0.85)', [2, 3], 1.5); + slam_drawSweepCone(); + slam_drawROV(slam_s.trueX, slam_s.trueY, slam_s.trueH, '#00bfff', 1); + slam_drawROV(slam_s.drX, slam_s.drY, slam_s.drH, '#ff6b6b', 0.65); + slam_drawROV(slam_s.slamX, slam_s.slamY, slam_s.slamH, '#56d364', 0.8); + slam_drawDegeneracyLabel(); + slam_drawLoopClosureEvent(); + slam_drawScale(); + + // Labels temps + état + const tp = slam_TRAJ[slam_s.step]; + slam_ctx.fillStyle = 'rgba(139,148,158,0.8)'; + slam_ctx.font = '11px monospace'; + slam_ctx.textAlign = 'left'; + slam_ctx.fillText(`dist: ${tp.cumDist.toFixed(1)} m kf: ${slam_s.keyframes.length}`, 8, 14); + if (slam_s.inCorridor && !slam_s.loopClosed) { + slam_ctx.fillStyle = 'rgba(210,153,34,0.85)'; + slam_ctx.fillText('couloir: dérive longitudinale active', 8, 28); + } +} + +// ─── MISE À JOUR UI ────────────────────────────────────────────────────────── +function slam_updateUI() { + const tp = slam_TRAJ[slam_s.step]; + document.getElementById('slam_dist').textContent = tp.cumDist.toFixed(2) + ' m'; + document.getElementById('slam_kf').textContent = slam_s.keyframes.length; + + const stateEl = document.getElementById('slam_state'); + if (slam_s.loopClosed) { + stateEl.textContent = 'Boucle fermée ✓'; + stateEl.style.color = 'var(--icp-ok)'; + } else if (slam_s.inCorridor) { + stateEl.textContent = 'Couloir — dégénérescence…'; + stateEl.style.color = 'var(--warn)'; + } else { + stateEl.textContent = 'Cartographie…'; + stateEl.style.color = 'var(--ping360)'; + } + + const maxErr = 3.0; + const drPct = Math.min(100, (slam_s.errDR / maxErr) * 100); + const slamPct = Math.min(100, (slam_s.errSLAM / maxErr * 4) * 100); + document.getElementById('slam_errDR').textContent = slam_s.errDR.toFixed(3) + ' m'; + document.getElementById('slam_errSLAM').textContent = slam_s.errSLAM.toFixed(3) + ' m'; + document.getElementById('slam_errBarDR').style.width = drPct + '%'; + document.getElementById('slam_errBarSLAM').style.width = slamPct + '%'; + + const frac = slam_s.step / (slam_TRAJ.length - 1); + document.getElementById('slam_slider').value = Math.round(frac * 1000); + const totalSecs = 180; + document.getElementById('slam_sliderTime').textContent = Math.round(frac * totalSecs) + ' s'; +} + +// ─── BOUCLE ANIMATION ──────────────────────────────────────────────────────── +let slam_animId = null; + +function slam_animate() { + if (slam_s.playing && slam_s.step < slam_TRAJ.length - 1) { + for (let i = 0; i < 3; i++) slam_stepSim(); + } + slam_renderFrame(); + slam_updateUI(); + slam_animId = requestAnimationFrame(slam_animate); +} + +// ─── CONTRÔLES ─────────────────────────────────────────────────────────────── +const slam_btnPlay = document.getElementById('slam_btnPlay'); +const slam_btnReset = document.getElementById('slam_btnReset'); +const slam_btnVO = document.getElementById('slam_btnVO'); +const slam_btnMap = document.getElementById('slam_btnMap'); +const slam_btnGraph = document.getElementById('slam_btnGraph'); +const slam_slider = document.getElementById('slam_slider'); + +slam_btnPlay.addEventListener('click', () => { + slam_s.playing = !slam_s.playing; + slam_btnPlay.textContent = slam_s.playing ? '⏸ Pause' : '▶ Play'; +}); + +slam_btnReset.addEventListener('click', () => { + slam_initSim(); + slam_s.playing = true; + slam_btnPlay.textContent = '⏸ Pause'; +}); + +slam_btnVO.addEventListener('click', () => { + slam_s.voEnabled = !slam_s.voEnabled; + slam_btnVO.textContent = slam_s.voEnabled ? '📷 VO caméra ON' : '📷 VO caméra OFF'; + slam_btnVO.classList.toggle('active', slam_s.voEnabled); + // Reset pour voir l'effet + slam_initSim(); + slam_s.playing = true; + slam_btnPlay.textContent = '⏸ Pause'; +}); + +slam_btnMap.addEventListener('click', () => { + slam_s.showMap = !slam_s.showMap; + slam_btnMap.textContent = slam_s.showMap ? '🗺 Masquer carte' : '🗺 Afficher carte'; +}); + +slam_btnGraph.addEventListener('click', () => { + slam_s.showGraph = !slam_s.showGraph; + slam_btnGraph.textContent = slam_s.showGraph ? '🔗 Masquer pose-graph' : '🔗 Afficher pose-graph'; +}); + +slam_slider.addEventListener('input', () => { + slam_s.playing = false; + slam_btnPlay.textContent = '▶ Play'; + const targetStep = Math.round((slam_slider.value / 1000) * (slam_TRAJ.length - 1)); + slam_initSim(); + slam_s.playing = false; + for (let i = 0; i < targetStep; i++) slam_stepSim(); + slam_renderFrame(); + slam_updateUI(); +}); + +// ─── DÉMARRAGE ─────────────────────────────────────────────────────────────── +slam_initSim(); +slam_animate(); + +})(); // IIFE — pas de pollution globale +