Add 260722 uav view planning

README.md

@@ -72,3 +72,4 @@ To merge your changes/added files into the official match-ROS repository, we nee
 | [250428](student_code/250428_Scan_to_map_localization_Mid360_Simulation/README.md) | Development and implementation of a concept for localization using 3D-LiDAR | Algorithm for scan-to-map based 3D real-time localization |
 | [250429](student_code/250429_real_time_stabilization/README.md) | Real-Time Compensation of Ground Irregularities for Mobile Robot in Construction Additive Manufacturing | Algorithm to stabilize the nozzle for additive manufacturing purposes using IMU-based inclination data during movement on uneven terrain |
 | [251127](student_code/251127_print_texture_localization/README.md) | Concept development for robot localization based on an object to be manufactured | Concept for texture-based localization approach for mobile manipulator based on the initial layers of the printed object, using image descriptors and ICP refinement|
+| [260722](student_code/260722_UAVViewPlanning/README.md) | Entwicklung eines Algorithmus zur optimalen Abtastung von großskaligen Messobjekten mittels UAV-basierter Sensorik | Offline View Planning für ein UAV-getragenes Streifenlichtsystem: Set Cover über eine geraycastete Sichtbarkeitsmatrix, 4-DoF-Posen, Registrierungsgraph und TSP-Sequenzierung |

student_code/260722_UAVViewPlanning/README.md

@@ -0,0 +1,78 @@
+# UAV View Planning für Streifenlicht-Messsysteme
+
+## Overview
+
+Code zur Studienarbeit „Entwicklung eines Algorithmus zur optimalen Abtastung von
+großskaligen Messobjekten mittels UAV-basierter Sensorik".
+
+Eine Drohne mit Streifenlicht-Projektionssystem vermisst großskalige Bauteile.
+Der Algorithmus berechnet offline eine minimale, vollständige und registrierbare
+Menge von Sensorposen — Set Cover über eine geraycastete Sichtbarkeitsmatrix, mit
+4-DoF-Kinematik (x, y, z, yaw), Sensor-Constraints (Arbeitsabstand,
+Einfallswinkel, Sichtfeld, duale Sicht Projektor/Kamera) und bodengebundenen
+Tracking-Einheiten. Reines Python, kein ROS.
+
+* `vpp2d` — 2D-Prototyp, gleiche Constraints, läuft in Sekunden
+* `vpp3d` — volle 3D-Implementierung inkl. Registrierungsgraph, Pose-Verfeinerung,
+  Tracking-Standorten und TSP-Sequenzierung
+
+## Installation
+
+**Python 3.12** (Open3D hat Stand 2026 keine Wheels für 3.13/3.14).
+
+```bash
+cd 260722_UAVViewPlanning
+python3.12 -m venv .venv
+.venv/bin/pip install -r requirements.txt      # Windows: .venv\Scripts\pip
+```
+
+Aufrufe immer aus **diesem** Verzeichnis als Modul, sonst stimmen die Pfade zu
+`Meshes/` und `config.toml` nicht.
+
+## Packages
+
+### vpp2d
+
+2D-Prototyp: Facetten → Posenregionen → Projektion/Dedup → Coverage-Matrix →
+Set Cover → TSP. Details: [`vpp2d/README.md`](vpp2d/README.md).
+
+```bash
+python -m vpp2d.run                                   # Testszene box, Greedy
+python -m vpp2d.run --scene notched_box --method both # Greedy vs. ILP
+python -m vpp2d.stepviz --scene notched_box --show    # Schritt für Schritt
+```
+
+### vpp3d
+
+Dieselbe Pipeline in 3D: Kegel-Sampling des Posenraums, Pitch-Klemmung,
+Sichtbarkeit per Open3D-Raycast (duale Sicht), Set Cover mit Greedy / ILP /
+Connected (OR-Tools CP-SAT), Registrierungsgraph, lokale Pose-Verfeinerung
+(Nelder-Mead), Tracking-Standorte und zweistufiger TSP.
+Details: [`vpp3d/README.md`](vpp3d/README.md), Kurzreferenz [`vpp3d/usage.md`](vpp3d/usage.md).
+
+```bash
+python -m vpp3d.run                                     # Testszene box, Greedy
+python -m vpp3d.run --scene notched_box --method both   # Greedy vs. ILP
+python -m vpp3d.run --mesh Meshes/TestKorper1.stl --method both
+python -m vpp3d.run --scene box --refine                # + Pose-Verfeinerung
+python -m vpp3d.inspector --mesh Meshes/TestKorper1.stl # interaktiver Viewer
+python -m vpp3d.stepviz --scene notched_box --show      # Schritt für Schritt
+```
+
+Ergebnis-PNGs und Laufprotokolle landen in `vpp2d/output/` bzw. `vpp3d/output/`.
+Alle Sensor- und Tracking-Parameter stehen in den jeweiligen `config.toml`.
+
+## Scripts
+
+Keine losen Skripte — alle Einstiegspunkte sind Module:
+
+| Modul | Funktion |
+|---|---|
+| `vpp3d.run` | kompletter Durchlauf, Ergebnis-PNGs |
+| `vpp3d.inspector` | interaktiver Open3D-Viewer der Lösung |
+| `vpp3d.stepviz` | Greedy-Abdeckung Pick für Pick |
+| `vpp3d.render_figures`, `vpp3d.stepfigs` | Abbildungen für die schriftliche Arbeit |
+| `vpp2d.run`, `vpp2d.stepviz` | Pendants in 2D |
+
+`Meshes/` enthält die Testkörper (`TestKorper1.stl` ≈ 4,4 × 2 × 3,1 m,
+Punktwolke `EasyCube.ply`); `box` und `notched_box` werden zur Laufzeit erzeugt.

student_code/260722_UAVViewPlanning/requirements.txt

@@ -0,0 +1,6 @@
+# Python 3.12 erforderlich — Open3D liefert (Stand 2026) keine Wheels für 3.13/3.14.
+numpy
+scipy
+matplotlib
+open3d
+ortools

student_code/260722_UAVViewPlanning/vpp2d/README.md

@@ -0,0 +1,105 @@
+# vpp2d — 2D-Prototyp des alternativen View-Planning-Ansatzes
+
+Eigenständige, **vom bestehenden 3D-Code (`../pipeline`, `../*.py`) bewusst
+getrennte** Implementierung des alternativen Entwurfs („Empfohlene Pipeline,
+offline VPP"). Sie validiert den Ansatz in einer 2D-Umgebung, die in Sekunden
+debugbar ist — vgl. die Empfehlung „erst 2D-Prototyp" in `../PIPELINE.md`.
+
+In 2D kollabiert die 4-DoF-Kinematik (x, y, z, yaw) auf 3 DoF (x, y, yaw); z und
+Pitch entfallen. **Die Constraint-Struktur — Inzidenzkegel, Arbeitsschale, FoV,
+Occlusion, duale Sicht — bleibt identisch** und wird hier vollständig
+durchgerechnet.
+
+> **Fokus: reine Sichtlinie (line of sight).** Kein Tracking-/Standortmodell —
+> Repositionierung der mobilen Tracking-Einheit wird *nicht* optimiert (für diese
+> Arbeit irrelevant). Es gibt daher keinen Arbeitskreis und kein bi-level set
+> cover; das Set Cover ist single-level über die Sichtbarkeitsmatrix.
+
+## Pipeline (Spiegel des Bild-Entwurfs, in 2D)
+
+| Schritt | Modul | Box im Entwurf |
+|---|---|---|
+| [1] Facetten als Primitiv | `scene.py` | Mesh-Facetten als Primitiv |
+| [2] Posenregion je Facette samplen | `candidates.sample_pose_regions` | Posenregion pro Facette samplen |
+| [3] Projektion auf Posenraum + Dedup | `candidates.project_and_dedup` | **Projektion auf 4-DoF-Raum** |
+| [4] Coverage-Matrix per Raycast, duale Sicht | `visibility.py` | Coverage-Matrix per Raycasting |
+| [6] Set Cover (Greedy + ILP) | `setcover.py` | (clustered) set cover → single-level |
+| [7] TSP-Tour | `sequencing.py` | TSP-Tour |
+
+### Differenzierende Elemente
+
+1. **Facetten** als Abdeckungsziel (Polygon-Segmente mit Außennormalen) statt
+   Poisson-gesampelter Patch-Punktwolke.
+2. **Projektion + Dedup** als expliziter eigener Schritt
+   (`candidates.project_and_dedup`): die pro Facette gesampelten Posen werden
+   auf den von der Drohne stellbaren Raum (x, y, yaw; Roll gesperrt, in 3D
+   zusätzlich Pitch geklemmt) projiziert und über ein Ortsvoxel- × Yaw-Bin-Raster
+   dedupliziert. Benachbarte Facetten teilen denselben Posenraum → der Dedup
+   kollabiert die hochredundante Rohmenge (typ. Faktor ~2 in 2D, in 3D höher).
+3. **Duale Sicht** (`visibility.py`): ein Scan gilt nur, wenn Projektor *und*
+   Kamera (um die Basislinie versetzt) die Facette unverdeckt und im FoV sehen.
+   Die bestehende `pipeline/visibility.py` macht nur einen einzelnen Raycast.
+   `baseline = 0` reduziert das Modell auf die klassische Einzelsicht (Ablation).
+
+## Aufruf
+
+Aus dem Verzeichnis `Code/` (venv mit numpy/scipy/matplotlib/ortools):
+
+```powershell
+.venv\Scripts\python -m vpp2d.run                                   # box, Greedy
+.venv\Scripts\python -m vpp2d.run --scene notched_box --method both # Greedy vs. ILP
+.venv\Scripts\python -m vpp2d.run --stl Meshes/TestKorper1.stl --method both   # realer Mesh-Schnitt
+.venv\Scripts\python -m vpp2d.run --scene notched_box --baseline 0  # Ablation: Einzelsicht statt dual
+```
+
+Ergebnis-PNGs (4-Panel-Übersicht: Facetten+Normalen, Kandidatenposen,
+Coverage-Grad, Lösung mit gewählten Posen + Route) landen in `vpp2d/output/`.
+
+### Schritt-für-Schritt-Debug-Viewer
+
+Zeigt den Greedy-Aufbau der Abdeckung Pick für Pick (Schritt 0 = nichts erfasst):
+
+```powershell
+.venv\Scripts\python -m vpp2d.stepviz --scene notched_box           # PNG-Frames
+.venv\Scripts\python -m vpp2d.stepviz --scene notched_box --show     # interaktiv
+.venv\Scripts\python -m vpp2d.stepviz --stl Meshes/TestKorper1.stl
+```
+
+- `--save` (Default): je Schritt ein PNG nach `output/steps_<szene>_greedy/step_000.png …`.
+- `--show`: interaktiver Navigator — **→ / n** vor, **← / b** zurück, **Home/End**,
+  **s** speichern, **q** schließen.
+
+Pro Frame sichtbar: bereits abgedeckte Facetten (grün), noch offene (rot),
+unerreichbare (grau); die in diesem Schritt gewählte Pose mit Blickrichtung und
+FoV-Footprint (Arbeitsschalen-Sektor); die **neu** erfassten Facetten (orange)
+und die **Überlappung** mit bereits Erfasstem (blau). Der Titel führt Fortschritt
+(`x/n erfasst`) und Posenzahl mit.
+
+### Testszenen
+
+- `box` — konvexes 4×4-m-Quadrat, 2D-Analogon zu **EasyCube** (alles frei
+  einsehbar).
+- `notched_box` — 4,4×3,1-m-Rechteck mit rechteckiger Tasche, 2D-Analogon zu
+  **TestKorper1**: erzeugt Selbstverdeckung (die duale Sicht verwirft Posen, bei
+  denen die Kamera durch die Taschenkante verdeckt wird) und unerreichbare
+  Facetten tief in der Tasche.
+- `--stl <pfad>` — horizontaler Schnitt durch ein reales STL-Mesh
+  (abhängigkeitsfreier Slicer), nutzt damit dieselben Beispiele wie die
+  3D-Pipeline.
+
+## Beispielergebnisse (Default-Config)
+
+| Szene | erreichbar | Greedy (Posen) | ILP (Posen) |
+|---|---|---|---|
+| box | 64/64 | 19 | 10 |
+| notched_box | 68/71 | 17 | 15 |
+| slice TestKorper1 | 54/54 | 15 | 10 |
+
+## Nicht enthalten / Abgrenzung
+
+- Kein z/Pitch (2D). Die 4-DoF-Projektion ist hier eine 3-DoF-(x,y,yaw)-
+  Projektion; die Pitch-Klemmung entfällt mangels dritter Dimension.
+- Kein Tracking-/Standortmodell und keine Repositionierungs-Optimierung.
+- Kollision nur als 1-NN-Clearance-Proxy; echte Freiraumprüfung erst im
+  ROS2/Gazebo-Teil.
+- Euklidische Distanzen in der Sequenzierung (keine Roadmap).

student_code/260722_UAVViewPlanning/vpp2d/__init__.py

@@ -0,0 +1,15 @@
+from .config import Config, load_config
+from .scene import Scene, Facets, build_scene, box, notched_box, from_stl_slice, SCENES
+from .candidates import Poses, sample_pose_regions, project_and_dedup
+from .visibility import VisibilityMatrix, compute_visibility
+from .setcover import CoverResult, greedy_set_cover, ilp_set_cover
+from .sequencing import Route, sequence_route
+
+__all__ = [
+    "Config", "load_config",
+    "Scene", "Facets", "build_scene", "box", "notched_box", "from_stl_slice", "SCENES",
+    "Poses", "sample_pose_regions", "project_and_dedup",
+    "VisibilityMatrix", "compute_visibility",
+    "CoverResult", "greedy_set_cover", "ilp_set_cover",
+    "Route", "sequence_route",
+]

student_code/260722_UAVViewPlanning/vpp2d/candidates.py

@@ -0,0 +1,90 @@
+from __future__ import annotations
+
+from dataclasses import dataclass, field
+
+import numpy as np
+from scipy.spatial import cKDTree
+
+from .config import Config
+from .scene import Scene
+
+
+@dataclass
+class Poses:
+    positions: np.ndarray
+    yaws: np.ndarray
+    ids: np.ndarray
+    seed_facet: np.ndarray = field(default_factory=lambda: np.empty(0, np.int64))
+
+    def __len__(self) -> int:
+        return len(self.ids)
+
+    @property
+    def view_dirs(self) -> np.ndarray:
+        return np.column_stack([np.cos(self.yaws), np.sin(self.yaws)])
+
+
+def _rotate(vec: np.ndarray, angles: np.ndarray) -> np.ndarray:
+    c, s = np.cos(angles), np.sin(angles)
+    return np.column_stack([c * vec[0] - s * vec[1], s * vec[0] + c * vec[1]])
+
+
+def sample_pose_regions(scene: Scene, cfg: Config) -> Poses:
+    incs = np.linspace(-cfg.theta_max_rad, cfg.theta_max_rad, cfg.n_incidence)
+    dists = (np.array([cfg.d_opt]) if cfg.n_distance == 1
+             else np.linspace(cfg.d_min, cfg.d_max, cfg.n_distance))
+
+    pos_list, yaw_list, seed_list = [], [], []
+    for fid, (c, n) in enumerate(zip(scene.facets.centers, scene.facets.normals)):
+        dirs = _rotate(n, incs)
+        for d in dists:
+            p = c[None, :] + dirs * d
+            to_facet = c[None, :] - p
+            yaw = np.arctan2(to_facet[:, 1], to_facet[:, 0])
+            pos_list.append(p)
+            yaw_list.append(yaw)
+            seed_list.append(np.full(len(p), fid, dtype=np.int64))
+
+    positions = np.vstack(pos_list)
+    yaws = np.concatenate(yaw_list)
+    seed = np.concatenate(seed_list)
+    return Poses(positions, yaws, np.arange(len(positions), dtype=np.int64), seed)
+
+
+def _clearance_mask(positions: np.ndarray, scene: Scene, safety: float) -> np.ndarray:
+    tree = cKDTree(scene.facets.centers)
+    dist, _ = tree.query(positions, k=1)
+    return dist >= safety
+
+
+def project_and_dedup(raw: Poses, scene: Scene, cfg: Config) -> tuple[Poses, dict]:
+    keep = _clearance_mask(raw.positions, scene, cfg.safety_distance)
+    pos = raw.positions[keep]
+    yaw = raw.yaws[keep]
+    seed = raw.seed_facet[keep]
+    n_after_clear = len(pos)
+
+    yaw_mod = np.mod(yaw, 2 * np.pi)
+    yaw_bins = np.floor(yaw_mod / cfg.yaw_bin_rad).astype(np.int64)
+
+    origin = pos.min(axis=0)
+    ix = np.floor((pos[:, 0] - origin[0]) / cfg.voxel).astype(np.int64)
+    iy = np.floor((pos[:, 1] - origin[1]) / cfg.voxel).astype(np.int64)
+
+    keys = np.column_stack([ix, iy, yaw_bins])
+    _, first_idx = np.unique(keys, axis=0, return_index=True)
+    first_idx = np.sort(first_idx)
+
+    poses = Poses(
+        positions=pos[first_idx],
+        yaws=yaw[first_idx],
+        ids=np.arange(len(first_idx), dtype=np.int64),
+        seed_facet=seed[first_idx],
+    )
+    stats = {
+        "n_raw": len(raw),
+        "n_after_clearance": n_after_clear,
+        "n_after_dedup": len(poses),
+        "dedup_ratio": len(poses) / max(1, n_after_clear),
+    }
+    return poses, stats

student_code/260722_UAVViewPlanning/vpp2d/config.py

@@ -0,0 +1,84 @@
+from __future__ import annotations
+
+import math
+import tomllib
+from dataclasses import dataclass
+from pathlib import Path
+
+_DEFAULT_PATH = Path(__file__).with_name("config.toml")
+
+
+@dataclass(frozen=True)
+class Config:
+    d_min: float
+    d_max: float
+    d_opt: float
+
+    fov_deg: float
+
+    baseline: float
+
+    theta_max_deg: float
+
+    safety_distance: float
+
+    min_overlap: float
+
+    n_incidence: int
+    n_distance: int
+    voxel: float
+    yaw_bin_deg: float
+    resolution: float
+
+    @property
+    def fov_rad(self) -> float:
+        return math.radians(self.fov_deg)
+
+    @property
+    def theta_max_rad(self) -> float:
+        return math.radians(self.theta_max_deg)
+
+    @property
+    def yaw_bin_rad(self) -> float:
+        return math.radians(self.yaw_bin_deg)
+
+    def summary(self) -> str:
+        return (
+            "Config(2D):\n"
+            f"  Arbeitsabstand : [{self.d_min}, {self.d_max}] m (d_opt={self.d_opt})\n"
+            f"  FoV / Inzidenz : {self.fov_deg}° / ≤{self.theta_max_deg}°\n"
+            f"  Basislinie     : {self.baseline} m "
+            f"({'duale Sicht' if self.baseline > 0 else 'Einzelsicht'})\n"
+            f"  Sampling       : {self.n_incidence}×Inzidenz × {self.n_distance}×Distanz, "
+            f"Dedup {self.voxel:.2f} m / {self.yaw_bin_deg}°"
+        )
+
+
+def load_config(path: str | Path | None = None) -> Config:
+    path = Path(path) if path is not None else _DEFAULT_PATH
+    with open(path, "rb") as fh:
+        raw = tomllib.load(fh)
+
+    wd = raw["working_distance"]
+    d_min = float(wd["d_min"])
+    d_max = float(wd["d_max"])
+    d_opt = float(wd.get("d_opt", (d_min + d_max) / 2.0))
+
+    smp = raw.get("sampling", {})
+    voxel = float(smp.get("voxel", 0.15 * d_opt))
+
+    return Config(
+        d_min=d_min,
+        d_max=d_max,
+        d_opt=d_opt,
+        fov_deg=float(raw["fov"]["fov_deg"]),
+        baseline=float(raw.get("sensor", {}).get("baseline", 0.0)),
+        theta_max_deg=float(raw["incidence"]["theta_max_deg"]),
+        safety_distance=float(raw["drone"]["safety_distance"]),
+        min_overlap=float(raw["registration"]["min_overlap"]),
+        n_incidence=int(smp.get("n_incidence", 7)),
+        n_distance=int(smp.get("n_distance", 2)),
+        voxel=voxel,
+        yaw_bin_deg=float(smp.get("yaw_bin_deg", 15.0)),
+        resolution=float(smp.get("resolution", 0.25)),
+    )