feat(moulin-mapper): section SLAM incrémental — pièce en L, recoins, fermeture de boucle
This commit is contained in:
Flag
914
web/index.html
914
web/index.html
@@ -535,6 +535,83 @@
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<!-- SLAM INCREMENTAL -->
|
||||
<div id="demoSLAM" style="padding:3.5rem 1rem;background:var(--surface);border-top:1px solid var(--border);border-bottom:1px solid var(--border);">
|
||||
<div class="inner" style="max-width:1080px;margin:0 auto;">
|
||||
<h2 style="margin-bottom:.5rem;"><span class="sec-num">07</span> Pièce longue & recoins — SLAM incrémental temps réel</h2>
|
||||
<p style="color:var(--text-muted);font-size:.9rem;margin-bottom:.75rem;">
|
||||
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.
|
||||
</p>
|
||||
|
||||
<div class="controls" style="margin-bottom:.5rem;">
|
||||
<button id="slam_btnPlay">⏸ Pause</button>
|
||||
<button id="slam_btnReset">↺ Réinitialiser</button>
|
||||
<button id="slam_btnVO" class="toggle-btn">📷 VO caméra OFF</button>
|
||||
<button id="slam_btnMap">🗺 Masquer carte</button>
|
||||
<button id="slam_btnGraph">🔗 Masquer pose-graph</button>
|
||||
</div>
|
||||
<div style="display:flex;align-items:center;gap:.5rem;margin-bottom:.75rem;">
|
||||
<span style="font-size:.8rem;color:var(--text-muted);">Temps:</span>
|
||||
<input type="range" id="slam_slider" min="0" max="1000" value="0" style="flex:1;max-width:340px;">
|
||||
<span id="slam_sliderTime" style="font-size:.8rem;color:var(--text-muted);min-width:40px;">0 s</span>
|
||||
</div>
|
||||
|
||||
<div class="demo-layout">
|
||||
<canvas id="slam_canvas" width="620" height="460" style="border:1px solid var(--border);border-radius:8px;background:#0a0e14;flex:1 1 520px;min-width:280px;"></canvas>
|
||||
|
||||
<div class="sidebar">
|
||||
<div class="ctrl-group">
|
||||
<label>Distance parcourue</label>
|
||||
<div class="value" id="slam_dist">0.00 m</div>
|
||||
</div>
|
||||
<div class="ctrl-group">
|
||||
<label>Keyframes</label>
|
||||
<div class="value" id="slam_kf">0</div>
|
||||
</div>
|
||||
<div class="ctrl-group">
|
||||
<label>État</label>
|
||||
<div class="value" id="slam_state" style="font-size:.95rem;color:var(--ping360);">Démarrage…</div>
|
||||
</div>
|
||||
<div class="error-display">
|
||||
<div class="err-label">Erreur position estimée</div>
|
||||
<div style="margin-top:.3rem;">
|
||||
<span style="font-size:.75rem;color:var(--imu-drift);">Dead-reckoning:</span>
|
||||
<span class="value bad" id="slam_errDR" style="font-size:1rem;">—</span>
|
||||
</div>
|
||||
<div class="err-bar-wrap" style="margin-bottom:.5rem;">
|
||||
<div class="err-bar" id="slam_errBarDR" style="width:0%;background:var(--imu-drift);"></div>
|
||||
</div>
|
||||
<div>
|
||||
<span style="font-size:.75rem;color:var(--icp-ok);">SLAM corrigé:</span>
|
||||
<span class="value ok" id="slam_errSLAM" style="font-size:1rem;">—</span>
|
||||
</div>
|
||||
<div class="err-bar-wrap">
|
||||
<div class="err-bar" id="slam_errBarSLAM" style="width:0%;background:var(--icp-ok);"></div>
|
||||
</div>
|
||||
</div>
|
||||
<div class="legend">
|
||||
<div class="legend-item"><div class="legend-dot" style="background:var(--ping360)"></div><span>Trajet réel</span></div>
|
||||
<div class="legend-item"><div class="legend-dot" style="background:var(--imu-drift)"></div><span>Dead-reckoning</span></div>
|
||||
<div class="legend-item"><div class="legend-dot" style="background:var(--icp-ok)"></div><span>SLAM estimé</span></div>
|
||||
<div class="legend-item"><div class="legend-dot" style="background:#a371f7"></div><span>Keyframes</span></div>
|
||||
<div class="legend-item"><div class="legend-dot" style="background:var(--wall-pt)"></div><span>Retours sonar</span></div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div class="math-box" style="margin-top:1.5rem;">
|
||||
<h3>Dégénérescence couloir & fermeture de boucle</h3>
|
||||
<p>Dans un couloir droit, les murs parallèles forment une géométrie <strong>symétrique selon l'axe longitudinal</strong>.
|
||||
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 <strong>fermeture de boucle</strong> (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 (<code>g2o</code> / linéarisée) corrige <em>toutes</em> les poses d'un coup.</p>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<footer>
|
||||
moulin-mapper · prototype · Cosma / Silent Flow
|
||||
</footer>
|
||||
@@ -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
|
||||
|
||||
</script>
|
||||
</body>
|
||||
</html>
|
||||
|
||||
Reference in New Issue
Block a user