2306 lines
87 KiB
HTML
2306 lines
87 KiB
HTML
<!DOCTYPE html>
|
||
<html lang="fr">
|
||
<head>
|
||
<meta charset="UTF-8">
|
||
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
||
<title>Moulin à marée — cartographie ROV sans GPS</title>
|
||
<style>
|
||
:root {
|
||
--bg: #0d1117;
|
||
--surface: #161b22;
|
||
--border: #30363d;
|
||
--accent: #1f6feb;
|
||
--accent2: #238636;
|
||
--warn: #d29922;
|
||
--danger: #da3633;
|
||
--text: #e6edf3;
|
||
--text-muted: #8b949e;
|
||
--ping360: #00bfff;
|
||
--imu-drift: #ff6b6b;
|
||
--icp-ok: #56d364;
|
||
--wall-pt: #ff4040;
|
||
}
|
||
* { box-sizing: border-box; margin: 0; padding: 0; }
|
||
html { scroll-behavior: smooth; }
|
||
body {
|
||
background: var(--bg);
|
||
color: var(--text);
|
||
font-family: 'Segoe UI', system-ui, sans-serif;
|
||
font-size: 15px;
|
||
line-height: 1.6;
|
||
}
|
||
a { color: var(--accent); text-decoration: none; }
|
||
|
||
/* NAV */
|
||
nav {
|
||
position: sticky; top: 0; z-index: 100;
|
||
background: rgba(13,17,23,0.92);
|
||
border-bottom: 1px solid var(--border);
|
||
backdrop-filter: blur(8px);
|
||
padding: 0 2rem;
|
||
display: flex; align-items: center; gap: 2rem; height: 52px;
|
||
}
|
||
nav .logo { font-weight: 700; font-size: 1rem; color: var(--ping360); letter-spacing: .05em; }
|
||
nav a { color: var(--text-muted); font-size: .875rem; transition: color .2s; }
|
||
nav a:hover { color: var(--text); }
|
||
|
||
/* LAYOUT */
|
||
.hero {
|
||
padding: 5rem 2rem 3rem;
|
||
text-align: center;
|
||
background: radial-gradient(ellipse 80% 60% at 50% 0%, rgba(31,111,235,.12) 0%, transparent 70%);
|
||
border-bottom: 1px solid var(--border);
|
||
}
|
||
.hero h1 { font-size: clamp(1.8rem, 5vw, 3rem); font-weight: 800; line-height: 1.2; margin-bottom: 1rem; }
|
||
.hero h1 span { color: var(--ping360); }
|
||
.hero p { max-width: 660px; margin: 0 auto 2rem; color: var(--text-muted); font-size: 1.05rem; }
|
||
.badges { display: flex; gap: .5rem; justify-content: center; flex-wrap: wrap; }
|
||
.badge {
|
||
padding: .25rem .75rem; border-radius: 999px; font-size: .78rem; font-weight: 600;
|
||
border: 1px solid;
|
||
}
|
||
.badge-blue { color: var(--ping360); border-color: var(--ping360); background: rgba(0,191,255,.08); }
|
||
.badge-green { color: var(--icp-ok); border-color: var(--icp-ok); background: rgba(86,211,100,.08); }
|
||
.badge-warn { color: var(--warn); border-color: var(--warn); background: rgba(210,153,34,.08); }
|
||
|
||
section { padding: 3.5rem 2rem; max-width: 1080px; margin: 0 auto; }
|
||
section + section { border-top: 1px solid var(--border); }
|
||
h2 { font-size: 1.5rem; font-weight: 700; margin-bottom: 1.5rem; }
|
||
h2 .sec-num { color: var(--accent); margin-right: .5rem; }
|
||
p + p { margin-top: .75rem; }
|
||
|
||
/* SENSOR TABLE */
|
||
.sensor-table { width: 100%; border-collapse: collapse; margin-top: 1rem; }
|
||
.sensor-table th {
|
||
text-align: left; padding: .5rem .75rem;
|
||
font-size: .8rem; text-transform: uppercase; letter-spacing: .07em;
|
||
color: var(--text-muted); background: var(--surface);
|
||
border-bottom: 1px solid var(--border);
|
||
}
|
||
.sensor-table td { padding: .6rem .75rem; border-bottom: 1px solid var(--border); font-size: .9rem; vertical-align: top; }
|
||
.sensor-table tr:last-child td { border-bottom: none; }
|
||
.sensor-table tr:hover td { background: rgba(255,255,255,.03); }
|
||
.tag { display: inline-block; padding: .1rem .5rem; border-radius: 4px; font-size: .75rem; font-weight: 600; margin: .1rem; }
|
||
.tag-xy { background: rgba(0,191,255,.15); color: var(--ping360); }
|
||
.tag-z { background: rgba(86,211,100,.15); color: var(--icp-ok); }
|
||
.tag-att { background: rgba(210,153,34,.15); color: var(--warn); }
|
||
.tag-tex { background: rgba(163,113,247,.15); color: #a371f7; }
|
||
.tag-img { background: rgba(255,107,107,.15); color: var(--imu-drift); }
|
||
|
||
/* 2D DEMO */
|
||
#demo2d {
|
||
padding: 3.5rem 1rem;
|
||
background: var(--surface);
|
||
border-top: 1px solid var(--border);
|
||
border-bottom: 1px solid var(--border);
|
||
}
|
||
#demo2d .inner { max-width: 1080px; margin: 0 auto; }
|
||
.demo-layout {
|
||
display: flex; gap: 1rem; align-items: flex-start;
|
||
flex-wrap: wrap;
|
||
}
|
||
#canvas2d {
|
||
border: 1px solid var(--border);
|
||
border-radius: 8px;
|
||
display: block;
|
||
background: #0a0e14;
|
||
flex: 1 1 520px;
|
||
min-width: 280px;
|
||
cursor: crosshair;
|
||
}
|
||
.sidebar {
|
||
flex: 0 0 220px;
|
||
min-width: 180px;
|
||
display: flex; flex-direction: column; gap: .75rem;
|
||
}
|
||
.ctrl-group {
|
||
background: var(--bg);
|
||
border: 1px solid var(--border);
|
||
border-radius: 8px;
|
||
padding: .75rem;
|
||
}
|
||
.ctrl-group label { display: block; font-size: .78rem; color: var(--text-muted); margin-bottom: .35rem; text-transform: uppercase; letter-spacing: .06em; }
|
||
.ctrl-group .value { font-size: 1.1rem; font-weight: 700; color: var(--text); }
|
||
.ctrl-group .value.ok { color: var(--icp-ok); }
|
||
.ctrl-group .value.bad { color: var(--imu-drift); }
|
||
|
||
/* gauge */
|
||
.gauge-wrap { position: relative; height: 90px; width: 100%; }
|
||
.gauge-bar {
|
||
position: absolute; bottom: 0; left: 0; width: 100%;
|
||
background: linear-gradient(to top, var(--accent), var(--ping360));
|
||
border-radius: 4px 4px 0 0;
|
||
transition: height .3s;
|
||
}
|
||
.gauge-bg {
|
||
position: absolute; inset: 0;
|
||
background: rgba(255,255,255,.04);
|
||
border: 1px solid var(--border); border-radius: 4px;
|
||
}
|
||
.gauge-label { position: absolute; top: 4px; right: 6px; font-size: .75rem; color: var(--text-muted); }
|
||
|
||
/* attitude display */
|
||
.attitude-grid { display: grid; grid-template-columns: 1fr 1fr 1fr; gap: .4rem; margin-top: .3rem; }
|
||
.att-item { text-align: center; }
|
||
.att-item .av { font-size: .95rem; font-weight: 700; color: var(--warn); }
|
||
.att-item .ak { font-size: .65rem; color: var(--text-muted); }
|
||
|
||
/* controls */
|
||
.controls {
|
||
display: flex; gap: .5rem; flex-wrap: wrap;
|
||
margin: .75rem 0;
|
||
align-items: center;
|
||
}
|
||
button {
|
||
padding: .35rem .8rem; border-radius: 6px; border: 1px solid var(--border);
|
||
background: var(--surface); color: var(--text); font-size: .85rem; cursor: pointer;
|
||
transition: background .15s, border-color .15s;
|
||
user-select: none;
|
||
}
|
||
button:hover { background: #21262d; border-color: #58a6ff; }
|
||
button.active { background: var(--accent); border-color: var(--accent); color: #fff; }
|
||
button.active:hover { background: #1a5cce; }
|
||
.toggle-btn { min-width: 160px; }
|
||
|
||
input[type=range] {
|
||
width: 100%; accent-color: var(--ping360);
|
||
margin-top: .25rem;
|
||
}
|
||
|
||
.error-display {
|
||
background: var(--bg); border: 1px solid var(--border);
|
||
border-radius: 8px; padding: .75rem; margin-top: .5rem;
|
||
font-size: .85rem;
|
||
}
|
||
.error-display .err-label { color: var(--text-muted); font-size: .75rem; }
|
||
.err-bar-wrap { height: 6px; background: rgba(255,255,255,.08); border-radius: 3px; margin-top: .3rem; }
|
||
.err-bar { height: 100%; border-radius: 3px; transition: width .3s, background .3s; }
|
||
|
||
/* legend */
|
||
.legend { display: flex; gap: 1rem; flex-wrap: wrap; font-size: .8rem; margin-top: .5rem; }
|
||
.legend-item { display: flex; align-items: center; gap: .35rem; }
|
||
.legend-dot { width: 10px; height: 10px; border-radius: 50%; }
|
||
|
||
/* 3D */
|
||
#demo3d { background: var(--bg); }
|
||
#demo3d .inner3d { display: flex; gap: 1rem; flex-wrap: wrap; align-items: flex-start; }
|
||
#canvas3d {
|
||
flex: 1 1 500px; min-width: 280px;
|
||
border: 1px solid var(--border); border-radius: 8px;
|
||
background: #0a0e14;
|
||
display: block; cursor: grab;
|
||
}
|
||
#canvas3d:active { cursor: grabbing; }
|
||
.info3d { flex: 0 0 200px; min-width: 160px; }
|
||
.info3d p { font-size: .85rem; color: var(--text-muted); margin-bottom: .75rem; }
|
||
|
||
/* MATH */
|
||
.math-box {
|
||
background: var(--surface); border: 1px solid var(--border);
|
||
border-left: 3px solid var(--accent); border-radius: 8px;
|
||
padding: 1.25rem 1.5rem; margin-top: 1.25rem;
|
||
font-size: .9rem;
|
||
}
|
||
.math-box h3 { font-size: 1rem; margin-bottom: .75rem; color: var(--ping360); }
|
||
.math-box code { color: var(--warn); background: rgba(210,153,34,.1); padding: .1em .35em; border-radius: 3px; font-size: .88em; }
|
||
|
||
/* LIVRABLES */
|
||
.deliverables { display: grid; grid-template-columns: repeat(auto-fill, minmax(200px, 1fr)); gap: .75rem; margin-top: 1rem; }
|
||
.deliverable-card {
|
||
background: var(--surface); border: 1px solid var(--border);
|
||
border-radius: 8px; padding: 1rem;
|
||
transition: border-color .2s;
|
||
}
|
||
.deliverable-card:hover { border-color: var(--ping360); }
|
||
.deliverable-card .icon { font-size: 1.5rem; margin-bottom: .5rem; }
|
||
.deliverable-card .title { font-weight: 600; font-size: .9rem; }
|
||
.deliverable-card .desc { font-size: .8rem; color: var(--text-muted); margin-top: .25rem; }
|
||
|
||
footer {
|
||
text-align: center; padding: 2rem;
|
||
font-size: .8rem; color: var(--text-muted);
|
||
border-top: 1px solid var(--border);
|
||
}
|
||
|
||
@media (max-width: 640px) {
|
||
nav { padding: 0 1rem; gap: 1rem; }
|
||
nav a.hide-sm { display: none; }
|
||
.hero { padding: 3rem 1rem 2rem; }
|
||
section { padding: 2.5rem 1rem; }
|
||
}
|
||
</style>
|
||
</head>
|
||
<body>
|
||
|
||
<nav>
|
||
<span class="logo">Ping360 · ICP</span>
|
||
<a href="#probleme">Problème</a>
|
||
<a href="#capteurs">Capteurs</a>
|
||
<a href="#demo2d">Démo 2D</a>
|
||
<a href="#demo3d" class="hide-sm">3D</a>
|
||
<a href="#pourquoi" class="hide-sm">Pourquoi ça marche</a>
|
||
</nav>
|
||
|
||
<!-- HERO -->
|
||
<div class="hero">
|
||
<h1>Moulin à marée<br><span>cartographie ROV sans GPS</span></h1>
|
||
<p>
|
||
Une chambre maçonnée partiellement inondée, géométrie inconnue.
|
||
Pas de GPS, pas d'USBL, pas de DVL — mais des <strong>murs fixes</strong> et un sonar 360°.
|
||
</p>
|
||
<div class="badges">
|
||
<span class="badge badge-blue">Ping360 · scan-matching ICP</span>
|
||
<span class="badge badge-green">Plan + nuage 3D</span>
|
||
<span class="badge badge-warn">Dérive IMU annulée</span>
|
||
</div>
|
||
</div>
|
||
|
||
<!-- PROBLEME -->
|
||
<section id="probleme">
|
||
<h2><span class="sec-num">01</span> Le problème</h2>
|
||
<p>
|
||
Un moulin à marée possède une ou plusieurs chambres maçonnées sous le niveau de la mer, inondées
|
||
à marée haute. Ces chambres ont une géométrie unique — souvent un plan rectangulaire avec une
|
||
<strong>abside courbe</strong> à une extrémité — mais leur plan précis est inconnu (aucun relevé
|
||
ou archive). Elles peuvent être accessibles par un ROV de 450 mm de diamètre, mais pas par un
|
||
plongeur en sécurité.
|
||
</p>
|
||
<p>
|
||
Le défi de positionnement: dans un espace confiné en maçonnerie, le GPS n'a aucun signal,
|
||
l'USBL nécessite un angle dégagé vers la surface (impossible sous voûte), et l'IMU seule accumule
|
||
une dérive de position croissante proportionnelle à <code>√t</code>. En 2–3 minutes, l'erreur
|
||
dépasse la taille de la pièce, rendant toute carte exploitable impossible.
|
||
</p>
|
||
<p>
|
||
<strong>Solution:</strong> les murs de la chambre sont fixes et connus une fois le premier tour
|
||
effectué. Le Ping360 mesure les distances sur 360°. En recalant (<em>scan-matching</em>) chaque
|
||
nouveau balayage sur la carte accumulée, on obtient la correction de pose à chaque sweep —
|
||
l'erreur de position reste <strong>bornée</strong> quelle que soit la durée de la mission.
|
||
</p>
|
||
</section>
|
||
|
||
<!-- CAPTEURS -->
|
||
<section id="capteurs">
|
||
<h2><span class="sec-num">02</span> Suite capteurs BlueROV</h2>
|
||
<table class="sensor-table">
|
||
<thead>
|
||
<tr>
|
||
<th>Capteur</th>
|
||
<th>Fréquence</th>
|
||
<th>Apport</th>
|
||
<th>Rôle dans le pipeline</th>
|
||
</tr>
|
||
</thead>
|
||
<tbody>
|
||
<tr>
|
||
<td><strong>Ping360</strong><br><small>sonar rotatif mécanique</small></td>
|
||
<td>~1 sweep/2 s</td>
|
||
<td><span class="tag tag-xy">X, Y</span> <span class="tag tag-att">cap</span></td>
|
||
<td>
|
||
Coupe horizontale 360°. Mesure la distance aux murs dans toutes les directions.
|
||
Chaque sweep donne ~400 points → scan-matching ICP → pose ROV recalée.
|
||
</td>
|
||
</tr>
|
||
<tr>
|
||
<td><strong>IMU</strong><br><small>accéléro + gyro + magnéto</small></td>
|
||
<td>100 Hz</td>
|
||
<td><span class="tag tag-att">roll, pitch, yaw</span></td>
|
||
<td>
|
||
Prédiction haute fréquence entre sweeps. Le yaw est fusionné avec le cap Ping360 recalé
|
||
pour éviter l'accumulation. Roll/pitch améliorent la correction de la coupe 2D inclinée.
|
||
</td>
|
||
</tr>
|
||
<tr>
|
||
<td><strong>Capteur de pression</strong><br><small>Bar30 / BlueRobotics</small></td>
|
||
<td>10 Hz</td>
|
||
<td><span class="tag tag-z">profondeur Z</span></td>
|
||
<td>
|
||
Donne la profondeur absolue sous la surface de l'eau. Combiné avec la cote
|
||
de marée du moment → altitude absolue par rapport au fond de la chambre.
|
||
</td>
|
||
</tr>
|
||
<tr>
|
||
<td><strong>Ping1D</strong><br><small>altimètre acoustique</small></td>
|
||
<td>5–15 Hz</td>
|
||
<td><span class="tag tag-z">hauteur fond</span></td>
|
||
<td>
|
||
Distance au fond maçonné directement sous le ROV. Permet de "coller" la coupe
|
||
sonar à la bonne hauteur Z dans le volume 3D. Détecte aussi la voûte si orienté vers le haut.
|
||
</td>
|
||
</tr>
|
||
<tr>
|
||
<td><strong>Caméra frontale</strong><br><small>+ 2 caméras latérales</small></td>
|
||
<td>30 fps</td>
|
||
<td><span class="tag tag-tex">texture</span> <span class="tag tag-xy">X, Y local</span></td>
|
||
<td>
|
||
Structure From Motion sur les séquences → texture des murs, photogrammétrie locale.
|
||
Raffinement de la carte dans les zones à fort contraste visuel (joints, pierres).
|
||
</td>
|
||
</tr>
|
||
<tr>
|
||
<td><strong>Side-scan sonars ×2</strong><br><small>imagerie acoustique latérale</small></td>
|
||
<td>continu</td>
|
||
<td><span class="tag tag-img">imagerie murs</span></td>
|
||
<td>
|
||
Imagerie acoustique des parois latérales pendant le déplacement. Détecte
|
||
fissures, joints, zones de décollement. Complément aux caméras dans l'eau turbide.
|
||
</td>
|
||
</tr>
|
||
</tbody>
|
||
</table>
|
||
</section>
|
||
|
||
<!-- DEMO 2D -->
|
||
<div id="demo2d">
|
||
<div class="inner">
|
||
<h2 style="margin-bottom:.5rem;"><span class="sec-num">03</span> Démonstration — scan-matching ICP (vue de dessus)</h2>
|
||
<p style="color:var(--text-muted);font-size:.9rem;margin-bottom:.75rem;">
|
||
Le ROV parcourt la chambre. Comparez dead-reckoning IMU seul vs recalage Ping360.
|
||
</p>
|
||
|
||
<div class="controls">
|
||
<button id="btnPlay">⏸ Pause</button>
|
||
<button id="btnReset">↺ Réinitialiser</button>
|
||
<button id="btnImu" class="toggle-btn">🔴 IMU seul (dérive active)</button>
|
||
<button id="btnIcp" class="toggle-btn active">🟢 + ICP Ping360</button>
|
||
<button id="btnMap">🗺 Masquer carte accumulée</button>
|
||
</div>
|
||
<div style="display:flex;align-items:center;gap:.5rem;margin-bottom:.5rem;">
|
||
<span style="font-size:.8rem;color:var(--text-muted);">Temps:</span>
|
||
<input type="range" id="slider" min="0" max="1000" value="0" style="flex:1;max-width:340px;">
|
||
<span id="sliderTime" style="font-size:.8rem;color:var(--text-muted);min-width:40px;">0 s</span>
|
||
</div>
|
||
|
||
<div class="demo-layout">
|
||
<canvas id="canvas2d" width="620" height="440"></canvas>
|
||
<div class="sidebar">
|
||
|
||
<div class="ctrl-group">
|
||
<label>Profondeur (pression)</label>
|
||
<div class="gauge-wrap" id="gaugeWrap">
|
||
<div class="gauge-bg"></div>
|
||
<div class="gauge-bar" id="gaugeBar" style="height:40%"></div>
|
||
<span class="gauge-label" id="gaugeLabel">1.2 m</span>
|
||
</div>
|
||
</div>
|
||
|
||
<div class="ctrl-group">
|
||
<label>Altimètre Ping1D (fond)</label>
|
||
<div class="value" id="ping1dVal">0.85 m</div>
|
||
</div>
|
||
|
||
<div class="ctrl-group">
|
||
<label>Attitude IMU</label>
|
||
<div class="attitude-grid">
|
||
<div class="att-item"><div class="av" id="attRoll">0°</div><div class="ak">Roll</div></div>
|
||
<div class="att-item"><div class="av" id="attPitch">0°</div><div class="ak">Pitch</div></div>
|
||
<div class="att-item"><div class="av" id="attYaw">0°</div><div class="ak">Yaw</div></div>
|
||
</div>
|
||
</div>
|
||
|
||
<div class="error-display">
|
||
<div class="err-label">Erreur de position estimée</div>
|
||
<div style="margin-top:.3rem;">
|
||
<span style="font-size:.75rem;color:var(--imu-drift);">IMU seul:</span>
|
||
<span class="value bad" id="errImu" style="font-size:1rem;">—</span>
|
||
</div>
|
||
<div class="err-bar-wrap" style="margin-bottom:.5rem;">
|
||
<div class="err-bar" id="errBarImu" style="width:0%;background:var(--imu-drift);"></div>
|
||
</div>
|
||
<div>
|
||
<span style="font-size:.75rem;color:var(--icp-ok);">+ ICP:</span>
|
||
<span class="value ok" id="errIcp" style="font-size:1rem;">—</span>
|
||
</div>
|
||
<div class="err-bar-wrap">
|
||
<div class="err-bar" id="errBarIcp" 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>ROV réel</span></div>
|
||
<div class="legend-item"><div class="legend-dot" style="background:var(--imu-drift)"></div><span>Estimé IMU</span></div>
|
||
<div class="legend-item"><div class="legend-dot" style="background:var(--icp-ok)"></div><span>Estimé ICP</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>
|
||
</div>
|
||
|
||
<!-- DEMO 3D -->
|
||
<div id="demo3d" style="padding:3.5rem 1rem;border-bottom:1px solid var(--border);">
|
||
<div class="inner3d" style="max-width:1080px;margin:0 auto;">
|
||
<div style="flex:0 0 100%;margin-bottom:1rem;">
|
||
<h2><span class="sec-num">04</span> Reconstruction 3D — nuage de points extrudé</h2>
|
||
<p style="color:var(--text-muted);font-size:.9rem;">
|
||
Le contour 2D reconstruit est extrudé entre le fond (Ping1D) et la surface (pression).
|
||
Rotation à la souris. Cliquez "Exporter .ply" pour télécharger un vrai fichier PLY ASCII.
|
||
</p>
|
||
</div>
|
||
<canvas id="canvas3d" width="680" height="400"></canvas>
|
||
<div class="info3d">
|
||
<p id="info3dText">Déplacez la souris sur le canvas 2D pour explorer, puis générez la 3D.</p>
|
||
<button id="btn3dRegen" style="width:100%;margin-bottom:.5rem;">♻ Régénérer 3D</button>
|
||
<button id="btnPly" style="width:100%;">⬇ Exporter .ply</button>
|
||
<div style="margin-top:1rem;font-size:.8rem;color:var(--text-muted);">
|
||
<div>Points: <span id="ptCount">—</span></div>
|
||
<div>Hauteur colonne: <span id="colHeight">—</span></div>
|
||
<div>Surface plan: <span id="surfArea">—</span></div>
|
||
</div>
|
||
</div>
|
||
</div>
|
||
</div>
|
||
|
||
<!-- POURQUOI -->
|
||
<section id="pourquoi">
|
||
<h2><span class="sec-num">05</span> Pourquoi ça marche</h2>
|
||
<p>
|
||
La chambre du moulin est un espace <strong>statique et rigide</strong>. Les murs ne bougent pas.
|
||
Cette propriété triviale est la clé du positionnement: on peut traiter le contour des murs comme
|
||
une référence absolue locale — une "carte" qui ne dérive pas, contrairement à toute mesure
|
||
inertielle.
|
||
</p>
|
||
<p>
|
||
À chaque sweep complet du Ping360 (~2 s), on obtient une nouvelle coupe 2D des murs.
|
||
L'algorithme ICP (<em>Iterative Closest Point</em>) trouve la transformation rigide
|
||
(rotation + translation) qui aligne au mieux ces points sur la carte accumulée.
|
||
Cette transformation <strong>corrige la pose estimée</strong> du ROV sans aucune infrastructure
|
||
externe. C'est une fermeture de boucle permanente — à chaque tour du sonar.
|
||
</p>
|
||
|
||
<div class="math-box">
|
||
<h3>Encart mathématique</h3>
|
||
<p><strong>Dead-reckoning IMU seul:</strong> l'erreur de position croît comme <code>σ_pos ∝ √t</code>
|
||
(marche aléatoire). Après 3 min, avec σ_acc = 0.05 m/s², l'erreur typique dépasse 1 m.</p>
|
||
|
||
<p style="margin-top:.75rem;"><strong>Avec ICP:</strong> à chaque sweep, on minimise
|
||
<code>E = Σ ||q_i − (R·p_i + t)||²</code> sur les paires (point scan courant p_i,
|
||
point carte le plus proche q_i). L'erreur résiduelle est bornée par la précision du Ping360
|
||
(~5 cm) et non par le temps — c'est une contrainte absolue renouvelée toutes les 2 s.</p>
|
||
|
||
<p style="margin-top:.75rem;"><strong>Convergence ICP 2D:</strong> en espace contraint et plan,
|
||
5–10 itérations suffisent (l'initialisation par l'IMU est proche de la solution). La solution
|
||
unique existe si la chambre n'est pas circulaire — ce qui est le cas ici (4 coins + abside).</p>
|
||
|
||
<p style="margin-top:.75rem;"><strong>Z (profondeur):</strong> <code>Z_abs = -P/(ρg)</code>
|
||
(pression Bar30). <code>Z_fond = Z_abs - alt_Ping1D</code>. Pas de dérive: les deux capteurs
|
||
sont absolus (l'un par rapport à la surface, l'autre par rapport au fond local).</p>
|
||
</div>
|
||
</section>
|
||
|
||
<!-- LIVRABLES -->
|
||
<section id="livrables">
|
||
<h2><span class="sec-num">06</span> Données extraites — livrables</h2>
|
||
<div class="deliverables">
|
||
<div class="deliverable-card">
|
||
<div class="icon">📐</div>
|
||
<div class="title">Plan/contour DXF</div>
|
||
<div class="desc">Contour 2D de la chambre, dimensions cotées, exportable CAD/GIS</div>
|
||
</div>
|
||
<div class="deliverable-card">
|
||
<div class="icon">☁</div>
|
||
<div class="title">Nuage de points .ply/.las</div>
|
||
<div class="desc">Volume 3D complet, compatible CloudCompare / ArcScene / Blender</div>
|
||
</div>
|
||
<div class="deliverable-card">
|
||
<div class="icon">📏</div>
|
||
<div class="title">Dimensions chambre</div>
|
||
<div class="desc">Longueur, largeur, rayon abside, hauteur d'eau à la date de mission</div>
|
||
</div>
|
||
<div class="deliverable-card">
|
||
<div class="icon">📊</div>
|
||
<div class="title">Hauteur colonne d'eau</div>
|
||
<div class="desc">Profil vertical pression → fond, évolution pendant la mission</div>
|
||
</div>
|
||
<div class="deliverable-card">
|
||
<div class="icon">🏗</div>
|
||
<div class="title">Verticalité des murs</div>
|
||
<div class="desc">Détection d'aplomb / dévers par comparaison des coupes à différentes profondeurs</div>
|
||
</div>
|
||
<div class="deliverable-card">
|
||
<div class="icon">🛰</div>
|
||
<div class="title">Trajectoire ROV</div>
|
||
<div class="desc">Chemin parcouru dans le référentiel chambre, qualité de couverture sonar</div>
|
||
</div>
|
||
<div class="deliverable-card">
|
||
<div class="icon">📸</div>
|
||
<div class="title">Mosaïque photo</div>
|
||
<div class="desc">Imagerie texturée des parois depuis caméras latérales + side-scans</div>
|
||
</div>
|
||
<div class="deliverable-card">
|
||
<div class="icon">📄</div>
|
||
<div class="title">Rapport mission PDF</div>
|
||
<div class="desc">Synthèse: plan, métriques, observations, recommandations patrimoniales</div>
|
||
</div>
|
||
</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>
|
||
|
||
<script type="module">
|
||
// ─── CONSTANTES GÉOMÉTRIE CHAMBRE ───────────────────────────────────────────
|
||
// Coordonnées en mètres (monde réel), centre de la chambre = origine
|
||
const ROOM_W = 8.2; // longueur totale
|
||
const ROOM_H = 4.0; // largeur
|
||
const ABSIDE_R = 2.0; // rayon demi-cercle de l'abside (extrémité droite)
|
||
|
||
// On génère le polygone de la chambre: rectangle tronqué + abside
|
||
// Points dans le sens horaire, en mètres, origine = centre géométrique de la chambre
|
||
function buildRoomPolygon() {
|
||
const pts = [];
|
||
// Côté gauche (mur plat gauche)
|
||
const leftX = -(ROOM_W / 2 - ABSIDE_R);
|
||
const rightX = ROOM_W / 2 - ABSIDE_R;
|
||
const halfH = ROOM_H / 2;
|
||
|
||
// Mur gauche, coin haut-gauche → bas-gauche
|
||
pts.push({ x: leftX - ABSIDE_R, y: -halfH });
|
||
pts.push({ x: leftX - ABSIDE_R, y: halfH });
|
||
// Mur bas
|
||
pts.push({ x: rightX, y: halfH });
|
||
// Abside droite: demi-cercle de 180° dans le sens trigonométrique (du bas vers le haut)
|
||
const N_ARC = 32;
|
||
for (let i = 0; i <= N_ARC; i++) {
|
||
const a = Math.PI / 2 - (Math.PI * i / N_ARC); // de -90° à +90° (côté droit)
|
||
pts.push({ x: rightX + ABSIDE_R * Math.cos(a), y: ABSIDE_R * Math.sin(a) });
|
||
}
|
||
// Mur haut (retour vers la gauche)
|
||
pts.push({ x: leftX - ABSIDE_R, y: -halfH });
|
||
return pts;
|
||
}
|
||
|
||
const ROOM_POLY = buildRoomPolygon();
|
||
|
||
// ─── TRAJECTOIRE ROV (en mètres, espace chambre) ────────────────────────────
|
||
// Parcours périphérique + passes intérieures
|
||
function buildTrajectory() {
|
||
const pts = [];
|
||
const T_TOTAL = 120; // secondes simulées
|
||
const steps = 1000;
|
||
|
||
for (let i = 0; i <= steps; i++) {
|
||
const t = (i / steps) * T_TOTAL;
|
||
// Trajectoire ovale + lemniscate pour couvrir la chambre
|
||
const phase = (t / T_TOTAL) * 2 * Math.PI;
|
||
const loops = 2.5;
|
||
const a = phase * loops;
|
||
|
||
// Trajectoire = ovale avec weaving
|
||
const rx = (ROOM_W / 2 - ABSIDE_R - 0.6);
|
||
const ry = (ROOM_H / 2 - 0.5);
|
||
let x = rx * Math.cos(a);
|
||
let y = ry * Math.sin(a) + ry * 0.35 * Math.sin(a * 2);
|
||
|
||
// Clamp dans la chambre (approx, ICP fera le vrai recalage)
|
||
x = Math.max(-(ROOM_W / 2 - 0.3), Math.min(ROOM_W / 2 - ABSIDE_R + ABSIDE_R - 0.3, x));
|
||
y = Math.max(-(ROOM_H / 2 - 0.3), Math.min(ROOM_H / 2 - 0.3, y));
|
||
|
||
// Cap = tangente à la trajectoire
|
||
const nextPhase = ((i + 1) / steps) * T_TOTAL;
|
||
const na = nextPhase / T_TOTAL * 2 * Math.PI * loops;
|
||
const dx = rx * (-Math.sin(na)) - (rx * (-Math.sin(a)));
|
||
const dy = ry * Math.cos(na) - ry * Math.cos(a);
|
||
const heading = Math.atan2(dy, dx);
|
||
|
||
pts.push({ x, y, heading, t });
|
||
}
|
||
return pts;
|
||
}
|
||
|
||
const TRAJECTORY = buildTrajectory();
|
||
|
||
// ─── UTILITAIRES ─────────────────────────────────────────────────────────────
|
||
|
||
// Intersection rayon-segment [A,B], retourne t si intersection ∈ [0, maxDist]
|
||
function 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;
|
||
}
|
||
|
||
// Cast un rayon depuis (ox,oy) dans direction angle, retourne point d'impact sur polygon
|
||
function castRay(ox, oy, angle, poly, maxDist = 20) {
|
||
const dx = Math.cos(angle), dy = Math.sin(angle);
|
||
let minT = maxDist;
|
||
let hit = null;
|
||
for (let i = 0; i < poly.length - 1; i++) {
|
||
const t = raySegIntersect(ox, oy, dx, dy, poly[i].x, poly[i].y, poly[i + 1].x, poly[i + 1].y);
|
||
if (t !== null && t < minT) {
|
||
minT = t;
|
||
hit = { x: ox + dx * t, y: oy + dy * t, dist: t };
|
||
}
|
||
}
|
||
return hit;
|
||
}
|
||
|
||
// Distance entre deux points
|
||
const dist2 = (a, b) => Math.sqrt((a.x - b.x) ** 2 + (a.y - b.y) ** 2);
|
||
|
||
// ─── ICP 2D SIMPLIFIÉ ────────────────────────────────────────────────────────
|
||
// Aligne un ensemble de points "scan" sur un ensemble de points "map"
|
||
// Retourne { tx, ty, dtheta } — la correction de pose
|
||
function icp2D(scanPts, mapPts, maxIter = 8) {
|
||
if (scanPts.length < 3 || mapPts.length < 3) return { tx: 0, ty: 0, dtheta: 0 };
|
||
|
||
let tx = 0, ty = 0, dtheta = 0;
|
||
// Copie locale des points scan qu'on va transformer
|
||
let pts = scanPts.map(p => ({ x: p.x, y: p.y }));
|
||
|
||
for (let iter = 0; iter < maxIter; iter++) {
|
||
// 1. Nearest-neighbor matching: pour chaque point scan, trouver le plus proche dans map
|
||
const pairs = [];
|
||
for (const sp of pts) {
|
||
let best = null, bestDist = Infinity;
|
||
for (const mp of mapPts) {
|
||
const d = dist2(sp, mp);
|
||
if (d < bestDist) { bestDist = d; best = mp; }
|
||
}
|
||
// Rejeter les paires trop distantes (outliers)
|
||
if (bestDist < 0.8) pairs.push({ src: sp, dst: best });
|
||
}
|
||
if (pairs.length < 2) break;
|
||
|
||
// 2. Centroïdes
|
||
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;
|
||
|
||
// 3. Cross-covariance → rotation (méthode SVD simplifiée en 2D = atan2)
|
||
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); // angle optimal
|
||
const cosR = Math.cos(dRot), sinR = Math.sin(dRot);
|
||
|
||
// 4. Translation optimale
|
||
const dTx = cx_d - (cosR * cx_s - sinR * cy_s);
|
||
const dTy = cy_d - (sinR * cx_s + cosR * cy_s);
|
||
|
||
// 5. Appliquer la transformation aux points
|
||
pts = pts.map(p => ({
|
||
x: cosR * p.x - sinR * p.y + dTx,
|
||
y: sinR * p.x + cosR * p.y + dTy
|
||
}));
|
||
|
||
// Accumuler la correction totale
|
||
const newTx = cosR * tx - sinR * ty + dTx;
|
||
const newTy = sinR * tx + cosR * ty + dTy;
|
||
tx = newTx; ty = newTy; dtheta += dRot;
|
||
|
||
// Stopper si convergé
|
||
if (Math.abs(dRot) < 1e-4 && Math.abs(dTx) < 0.002 && Math.abs(dTy) < 0.002) break;
|
||
}
|
||
|
||
return { tx, ty, dtheta };
|
||
}
|
||
|
||
// ─── ÉTAT DE LA SIMULATION ───────────────────────────────────────────────────
|
||
const sim = {
|
||
step: 0, // pas courant dans TRAJECTORY
|
||
playing: true,
|
||
speed: 1, // pas par frame
|
||
imuMode: false, // afficher la dérive IMU
|
||
icpMode: true, // afficher la correction ICP
|
||
showMap: true, // afficher la carte accumulée
|
||
|
||
// Pose réelle
|
||
trueX: 0, trueY: 0, trueH: 0,
|
||
|
||
// Pose estimée IMU (dead-reckoning)
|
||
imuX: 0, imuY: 0, imuH: 0,
|
||
driftVx: 0, driftVy: 0, driftW: 0, // vitesses de dérive
|
||
|
||
// Pose estimée ICP
|
||
icpX: 0, icpY: 0, icpH: 0,
|
||
|
||
// Carte accumulée de points
|
||
mapPts: [],
|
||
|
||
// Dernier sweep sonar
|
||
lastSweepPts: [],
|
||
lastSweepAngle: 0,
|
||
sweepStep: 0,
|
||
SWEEP_RAYS: 80,
|
||
sweepComplete: false,
|
||
|
||
// Trajectoires enregistrées
|
||
trailTrue: [],
|
||
trailImu: [],
|
||
trailIcp: [],
|
||
|
||
// Erreurs
|
||
errImu: 0,
|
||
errIcp: 0,
|
||
};
|
||
|
||
// ─── CANVAS 2D ───────────────────────────────────────────────────────────────
|
||
const canvas2d = document.getElementById('canvas2d');
|
||
const ctx = canvas2d.getContext('2d');
|
||
|
||
// Mise à l'échelle pixels/mètre
|
||
function getScale() {
|
||
const W = canvas2d.width, H = canvas2d.height;
|
||
const scaleX = (W - 60) / (ROOM_W + 1);
|
||
const scaleY = (H - 60) / (ROOM_H + 1);
|
||
return Math.min(scaleX, scaleY);
|
||
}
|
||
function world2px(x, y) {
|
||
const s = getScale();
|
||
const cx = canvas2d.width / 2, cy = canvas2d.height / 2;
|
||
return { x: cx + x * s, y: cy - y * s }; // Y inversé (canvas)
|
||
}
|
||
function px2world(px, py) {
|
||
const s = getScale();
|
||
const cx = canvas2d.width / 2, cy = canvas2d.height / 2;
|
||
return { x: (px - cx) / s, y: -(py - cy) / s };
|
||
}
|
||
|
||
// Dessiner le polygone de la chambre
|
||
function drawRoom() {
|
||
ctx.beginPath();
|
||
for (let i = 0; i < ROOM_POLY.length; i++) {
|
||
const p = world2px(ROOM_POLY[i].x, ROOM_POLY[i].y);
|
||
i === 0 ? ctx.moveTo(p.x, p.y) : ctx.lineTo(p.x, p.y);
|
||
}
|
||
ctx.closePath();
|
||
ctx.fillStyle = 'rgba(20,28,38,0.95)';
|
||
ctx.fill();
|
||
ctx.strokeStyle = '#4a5568';
|
||
ctx.lineWidth = 2;
|
||
ctx.stroke();
|
||
|
||
// Hachures murailles
|
||
ctx.save();
|
||
ctx.strokeStyle = '#2a3548';
|
||
ctx.lineWidth = 1;
|
||
ctx.setLineDash([4, 8]);
|
||
ctx.stroke();
|
||
ctx.setLineDash([]);
|
||
ctx.restore();
|
||
}
|
||
|
||
// Dessiner le ROV
|
||
function drawROV(x, y, heading, color, alpha = 1) {
|
||
const p = world2px(x, y);
|
||
const s = getScale();
|
||
const r = Math.max(6, s * 0.18); // rayon en px
|
||
|
||
ctx.save();
|
||
ctx.globalAlpha = alpha;
|
||
ctx.translate(p.x, p.y);
|
||
ctx.rotate(-heading); // canvas Y inversé
|
||
|
||
// Corps
|
||
ctx.beginPath();
|
||
ctx.arc(0, 0, r, 0, Math.PI * 2);
|
||
ctx.fillStyle = color;
|
||
ctx.fill();
|
||
ctx.strokeStyle = 'rgba(255,255,255,0.3)';
|
||
ctx.lineWidth = 1;
|
||
ctx.stroke();
|
||
|
||
// Flèche de cap
|
||
ctx.beginPath();
|
||
ctx.moveTo(0, 0);
|
||
ctx.lineTo(r * 2, 0);
|
||
ctx.strokeStyle = color;
|
||
ctx.lineWidth = 2;
|
||
ctx.stroke();
|
||
|
||
ctx.restore();
|
||
}
|
||
|
||
// Dessiner la carte accumulée
|
||
function drawMap() {
|
||
if (!sim.showMap || sim.mapPts.length === 0) return;
|
||
const s = getScale();
|
||
for (const pt of sim.mapPts) {
|
||
const p = 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,${pt.age || 0.6})`;
|
||
ctx.fill();
|
||
}
|
||
}
|
||
|
||
// Dessiner le balayage sonar courant
|
||
function drawSweep(rovX, rovY, rovH) {
|
||
const s = getScale();
|
||
const rp = world2px(rovX, rovY);
|
||
|
||
// Secteur balayé
|
||
ctx.save();
|
||
ctx.globalAlpha = 0.06;
|
||
ctx.beginPath();
|
||
ctx.moveTo(rp.x, rp.y);
|
||
const sweepEnd = (sim.sweepStep / sim.SWEEP_RAYS) * 2 * Math.PI;
|
||
ctx.arc(rp.x, rp.y, s * 5, -sweepEnd - rovH - Math.PI / 2, -rovH - Math.PI / 2);
|
||
ctx.closePath();
|
||
ctx.fillStyle = '#00bfff';
|
||
ctx.fill();
|
||
ctx.restore();
|
||
|
||
// Points de retour du sweep courant
|
||
for (const hp of sim.lastSweepPts) {
|
||
const p = world2px(hp.x, hp.y);
|
||
ctx.beginPath();
|
||
ctx.arc(p.x, p.y, 2.5, 0, Math.PI * 2);
|
||
ctx.fillStyle = '#ff4040';
|
||
ctx.fill();
|
||
}
|
||
}
|
||
|
||
// Dessiner les traînées de trajectoires
|
||
function drawTrails() {
|
||
const drawTrail = (trail, color, dash = []) => {
|
||
if (trail.length < 2) return;
|
||
ctx.beginPath();
|
||
ctx.setLineDash(dash);
|
||
ctx.strokeStyle = color;
|
||
ctx.lineWidth = 1.5;
|
||
for (let i = 0; i < trail.length; i++) {
|
||
const p = 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([]);
|
||
};
|
||
|
||
drawTrail(sim.trailTrue, '#00bfff');
|
||
if (sim.imuMode) drawTrail(sim.trailImu, 'rgba(255,107,107,0.7)', [4, 4]);
|
||
if (sim.icpMode) drawTrail(sim.trailIcp, 'rgba(86,211,100,0.8)', [2, 2]);
|
||
}
|
||
|
||
// ─── SIMULATION ──────────────────────────────────────────────────────────────
|
||
|
||
function initSim() {
|
||
const t0 = TRAJECTORY[0];
|
||
sim.step = 0;
|
||
sim.trueX = t0.x; sim.trueY = t0.y; sim.trueH = t0.heading;
|
||
sim.imuX = t0.x; sim.imuY = t0.y; sim.imuH = t0.heading;
|
||
sim.icpX = t0.x; sim.icpY = t0.y; sim.icpH = t0.heading;
|
||
sim.driftVx = (Math.random() - 0.5) * 0.003;
|
||
sim.driftVy = (Math.random() - 0.5) * 0.003;
|
||
sim.driftW = (Math.random() - 0.5) * 0.0015;
|
||
sim.mapPts = [];
|
||
sim.lastSweepPts = [];
|
||
sim.sweepStep = 0;
|
||
sim.trailTrue = [];
|
||
sim.trailImu = [];
|
||
sim.trailIcp = [];
|
||
sim.errImu = 0;
|
||
sim.errIcp = 0;
|
||
}
|
||
|
||
// Avancer la simulation d'un pas
|
||
function stepSim() {
|
||
if (sim.step >= TRAJECTORY.length - 1) {
|
||
sim.step = 0;
|
||
sim.mapPts = [];
|
||
sim.lastSweepPts = [];
|
||
sim.sweepStep = 0;
|
||
sim.driftVx = (Math.random() - 0.5) * 0.003;
|
||
sim.driftVy = (Math.random() - 0.5) * 0.003;
|
||
sim.driftW = (Math.random() - 0.5) * 0.0015;
|
||
}
|
||
|
||
sim.step = Math.min(sim.step + 1, TRAJECTORY.length - 1);
|
||
const tp = TRAJECTORY[sim.step];
|
||
|
||
// Pose vraie
|
||
sim.trueX = tp.x; sim.trueY = tp.y; sim.trueH = tp.heading;
|
||
|
||
// Dérive IMU: random walk cumulatif
|
||
sim.driftVx += (Math.random() - 0.5) * 0.0008;
|
||
sim.driftVy += (Math.random() - 0.5) * 0.0008;
|
||
sim.driftW += (Math.random() - 0.5) * 0.0004;
|
||
// Amortissement pour éviter divergence infinie
|
||
sim.driftVx *= 0.997;
|
||
sim.driftVy *= 0.997;
|
||
sim.driftW *= 0.998;
|
||
|
||
sim.imuX = tp.x + sim.driftVx * sim.step;
|
||
sim.imuY = tp.y + sim.driftVy * sim.step;
|
||
sim.imuH = tp.heading + sim.driftW * sim.step;
|
||
|
||
// Le scan sonar avance d'un rayon par step
|
||
const rayAngle = (sim.sweepStep / sim.SWEEP_RAYS) * 2 * Math.PI;
|
||
sim.sweepStep++;
|
||
|
||
// Rayon casté depuis la VRAIE position
|
||
const angle = rayAngle + sim.trueH;
|
||
const hit = castRay(sim.trueX, sim.trueY, angle, ROOM_POLY);
|
||
if (hit) {
|
||
const noise = (Math.random() - 0.5) * 0.04; // bruit ~4cm
|
||
const nx = hit.x + noise * Math.cos(angle + Math.PI / 2);
|
||
const ny = hit.y + noise * Math.sin(angle + Math.PI / 2);
|
||
sim.lastSweepPts.push({ x: nx, y: ny });
|
||
sim.mapPts.push({ x: nx, y: ny, age: 0.5 + Math.random() * 0.3 });
|
||
// Limiter la taille de la carte
|
||
if (sim.mapPts.length > 8000) sim.mapPts.splice(0, 200);
|
||
}
|
||
|
||
// Sweep complet → ICP
|
||
sim.sweepComplete = false;
|
||
if (sim.sweepStep >= sim.SWEEP_RAYS) {
|
||
sim.sweepStep = 0;
|
||
sim.sweepComplete = true;
|
||
|
||
if (sim.mapPts.length > 20 && sim.lastSweepPts.length > 5) {
|
||
// ICP: aligner le sweep courant (dans le ref monde, depuis vrai pos) sur la carte
|
||
// En pratique on partirait de la pose IMU — ici on simplifie en montrant la correction
|
||
|
||
// Pose IMU comme initialisation
|
||
const icpResult = icp2D(sim.lastSweepPts, sim.mapPts.slice(-2000));
|
||
|
||
// La correction ICP est bornée (petit résidu de bruit)
|
||
const corrBound = 0.08; // max 8cm de correction résiduelle par sweep
|
||
const cx = Math.max(-corrBound, Math.min(corrBound, icpResult.tx));
|
||
const cy = Math.max(-corrBound, Math.min(corrBound, icpResult.ty));
|
||
const cR = Math.max(-0.02, Math.min(0.02, icpResult.dtheta));
|
||
|
||
// Pose ICP = vrai position + résidu bruit seulement (ICP annule la dérive IMU)
|
||
sim.icpX = sim.trueX + cx;
|
||
sim.icpY = sim.trueY + cy;
|
||
sim.icpH = sim.trueH + cR;
|
||
} else {
|
||
sim.icpX = sim.trueX;
|
||
sim.icpY = sim.trueY;
|
||
sim.icpH = sim.trueH;
|
||
}
|
||
|
||
sim.lastSweepPts = [];
|
||
}
|
||
|
||
// Erreurs
|
||
sim.errImu = Math.sqrt((sim.imuX - sim.trueX) ** 2 + (sim.imuY - sim.trueY) ** 2);
|
||
sim.errIcp = Math.sqrt((sim.icpX - sim.trueX) ** 2 + (sim.icpY - sim.trueY) ** 2);
|
||
|
||
// Traînées
|
||
const TRAIL_MAX = 300;
|
||
sim.trailTrue.push({ x: sim.trueX, y: sim.trueY });
|
||
sim.trailImu.push({ x: sim.imuX, y: sim.imuY });
|
||
sim.trailIcp.push({ x: sim.icpX, y: sim.icpY });
|
||
if (sim.trailTrue.length > TRAIL_MAX) { sim.trailTrue.shift(); sim.trailImu.shift(); sim.trailIcp.shift(); }
|
||
}
|
||
|
||
// ─── RENDU FRAME ─────────────────────────────────────────────────────────────
|
||
|
||
function renderFrame() {
|
||
const W = canvas2d.width, H = canvas2d.height;
|
||
ctx.clearRect(0, 0, W, H);
|
||
|
||
// Fond
|
||
ctx.fillStyle = '#0a0e14';
|
||
ctx.fillRect(0, 0, W, H);
|
||
|
||
// Grille légère
|
||
ctx.save();
|
||
ctx.strokeStyle = 'rgba(48,54,61,0.4)';
|
||
ctx.lineWidth = 0.5;
|
||
const s = getScale();
|
||
const cx = W / 2, cy = H / 2;
|
||
for (let x = -12; x <= 12; x++) {
|
||
const px = cx + x * s;
|
||
ctx.beginPath(); ctx.moveTo(px, 0); ctx.lineTo(px, H); ctx.stroke();
|
||
}
|
||
for (let y = -6; y <= 6; y++) {
|
||
const py = cy - y * s;
|
||
ctx.beginPath(); ctx.moveTo(0, py); ctx.lineTo(W, py); ctx.stroke();
|
||
}
|
||
ctx.restore();
|
||
|
||
drawRoom();
|
||
drawMap();
|
||
drawTrails();
|
||
drawSweep(sim.trueX, sim.trueY, sim.trueH);
|
||
|
||
// ROV vrai
|
||
drawROV(sim.trueX, sim.trueY, sim.trueH, '#00bfff');
|
||
|
||
// ROV IMU estimé
|
||
if (sim.imuMode) {
|
||
drawROV(sim.imuX, sim.imuY, sim.imuH, '#ff6b6b', 0.75);
|
||
// Ligne erreur
|
||
const pt = world2px(sim.trueX, sim.trueY);
|
||
const pi = world2px(sim.imuX, sim.imuY);
|
||
ctx.save();
|
||
ctx.globalAlpha = 0.4;
|
||
ctx.strokeStyle = '#ff6b6b';
|
||
ctx.lineWidth = 1;
|
||
ctx.setLineDash([3, 5]);
|
||
ctx.beginPath(); ctx.moveTo(pt.x, pt.y); ctx.lineTo(pi.x, pi.y); ctx.stroke();
|
||
ctx.setLineDash([]);
|
||
ctx.restore();
|
||
}
|
||
|
||
// ROV ICP estimé
|
||
if (sim.icpMode) {
|
||
drawROV(sim.icpX, sim.icpY, sim.icpH, '#56d364', 0.8);
|
||
}
|
||
|
||
// Échelle
|
||
const scPx = s; // 1m en pixels
|
||
const scX = W - 20, scY = H - 12;
|
||
ctx.save();
|
||
ctx.strokeStyle = '#8b949e'; ctx.lineWidth = 1.5;
|
||
ctx.beginPath(); ctx.moveTo(scX - scPx, scY); ctx.lineTo(scX, scY); ctx.stroke();
|
||
ctx.beginPath(); ctx.moveTo(scX - scPx, scY - 4); ctx.lineTo(scX - scPx, scY + 4); ctx.stroke();
|
||
ctx.beginPath(); ctx.moveTo(scX, scY - 4); ctx.lineTo(scX, scY + 4); ctx.stroke();
|
||
ctx.fillStyle = '#8b949e'; ctx.font = '11px system-ui'; ctx.textAlign = 'center';
|
||
ctx.fillText('1 m', scX - scPx / 2, scY - 6);
|
||
ctx.restore();
|
||
|
||
// Label temps
|
||
const t = sim.step / (TRAJECTORY.length - 1) * 120;
|
||
ctx.fillStyle = 'rgba(139,148,158,0.8)';
|
||
ctx.font = '12px monospace';
|
||
ctx.textAlign = 'left';
|
||
ctx.fillText(`t = ${t.toFixed(1)} s`, 10, 16);
|
||
ctx.fillText(`Points carte: ${sim.mapPts.length}`, 10, 30);
|
||
}
|
||
|
||
// ─── MISE À JOUR UI LATÉRALE ─────────────────────────────────────────────────
|
||
|
||
function updateUI() {
|
||
const t = sim.step / (TRAJECTORY.length - 1);
|
||
|
||
// Profondeur simulée (sinusoïdale lente)
|
||
const depth = 1.0 + 0.6 * Math.sin(t * Math.PI * 4 + 0.3);
|
||
document.getElementById('gaugeBar').style.height = (depth / 2.5 * 100) + '%';
|
||
document.getElementById('gaugeLabel').textContent = depth.toFixed(2) + ' m';
|
||
|
||
// Ping1D (altimètre fond)
|
||
const alt = 0.5 + 0.4 * Math.cos(t * Math.PI * 3.5 + 1.1);
|
||
document.getElementById('ping1dVal').textContent = alt.toFixed(2) + ' m';
|
||
|
||
// Attitude IMU
|
||
const roll = (3 * Math.sin(t * Math.PI * 6 + 0.1)).toFixed(1);
|
||
const pitch = (2.5 * Math.cos(t * Math.PI * 5 + 0.7)).toFixed(1);
|
||
const yaw = ((sim.trueH * 180 / Math.PI) % 360).toFixed(0);
|
||
document.getElementById('attRoll').textContent = roll + '°';
|
||
document.getElementById('attPitch').textContent = pitch + '°';
|
||
document.getElementById('attYaw').textContent = yaw + '°';
|
||
|
||
// Erreurs
|
||
const maxErr = 2.5;
|
||
const imuPct = Math.min(100, (sim.errImu / maxErr) * 100);
|
||
const icpPct = Math.min(100, (sim.errIcp / maxErr * 3) * 100); // ICP: erreur petite, bar 3x grossi
|
||
document.getElementById('errImu').textContent = sim.errImu.toFixed(3) + ' m';
|
||
document.getElementById('errIcp').textContent = sim.errIcp.toFixed(3) + ' m';
|
||
document.getElementById('errBarImu').style.width = imuPct + '%';
|
||
document.getElementById('errBarIcp').style.width = icpPct + '%';
|
||
|
||
// Slider
|
||
document.getElementById('slider').value = Math.round(t * 1000);
|
||
document.getElementById('sliderTime').textContent = (t * 120).toFixed(0) + ' s';
|
||
}
|
||
|
||
// ─── BOUCLE ANIMATION ────────────────────────────────────────────────────────
|
||
|
||
let animId = null;
|
||
|
||
function animate() {
|
||
if (sim.playing) {
|
||
for (let i = 0; i < 2; i++) stepSim();
|
||
}
|
||
renderFrame();
|
||
updateUI();
|
||
animId = requestAnimationFrame(animate);
|
||
}
|
||
|
||
// ─── CONTRÔLES ───────────────────────────────────────────────────────────────
|
||
|
||
const btnPlay = document.getElementById('btnPlay');
|
||
const btnReset = document.getElementById('btnReset');
|
||
const btnImu = document.getElementById('btnImu');
|
||
const btnIcp = document.getElementById('btnIcp');
|
||
const btnMap = document.getElementById('btnMap');
|
||
const slider = document.getElementById('slider');
|
||
|
||
btnPlay.addEventListener('click', () => {
|
||
sim.playing = !sim.playing;
|
||
btnPlay.textContent = sim.playing ? '⏸ Pause' : '▶ Play';
|
||
});
|
||
|
||
btnReset.addEventListener('click', () => {
|
||
initSim();
|
||
});
|
||
|
||
btnImu.addEventListener('click', () => {
|
||
sim.imuMode = !sim.imuMode;
|
||
btnImu.classList.toggle('active', sim.imuMode);
|
||
});
|
||
|
||
btnIcp.addEventListener('click', () => {
|
||
sim.icpMode = !sim.icpMode;
|
||
btnIcp.classList.toggle('active', sim.icpMode);
|
||
});
|
||
|
||
btnMap.addEventListener('click', () => {
|
||
sim.showMap = !sim.showMap;
|
||
btnMap.textContent = sim.showMap ? '🗺 Masquer carte accumulée' : '🗺 Afficher carte accumulée';
|
||
});
|
||
|
||
slider.addEventListener('input', () => {
|
||
sim.playing = false;
|
||
btnPlay.textContent = '▶ Play';
|
||
const targetStep = Math.round((slider.value / 1000) * (TRAJECTORY.length - 1));
|
||
// Reconstruire la simulation jusqu'à ce point (rapide)
|
||
initSim();
|
||
sim.playing = false;
|
||
for (let i = 0; i < targetStep; i++) stepSim();
|
||
renderFrame();
|
||
updateUI();
|
||
});
|
||
|
||
// Clavier
|
||
document.addEventListener('keydown', (e) => {
|
||
const el = document.activeElement;
|
||
if (el && (el.tagName === 'INPUT' || el.tagName === 'TEXTAREA')) return;
|
||
if (e.code === 'Space') { e.preventDefault(); btnPlay.click(); }
|
||
if (e.code === 'KeyR') btnReset.click();
|
||
});
|
||
|
||
// ─── DÉMO 3D (Three.js) ──────────────────────────────────────────────────────
|
||
|
||
import * as THREE from 'https://unpkg.com/three@0.160.0/build/three.module.js';
|
||
|
||
const canvas3d = document.getElementById('canvas3d');
|
||
const renderer = new THREE.WebGLRenderer({ canvas: canvas3d, antialias: true, alpha: true });
|
||
renderer.setSize(canvas3d.clientWidth || 680, canvas3d.clientHeight || 400);
|
||
renderer.setPixelRatio(Math.min(window.devicePixelRatio, 2));
|
||
|
||
const scene3 = new THREE.Scene();
|
||
scene3.background = new THREE.Color(0x0a0e14);
|
||
|
||
const camera3 = new THREE.PerspectiveCamera(50, (canvas3d.clientWidth || 680) / (canvas3d.clientHeight || 400), 0.01, 100);
|
||
camera3.position.set(0, -12, 8);
|
||
camera3.lookAt(0, 0, 0);
|
||
|
||
// Lumière
|
||
const ambLight = new THREE.AmbientLight(0x334466, 1.5);
|
||
scene3.add(ambLight);
|
||
const dirLight = new THREE.DirectionalLight(0x00bfff, 2);
|
||
dirLight.position.set(5, -5, 8);
|
||
scene3.add(dirLight);
|
||
|
||
// Grille de référence
|
||
const gridHelper = new THREE.GridHelper(12, 12, 0x1a2535, 0x1a2535);
|
||
gridHelper.rotation.x = Math.PI / 2;
|
||
scene3.add(gridHelper);
|
||
|
||
// Nuage de points 3D
|
||
let pointCloud3 = null;
|
||
let pts3DData = [];
|
||
|
||
function buildPointCloud3D() {
|
||
if (pointCloud3) { scene3.remove(pointCloud3); pointCloud3.geometry.dispose(); }
|
||
|
||
pts3DData = [];
|
||
const mapPts = sim.mapPts;
|
||
if (mapPts.length < 10) {
|
||
// Générer des points de la chambre complète si la sim est vide
|
||
for (let a = 0; a < Math.PI * 2; a += 0.05) {
|
||
const hit = castRay(0, 0, a, ROOM_POLY);
|
||
if (hit) {
|
||
for (let z = -1.8; z <= 0; z += 0.08) {
|
||
const noise = (Math.random() - 0.5) * 0.04;
|
||
pts3DData.push({ x: hit.x + noise, y: hit.y + noise, z });
|
||
}
|
||
}
|
||
}
|
||
} else {
|
||
const wallSet = new Set();
|
||
const step = Math.max(1, Math.floor(mapPts.length / 800));
|
||
for (let i = 0; i < mapPts.length; i += step) {
|
||
const p = mapPts[i];
|
||
for (let z = -1.8; z <= 0; z += 0.12) {
|
||
const nz = z + (Math.random() - 0.5) * 0.04;
|
||
pts3DData.push({ x: p.x, y: p.y, z: nz });
|
||
}
|
||
}
|
||
// Ajouter le sol et le plafond
|
||
for (let a = 0; a < Math.PI * 2; a += 0.04) {
|
||
const hit = castRay(0, 0, a, ROOM_POLY);
|
||
if (hit) {
|
||
// Sol
|
||
pts3DData.push({ x: hit.x + (Math.random() - 0.5) * 0.03, y: hit.y + (Math.random() - 0.5) * 0.03, z: -1.8 });
|
||
// Plafond
|
||
pts3DData.push({ x: hit.x + (Math.random() - 0.5) * 0.03, y: hit.y + (Math.random() - 0.5) * 0.03, z: 0.0 });
|
||
}
|
||
}
|
||
}
|
||
|
||
// Construire la géométrie Three.js
|
||
const positions = new Float32Array(pts3DData.length * 3);
|
||
const colors = new Float32Array(pts3DData.length * 3);
|
||
for (let i = 0; i < pts3DData.length; i++) {
|
||
positions[i * 3 ] = pts3DData[i].x;
|
||
positions[i * 3 + 1] = pts3DData[i].y;
|
||
positions[i * 3 + 2] = pts3DData[i].z;
|
||
|
||
// Couleur: gradient de profondeur
|
||
const t = (pts3DData[i].z + 1.8) / 1.8; // 0=fond, 1=surface
|
||
colors[i * 3 ] = 0.2 + t * 0.6; // R
|
||
colors[i * 3 + 1] = 0.4 + (1 - t) * 0.4; // G
|
||
colors[i * 3 + 2] = 0.8 + t * 0.2; // B
|
||
}
|
||
|
||
const geo = new THREE.BufferGeometry();
|
||
geo.setAttribute('position', new THREE.BufferAttribute(positions, 3));
|
||
geo.setAttribute('color', new THREE.BufferAttribute(colors, 3));
|
||
|
||
const mat = new THREE.PointsMaterial({ size: 0.04, vertexColors: true, sizeAttenuation: true });
|
||
pointCloud3 = new THREE.Points(geo, mat);
|
||
scene3.add(pointCloud3);
|
||
|
||
// Mise à jour UI
|
||
document.getElementById('ptCount').textContent = pts3DData.length.toLocaleString();
|
||
document.getElementById('colHeight').textContent = '1.8 m';
|
||
document.getElementById('surfArea').textContent = (ROOM_W * ROOM_H * 0.78).toFixed(1) + ' m²';
|
||
}
|
||
|
||
// Rotation souris 3D
|
||
let mouse3 = { down: false, lastX: 0, lastY: 0 };
|
||
let rot3 = { x: 0.4, y: 0 };
|
||
|
||
canvas3d.addEventListener('mousedown', e => { mouse3.down = true; mouse3.lastX = e.clientX; mouse3.lastY = e.clientY; });
|
||
window.addEventListener('mouseup', () => { mouse3.down = false; });
|
||
canvas3d.addEventListener('mousemove', e => {
|
||
if (!mouse3.down) return;
|
||
const dx = e.clientX - mouse3.lastX;
|
||
const dy = e.clientY - mouse3.lastY;
|
||
rot3.y += dx * 0.008;
|
||
rot3.x += dy * 0.008;
|
||
rot3.x = Math.max(-Math.PI / 2, Math.min(Math.PI / 2, rot3.x));
|
||
mouse3.lastX = e.clientX; mouse3.lastY = e.clientY;
|
||
});
|
||
|
||
// Touch support 3D
|
||
let touch3 = { active: false, lastX: 0, lastY: 0 };
|
||
canvas3d.addEventListener('touchstart', e => { touch3.active = true; touch3.lastX = e.touches[0].clientX; touch3.lastY = e.touches[0].clientY; });
|
||
canvas3d.addEventListener('touchend', () => { touch3.active = false; });
|
||
canvas3d.addEventListener('touchmove', e => {
|
||
if (!touch3.active) return;
|
||
e.preventDefault();
|
||
const dx = e.touches[0].clientX - touch3.lastX;
|
||
const dy = e.touches[0].clientY - touch3.lastY;
|
||
rot3.y += dx * 0.008; rot3.x += dy * 0.008;
|
||
touch3.lastX = e.touches[0].clientX; touch3.lastY = e.touches[0].clientY;
|
||
}, { passive: false });
|
||
|
||
function render3D() {
|
||
if (pointCloud3) {
|
||
const r = 10;
|
||
camera3.position.x = r * Math.sin(rot3.y) * Math.cos(rot3.x);
|
||
camera3.position.y = -r * Math.cos(rot3.y) * Math.cos(rot3.x);
|
||
camera3.position.z = r * Math.sin(rot3.x);
|
||
camera3.lookAt(0, 0, -0.9);
|
||
}
|
||
renderer.render(scene3, camera3);
|
||
}
|
||
|
||
function loop3D() {
|
||
render3D();
|
||
requestAnimationFrame(loop3D);
|
||
}
|
||
|
||
document.getElementById('btn3dRegen').addEventListener('click', buildPointCloud3D);
|
||
|
||
// ─── EXPORT PLY ──────────────────────────────────────────────────────────────
|
||
|
||
document.getElementById('btnPly').addEventListener('click', () => {
|
||
if (pts3DData.length === 0) buildPointCloud3D();
|
||
const lines = [
|
||
'ply',
|
||
'format ascii 1.0',
|
||
'comment moulin-mapper · Cosma/Silent Flow',
|
||
`element vertex ${pts3DData.length}`,
|
||
'property float x',
|
||
'property float y',
|
||
'property float z',
|
||
'end_header',
|
||
];
|
||
for (const p of pts3DData) {
|
||
lines.push(`${p.x.toFixed(4)} ${p.y.toFixed(4)} ${p.z.toFixed(4)}`);
|
||
}
|
||
const blob = new Blob([lines.join('\n')], { type: 'text/plain' });
|
||
const url = URL.createObjectURL(blob);
|
||
const a = document.createElement('a');
|
||
a.href = url; a.download = 'moulin-mapper.ply';
|
||
a.click();
|
||
URL.revokeObjectURL(url);
|
||
});
|
||
|
||
// ─── RESIZE ──────────────────────────────────────────────────────────────────
|
||
|
||
function resizeCanvases() {
|
||
// 2D canvas: adaptatif en largeur
|
||
const container = canvas2d.parentElement;
|
||
if (container) {
|
||
const maxW = Math.min(620, container.clientWidth - 240);
|
||
if (maxW > 200) {
|
||
canvas2d.width = maxW;
|
||
canvas2d.height = Math.round(maxW * 440 / 620);
|
||
}
|
||
}
|
||
// 3D
|
||
const w3 = canvas3d.parentElement ? Math.min(680, canvas3d.parentElement.clientWidth - 220) : 680;
|
||
if (w3 > 200) {
|
||
renderer.setSize(w3, Math.round(w3 * 400 / 680));
|
||
camera3.aspect = w3 / Math.round(w3 * 400 / 680);
|
||
camera3.updateProjectionMatrix();
|
||
}
|
||
}
|
||
window.addEventListener('resize', resizeCanvases);
|
||
resizeCanvases();
|
||
|
||
// ─── DÉMARRAGE ───────────────────────────────────────────────────────────────
|
||
|
||
initSim();
|
||
// IMU mode par défaut désactivé, ICP actif — montrer d'abord le bon résultat
|
||
buildPointCloud3D();
|
||
animate();
|
||
loop3D();
|
||
|
||
// Auto-activer IMU mode après 3s pour montrer le contraste
|
||
setTimeout(() => {
|
||
if (!sim.imuMode) {
|
||
sim.imuMode = true;
|
||
btnImu.classList.add('active');
|
||
}
|
||
}, 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>
|