diff --git a/README.md b/README.md index 30b1468..2bcd07f 100644 --- a/README.md +++ b/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 | diff --git a/student_code/260722_UAVViewPlanning/Meshes/EasyCube.ply b/student_code/260722_UAVViewPlanning/Meshes/EasyCube.ply new file mode 100644 index 0000000..fb58d83 Binary files /dev/null and b/student_code/260722_UAVViewPlanning/Meshes/EasyCube.ply differ diff --git a/student_code/260722_UAVViewPlanning/Meshes/TestKorper1.stl b/student_code/260722_UAVViewPlanning/Meshes/TestKorper1.stl new file mode 100644 index 0000000..f25868c Binary files /dev/null and b/student_code/260722_UAVViewPlanning/Meshes/TestKorper1.stl differ diff --git a/student_code/260722_UAVViewPlanning/README.md b/student_code/260722_UAVViewPlanning/README.md new file mode 100644 index 0000000..c2fd9d9 --- /dev/null +++ b/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. diff --git a/student_code/260722_UAVViewPlanning/requirements.txt b/student_code/260722_UAVViewPlanning/requirements.txt new file mode 100644 index 0000000..2258cba --- /dev/null +++ b/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 diff --git a/student_code/260722_UAVViewPlanning/vpp2d/README.md b/student_code/260722_UAVViewPlanning/vpp2d/README.md new file mode 100644 index 0000000..ff92e59 --- /dev/null +++ b/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__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 ` — 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). diff --git a/student_code/260722_UAVViewPlanning/vpp2d/__init__.py b/student_code/260722_UAVViewPlanning/vpp2d/__init__.py new file mode 100644 index 0000000..4656c40 --- /dev/null +++ b/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", +] diff --git a/student_code/260722_UAVViewPlanning/vpp2d/candidates.py b/student_code/260722_UAVViewPlanning/vpp2d/candidates.py new file mode 100644 index 0000000..185885d --- /dev/null +++ b/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 diff --git a/student_code/260722_UAVViewPlanning/vpp2d/config.py b/student_code/260722_UAVViewPlanning/vpp2d/config.py new file mode 100644 index 0000000..d52b926 --- /dev/null +++ b/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)), + ) diff --git a/student_code/260722_UAVViewPlanning/vpp2d/config.toml b/student_code/260722_UAVViewPlanning/vpp2d/config.toml new file mode 100644 index 0000000..51098f6 --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp2d/config.toml @@ -0,0 +1,61 @@ +# Constraints des 2D-Solver-Skeletts (vpp2d). +# +# Eigenständige Konfiguration des alternativen Ansatzes. Bewusst getrennt von +# der 3D-Pipeline-Config (../config.toml), damit der 2D-Prototyp self-contained +# bleibt. Distanzen in Metern, Winkel in Grad. +# +# Der 2D-Prototyp validiert den alternativen Ansatz mit Fokus auf der reinen +# Sichtlinie (line of sight): +# (1) Projektion auf den unteraktuierten Posenraum + Dedup +# (2) Coverage-Matrix per Raycasting mit dualer Sicht (Projektor + Kamera) +# (3) Set Cover (Greedy + ILP) ueber die Sichtbarkeitsmatrix +# in einer Umgebung, die in Minuten debugbar ist. In 2D kollabiert die +# 4-DoF-Kinematik (x, y, z, yaw) auf 3 DoF (x, y, yaw); z und pitch entfallen. +# Die Constraint-Struktur bleibt identisch. +# +# Kein Tracking-/Standortmodell: Repositionierung wird nicht optimiert (irrelevant +# fuer diese Arbeit), daher kein Arbeitskreis und kein bi-level set cover. + +[working_distance] +# Arbeitsvolumen des Streifenprojektors: nahe/ferne Distanzgrenze [m]. +d_min = 2.8 +d_max = 3.2 +# Soll-Standoff [m] fuer die Kandidatenplatzierung. Auskommentiert -> Mittelwert. +# d_opt = 3.0 + +[fov] +# Sichtfeld in der Ebene [Grad], voller Oeffnungswinkel (in 2D = Kreissegment). +fov_deg = 40.0 + +[sensor] +# Basislinie zwischen Projektor und Kamera des Streifenlichtsystems [m]. +# Ein Scan ist nur gueltig, wenn *beide* (Projektor UND Kamera) die Facette +# unverdeckt und im Sichtfeld sehen ("duale Sicht"). baseline = 0 -> klassische +# Einzelsicht (Ablation). Die Kamera sitzt um baseline seitlich (rechtwinklig zur +# Blickachse) versetzt. +baseline = 0.4 + +[sampling] +# Abtastung der zulaessigen Posenregion je Facette (Schritt [2]). +# Inzidenzwinkel-Stuetzstellen im Kegel [-theta_max, +theta_max] (ungerade -> +# frontaler Strahl enthalten) und Distanz-Stuetzstellen in [d_min, d_max]. +n_incidence = 7 +n_distance = 2 +# Dedup-Aufloesung der Projektion (Schritt [3]): Ortsraster [m] und Yaw-Bin [Grad]. +# Auskommentiertes voxel -> 0.15 * d_opt. +# voxel = 0.45 +yaw_bin_deg = 15.0 +# Facetten-Aufloesung der Szene: Soll-Segmentlaenge [m]. +resolution = 0.25 + +[incidence] +# Maximaler Einfallswinkel zwischen Sichtstrahl und Patch-Normale [Grad]. +theta_max_deg = 60.0 + +[drone] +# Mindest-Sicherheitsabstand der Drohne zum Messobjekt [m]. +safety_distance = 1.5 + +[registration] +# Mindest-Overlap zweier Scans (Anteil gemeinsamer Patches, kleinerer Scan). +min_overlap = 0.25 diff --git a/student_code/260722_UAVViewPlanning/vpp2d/plotting.py b/student_code/260722_UAVViewPlanning/vpp2d/plotting.py new file mode 100644 index 0000000..47f56d3 --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp2d/plotting.py @@ -0,0 +1,74 @@ +from __future__ import annotations + +import numpy as np + +import matplotlib +matplotlib.use("Agg") +import matplotlib.pyplot as plt + + +def _draw_scene(ax, scene): + for poly in scene.polygons: + p = np.vstack([poly, poly[0]]) + ax.fill(p[:, 0], p[:, 1], facecolor="0.85", edgecolor="0.4", lw=1.2, zorder=1) + ax.set_aspect("equal") + ax.set_xlabel("x [m]"); ax.set_ylabel("y [m]") + + +def plot_overview(scene, poses, vis, cover, route, cfg, + path: str, show: bool = False): + fig, axes = plt.subplots(2, 2, figsize=(14, 12)) + fc = scene.facets + + ax = axes[0, 0] + _draw_scene(ax, scene) + ax.quiver(fc.centers[:, 0], fc.centers[:, 1], fc.normals[:, 0], fc.normals[:, 1], + color="tab:blue", scale=30, width=0.003, zorder=3) + ax.set_title(f"[1] Facetten ({len(fc)}) + Außennormalen") + + ax = axes[0, 1] + _draw_scene(ax, scene) + vd = poses.view_dirs + ax.quiver(poses.positions[:, 0], poses.positions[:, 1], vd[:, 0], vd[:, 1], + color="tab:green", scale=40, width=0.002, alpha=0.6, zorder=3) + ax.set_title(f"[2/3] Kandidatenposen nach Projektion+Dedup ({len(poses)})") + + ax = axes[1, 0] + _draw_scene(ax, scene) + cov = vis.coverage_per_facet() + sc = ax.scatter(fc.centers[:, 0], fc.centers[:, 1], c=cov, cmap="viridis", + s=18, zorder=3) + fig.colorbar(sc, ax=ax, label="Posen mit Sicht (duale Sicht)") + ax.set_title(f"[4] Coverage-Matrix: {(cov > 0).sum()}/{len(fc)} erreichbar") + + ax = axes[1, 1] + _draw_scene(ax, scene) + sel_V = vis.V.tocsr()[cover.poses] if len(cover.poses) else None + covered = (np.asarray(sel_V.sum(axis=0)).ravel() > 0) if sel_V is not None \ + else np.zeros(len(fc), bool) + ax.scatter(fc.centers[covered, 0], fc.centers[covered, 1], c="tab:green", + s=12, zorder=2, label="abgedeckt") + ax.scatter(fc.centers[~covered, 0], fc.centers[~covered, 1], c="tab:red", + s=12, zorder=2, label="offen") + + pp = poses.positions[cover.poses] + pvd = poses.view_dirs[cover.poses] + ax.quiver(pp[:, 0], pp[:, 1], pvd[:, 0], pvd[:, 1], color="black", + scale=22, width=0.005, zorder=6) + + if len(route.order): + rp = pp[route.order] + ax.plot(rp[:, 0], rp[:, 1], "-", color="tab:orange", lw=1.0, alpha=0.8, + zorder=4) + + ax.set_title(f"[6/7] {len(cover.poses)} Posen, Weg {route.length:.0f} m") + ax.legend(loc="upper right", fontsize=8) + + fig.suptitle(f"vpp2d — {scene.name} — {cover.method}", fontsize=14) + fig.tight_layout(rect=(0, 0, 1, 0.98)) + fig.savefig(path, dpi=110) + if show: + import matplotlib.pyplot as _plt + _plt.show() + plt.close(fig) + return path diff --git a/student_code/260722_UAVViewPlanning/vpp2d/run.py b/student_code/260722_UAVViewPlanning/vpp2d/run.py new file mode 100644 index 0000000..572bed2 --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp2d/run.py @@ -0,0 +1,106 @@ +from __future__ import annotations + +import argparse +import dataclasses +import sys +from pathlib import Path + +try: + sys.stdout.reconfigure(encoding="utf-8") +except (AttributeError, ValueError): + pass + +from .config import load_config +from . import scene as scene_mod +from .candidates import sample_pose_regions, project_and_dedup +from .visibility import compute_visibility +from .setcover import greedy_set_cover, ilp_set_cover +from .sequencing import sequence_route +from .plotting import plot_overview + +_OUT = Path(__file__).with_name("output") + + +def build_scene_from_args(args, cfg): + if args.stl: + return scene_mod.from_stl_slice( + args.stl, level=args.stl_level, resolution=cfg.resolution) + return scene_mod.SCENES[args.scene](cfg.resolution) + + +def run(args) -> None: + cfg = load_config(args.config) + if args.baseline is not None: + cfg = dataclasses.replace(cfg, baseline=args.baseline) + + print(cfg.summary()) + print("=" * 64) + + scene = build_scene_from_args(args, cfg) + print(scene.summary()) + + raw = sample_pose_regions(scene, cfg) + poses, dstats = project_and_dedup(raw, scene, cfg) + print(f"[2/3] Posen: roh {dstats['n_raw']:,} -> Clearance " + f"{dstats['n_after_clearance']:,} -> Dedup {dstats['n_after_dedup']:,} " + f"(Faktor {dstats['n_after_clearance'] / max(1, dstats['n_after_dedup']):.1f})") + + vis = compute_visibility(scene, poses, cfg) + print(vis.summary()) + if cfg.baseline > 0: + s = vis.stats + lost = s["occlusion_single"] - s["dual"] + print(f" duale Sicht verwirft {lost:,} Paare, die die Einzelsicht " + f"zulassen würde (Kamera verdeckt).") + + methods = ["greedy", "ilp"] if args.method == "both" else [args.method] + results = {} + for meth in methods: + if meth == "greedy": + res = greedy_set_cover(vis.V) + else: + res = ilp_set_cover(vis.V, time_limit=args.time_limit) + results[meth] = res + print("-" * 64) + print(res.summary()) + + if "greedy" in results and "ilp" in results: + g, il = results["greedy"], results["ilp"] + print("-" * 64) + print(f"Greedy vs. ILP: Posen {len(g.poses)} vs. {len(il.poses)} " + f"(Faktor {len(g.poses) / max(1, len(il.poses)):.2f})") + + _OUT.mkdir(exist_ok=True) + for meth, res in results.items(): + route = sequence_route(poses.positions[res.poses]) + print(f"[7] {meth}: {route.summary()}") + png = _OUT / f"{scene.name}_{meth}.png" + plot_overview(scene, poses, vis, res, route, cfg, str(png), show=args.show) + print(f" -> {png}") + + +def main() -> None: + ap = argparse.ArgumentParser(description="vpp2d — 2D-Prototyp (alternativer VPP-Ansatz)") + ap.add_argument("--scene", default="box", choices=list(scene_mod.SCENES), + help="synthetische Testszene") + ap.add_argument("--stl", default=None, + help="statt Szene: 2D-Schnitt durch ein STL-Mesh") + ap.add_argument("--stl-level", type=float, default=None, + help="Schnitthöhe z für --stl (Default: Mitte)") + ap.add_argument("--method", default="greedy", choices=["greedy", "ilp", "both"]) + ap.add_argument("--baseline", type=float, default=None, + help="Basislinie überschreiben (0 = Einzelsicht-Ablation)") + ap.add_argument("--time-limit", type=float, default=60.0, + help="Zeitlimit ILP [s]") + ap.add_argument("--config", default=None, help="alternative config.toml") + ap.add_argument("--show", action="store_true", help="Plots interaktiv anzeigen") + args = ap.parse_args() + + if args.show: + import matplotlib + matplotlib.use("TkAgg") + run(args) + + +if __name__ == "__main__": + main() diff --git a/student_code/260722_UAVViewPlanning/vpp2d/scene.py b/student_code/260722_UAVViewPlanning/vpp2d/scene.py new file mode 100644 index 0000000..e5f7557 --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp2d/scene.py @@ -0,0 +1,198 @@ +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np + + +@dataclass +class Facets: + centers: np.ndarray + normals: np.ndarray + a: np.ndarray + b: np.ndarray + ids: np.ndarray + + def __len__(self) -> int: + return len(self.ids) + + +@dataclass +class Scene: + polygons: list[np.ndarray] + facets: Facets + edges: np.ndarray + name: str = "scene" + + @property + def bounds(self) -> tuple[float, float, float, float]: + allv = np.vstack(self.polygons) + return (allv[:, 0].min(), allv[:, 1].min(), + allv[:, 0].max(), allv[:, 1].max()) + + def summary(self) -> str: + x0, y0, x1, y1 = self.bounds + return ( + f"Scene '{self.name}': {len(self.polygons)} Polygon(e), " + f"{len(self.facets)} Facetten, {len(self.edges)} Occluder-Kanten\n" + f" Ausdehnung: {x1 - x0:.1f} × {y1 - y0:.1f} m" + ) + + +def _signed_area(poly: np.ndarray) -> float: + x, y = poly[:, 0], poly[:, 1] + return 0.5 * np.sum(x * np.roll(y, -1) - np.roll(x, -1) * y) + + +def _ensure_ccw(poly: np.ndarray) -> np.ndarray: + return poly if _signed_area(poly) > 0 else poly[::-1].copy() + + +def build_scene(polygons: list[np.ndarray], resolution: float, + name: str = "scene") -> Scene: + centers, normals, segs_a, segs_b = [], [], [], [] + edges = [] + for poly in polygons: + poly = _ensure_ccw(np.asarray(poly, dtype=float)) + m = len(poly) + for i in range(m): + v0 = poly[i] + v1 = poly[(i + 1) % m] + edges.append([v0, v1]) + d = v1 - v0 + length = float(np.hypot(*d)) + if length < 1e-12: + continue + n = np.array([d[1], -d[0]]) / length + k = max(1, int(round(length / resolution))) + ts = (np.arange(k) + 0.5) / k + for j in range(k): + a = v0 + d * (j / k) + b = v0 + d * ((j + 1) / k) + centers.append(v0 + d * ts[j]) + normals.append(n) + segs_a.append(a) + segs_b.append(b) + + centers = np.asarray(centers, dtype=float) + normals = np.asarray(normals, dtype=float) + facets = Facets( + centers=centers, + normals=normals, + a=np.asarray(segs_a, dtype=float), + b=np.asarray(segs_b, dtype=float), + ids=np.arange(len(centers), dtype=np.int64), + ) + return Scene( + polygons=[_ensure_ccw(np.asarray(p, float)) for p in polygons], + facets=facets, + edges=np.asarray(edges, dtype=float), + name=name, + ) + + +def box(resolution: float, size: float = 4.0, name: str = "box") -> Scene: + h = size / 2.0 + poly = np.array([[-h, -h], [h, -h], [h, h], [-h, h]]) + return build_scene([poly], resolution, name) + + +def notched_box(resolution: float, w: float = 4.4, h: float = 3.1, + notch: float = 1.2, name: str = "notched_box") -> Scene: + x0, y0 = 0.0, 0.0 + x1, y1 = w, h + nd = notch + cy = h / 2.0 + poly = np.array([ + [x0, y0], + [x1, y0], + [x1, cy - nd / 2], + [x1 - nd, cy - nd / 2], + [x1 - nd, cy + nd / 2], + [x1, cy + nd / 2], + [x1, y1], + [x0, y1], + ]) + return build_scene([poly], resolution, name) + + +def _read_stl_triangles(path: str) -> np.ndarray: + with open(path, "rb") as fh: + head = fh.read(5) + fh.seek(0) + if head == b"solid": + data = fh.read() + if b"facet" in data[:512]: + verts = [] + for line in data.decode("ascii", "ignore").splitlines(): + p = line.split() + if len(p) == 4 and p[0] == "vertex": + verts.append([float(p[1]), float(p[2]), float(p[3])]) + tri = np.asarray(verts, dtype=float).reshape(-1, 3, 3) + return tri + fh.seek(0) + fh.seek(80) + n = int(np.frombuffer(fh.read(4), dtype=" Scene: + tri = _read_stl_triangles(path) + if level is None: + level = float((tri[..., axis].min() + tri[..., axis].max()) / 2.0) + + segs: list[np.ndarray] = [] + pa, pb = plane + for t in tri: + d = t[:, axis] - level + pts = [] + for i in range(3): + j = (i + 1) % 3 + di, dj = d[i], d[j] + if (di <= 0 < dj) or (dj <= 0 < di): + s = di / (di - dj) + p = t[i] + s * (t[j] - t[i]) + pts.append([p[pa], p[pb]]) + if len(pts) == 2: + segs.append(np.asarray(pts, dtype=float)) + + if not segs: + raise ValueError(f"Schnitt bei {axis}={level} ergab keine Segmente.") + + polygons = _chain_segments(np.asarray(segs)) + stem = path.replace("\\", "/").split("/")[-1].rsplit(".", 1)[0] + nm = name or f"slice_{stem}" + return build_scene(polygons, resolution, nm) + + +def _chain_segments(segs: np.ndarray, tol: float = 1e-6) -> list[np.ndarray]: + remaining = list(map(tuple, segs.reshape(-1, 2, 2))) + polys: list[np.ndarray] = [] + while remaining: + seg = remaining.pop() + chain = [np.asarray(seg[0]), np.asarray(seg[1])] + changed = True + while changed: + changed = False + for idx, s in enumerate(remaining): + s0, s1 = np.asarray(s[0]), np.asarray(s[1]) + if np.allclose(chain[-1], s0, atol=tol): + chain.append(s1); remaining.pop(idx); changed = True; break + if np.allclose(chain[-1], s1, atol=tol): + chain.append(s0); remaining.pop(idx); changed = True; break + poly = np.asarray(chain) + if len(poly) >= 3: + if np.allclose(poly[0], poly[-1], atol=tol): + poly = poly[:-1] + polys.append(poly) + return polys + + +SCENES = { + "box": box, + "notched_box": notched_box, +} diff --git a/student_code/260722_UAVViewPlanning/vpp2d/sequencing.py b/student_code/260722_UAVViewPlanning/vpp2d/sequencing.py new file mode 100644 index 0000000..020f74e --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp2d/sequencing.py @@ -0,0 +1,58 @@ +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np + + +@dataclass +class Route: + order: np.ndarray + length: float + + def summary(self) -> str: + return f"Route: {len(self.order)} Posen, Flugweg {self.length:.1f} m" + + +def _nn_order(points: np.ndarray, start: int = 0) -> list[int]: + n = len(points) + if n <= 1: + return list(range(n)) + unvisited = set(range(n)) + order = [start] + unvisited.discard(start) + while unvisited: + last = order[-1] + nxt = min(unvisited, key=lambda i: np.hypot(*(points[i] - points[last]))) + order.append(nxt) + unvisited.discard(nxt) + return order + + +def _path_len(points: np.ndarray, order: list[int]) -> float: + return float(sum(np.hypot(*(points[order[i + 1]] - points[order[i]])) + for i in range(len(order) - 1))) + + +def _two_opt(points: np.ndarray, order: list[int]) -> list[int]: + best = order[:] + best_len = _path_len(points, best) + improved = True + while improved: + improved = False + for i in range(1, len(best) - 1): + for k in range(i + 1, len(best)): + cand = best[:i] + best[i:k + 1][::-1] + best[k + 1:] + cl = _path_len(points, cand) + if cl + 1e-9 < best_len: + best, best_len = cand, cl + improved = True + return best + + +def sequence_route(pose_positions: np.ndarray) -> Route: + if len(pose_positions) == 0: + return Route(np.empty(0, np.int64), 0.0) + order = _nn_order(pose_positions, start=0) + order = _two_opt(pose_positions, order) + return Route(np.asarray(order, np.int64), _path_len(pose_positions, order)) diff --git a/student_code/260722_UAVViewPlanning/vpp2d/setcover.py b/student_code/260722_UAVViewPlanning/vpp2d/setcover.py new file mode 100644 index 0000000..c753e4b --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp2d/setcover.py @@ -0,0 +1,106 @@ +from __future__ import annotations + +import time +from dataclasses import dataclass, field + +import numpy as np +import scipy.sparse as sp + + +@dataclass +class CoverResult: + poses: np.ndarray + method: str + n_coverable: int = 0 + n_covered: int = 0 + runtime: float = 0.0 + optimal: bool = False + extra: dict = field(default_factory=dict) + + def summary(self) -> str: + opt = " (Optimum bewiesen)" if self.optimal else "" + return ( + f"Set Cover [{self.method}{opt}]:\n" + f" Drohnenposen (Scans) : {len(self.poses)}\n" + f" Abdeckung erreichbarer Facetten : " + f"{self.n_covered}/{self.n_coverable} " + f"({self.n_covered / max(1, self.n_coverable):.1%})\n" + f" Rechenzeit : {self.runtime:.2f} s" + ) + + +def _coverable(V: sp.csr_matrix) -> np.ndarray: + return np.asarray(V.sum(axis=0)).ravel() > 0 + + +def greedy_set_cover(V: sp.csr_matrix, trace: list | None = None) -> CoverResult: + t0 = time.perf_counter() + Vc = V.tocsr().astype(np.int64) + coverable = _coverable(Vc) + deficit = coverable.copy() + chosen = np.zeros(Vc.shape[0], dtype=bool) + selected: list[int] = [] + + while deficit.any(): + gain = np.asarray(Vc @ deficit.astype(np.int64)).ravel() + gain[chosen] = 0 + j = int(np.argmax(gain)) + if gain[j] == 0: + break + seen = Vc.indices[Vc.indptr[j]:Vc.indptr[j + 1]] + if trace is not None: + open_seen = deficit[seen] + trace.append({ + "pose": int(j), + "covered_before": (coverable & ~deficit).copy(), + "new_facets": seen[open_seen].copy(), + "overlap_facets": seen[~open_seen].copy(), + }) + chosen[j] = True + selected.append(j) + deficit[seen] = False + + n_covered = int(coverable.sum() - deficit.sum()) + return CoverResult( + poses=np.asarray(selected, dtype=np.int64), + method="greedy", + n_coverable=int(coverable.sum()), + n_covered=n_covered, + runtime=time.perf_counter() - t0, + ) + + +def ilp_set_cover(V: sp.csr_matrix, time_limit: float = 120.0) -> CoverResult: + from ortools.sat.python import cp_model + + t0 = time.perf_counter() + Vcsc = V.tocsc() + M, N = V.shape + coverable = _coverable(V) + + model = cp_model.CpModel() + x = [model.new_bool_var(f"x{j}") for j in range(M)] + for i in np.flatnonzero(coverable): + rows = Vcsc.indices[Vcsc.indptr[i]:Vcsc.indptr[i + 1]] + model.add(sum(x[j] for j in rows) >= 1) + model.minimize(sum(x)) + + solver = cp_model.CpSolver() + solver.parameters.max_time_in_seconds = float(time_limit) + solver.parameters.num_workers = 8 + status = solver.solve(model) + if status not in (cp_model.OPTIMAL, cp_model.FEASIBLE): + raise RuntimeError(f"CP-SAT ohne Loesung ({solver.status_name(status)})") + + sel = np.asarray([j for j in range(M) if solver.value(x[j])], dtype=np.int64) + times = np.asarray(V.tocsr()[sel].sum(axis=0)).ravel() if sel.size else np.zeros(N) + n_cov = int((coverable & (times >= 1)).sum()) + return CoverResult( + poses=sel, + method="ilp", + n_coverable=int(coverable.sum()), + n_covered=n_cov, + runtime=time.perf_counter() - t0, + optimal=status == cp_model.OPTIMAL, + extra={"solver_status": solver.status_name(status)}, + ) diff --git a/student_code/260722_UAVViewPlanning/vpp2d/stepviz.py b/student_code/260722_UAVViewPlanning/vpp2d/stepviz.py new file mode 100644 index 0000000..b8bb971 --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp2d/stepviz.py @@ -0,0 +1,211 @@ +from __future__ import annotations + +import argparse +import dataclasses +import sys +from pathlib import Path + +import numpy as np + +try: + sys.stdout.reconfigure(encoding="utf-8") +except (AttributeError, ValueError): + pass + +from .config import load_config +from . import scene as scene_mod +from .candidates import sample_pose_regions, project_and_dedup +from .visibility import compute_visibility +from .setcover import greedy_set_cover +from .plotting import _draw_scene + +_OUT = Path(__file__).with_name("output") + +C_NONCOVER = "0.55" +C_OPEN = "#e34a33" +C_DONE = "#31a354" +C_NEW = "#fd8d3c" +C_OVERLAP = "#3182bd" + + +class StepData: + def __init__(self, scene, poses, vis, cfg, trace): + self.scene = scene + self.poses = poses + self.vis = vis + self.cfg = cfg + self.trace = trace + self.coverable = vis.coverage_per_facet() > 0 + self.n_coverable = int(self.coverable.sum()) + self.n_steps = len(trace) + + +def build(args) -> StepData: + cfg = load_config(args.config) + if args.baseline is not None: + cfg = dataclasses.replace(cfg, baseline=args.baseline) + + if args.stl: + scene = scene_mod.from_stl_slice(args.stl, level=args.stl_level, + resolution=cfg.resolution) + else: + scene = scene_mod.SCENES[args.scene](cfg.resolution) + + raw = sample_pose_regions(scene, cfg) + poses, _ = project_and_dedup(raw, scene, cfg) + vis = compute_visibility(scene, poses, cfg) + + trace: list = [] + res = greedy_set_cover(vis.V, trace=trace) + print(f"{scene.name}: {res.summary()}") + print(f" -> {len(trace)} Greedy-Schritte aufgezeichnet.") + return StepData(scene, poses, vis, cfg, trace) + + +def draw_step(ax, data: StepData, k: int) -> None: + from matplotlib.patches import Wedge + + ax.clear() + sc = data.scene + fc = sc.facets + cfg = data.cfg + _draw_scene(ax, sc) + + if k == 0: + covered_before = np.zeros(len(fc), dtype=bool) + new = np.empty(0, dtype=int) + overlap = np.empty(0, dtype=int) + pose = None + else: + st = data.trace[k - 1] + covered_before = st["covered_before"] + new = st["new_facets"] + overlap = st["overlap_facets"] + pose = st["pose"] + + open_mask = data.coverable & ~covered_before + if k > 0: + open_mask[new] = False + ax.scatter(fc.centers[~data.coverable, 0], fc.centers[~data.coverable, 1], + c=C_NONCOVER, s=12, zorder=2, label="unerreichbar") + ax.scatter(fc.centers[covered_before, 0], fc.centers[covered_before, 1], + c=C_DONE, s=14, zorder=2, label="abgedeckt") + ax.scatter(fc.centers[open_mask, 0], fc.centers[open_mask, 1], + c=C_OPEN, s=14, zorder=2, label="offen") + + if pose is not None: + p = data.poses.positions[pose] + yaw = data.poses.yaws[pose] + yaw_deg = np.degrees(yaw) + half = np.degrees(cfg.fov_rad / 2.0) + ax.add_patch(Wedge(p, cfg.d_max, yaw_deg - half, yaw_deg + half, + width=cfg.d_max - cfg.d_min, facecolor="gold", + alpha=0.18, edgecolor="goldenrod", lw=0.8, zorder=3)) + for f in overlap: + ax.plot([p[0], fc.centers[f, 0]], [p[1], fc.centers[f, 1]], + color=C_OVERLAP, lw=0.6, alpha=0.7, zorder=4) + for f in new: + ax.plot([p[0], fc.centers[f, 0]], [p[1], fc.centers[f, 1]], + color=C_NEW, lw=0.8, alpha=0.9, zorder=4) + if len(overlap): + ax.scatter(fc.centers[overlap, 0], fc.centers[overlap, 1], + c=C_OVERLAP, s=42, edgecolor="white", lw=0.5, zorder=7, + label="Überlappung") + if len(new): + ax.scatter(fc.centers[new, 0], fc.centers[new, 1], + c=C_NEW, s=42, edgecolor="white", lw=0.5, zorder=7, + label="neu erfasst") + u = np.array([np.cos(yaw), np.sin(yaw)]) + ax.plot(*p, marker="o", color="black", ms=8, zorder=8) + ax.annotate("", xy=p + u * 0.9, xytext=p, + arrowprops=dict(arrowstyle="-|>", color="black", lw=1.5), + zorder=8) + + done = int(covered_before.sum()) + (len(new) if k > 0 else 0) + frac = done / max(1, data.n_coverable) + if k == 0: + title = (f"Schritt 0 / {data.n_steps} — Start: 0 Posen, " + f"0 / {data.n_coverable} erfasst (0 %)") + else: + title = (f"Schritt {k} / {data.n_steps} — Pose #{pose}\n" + f"+{len(new)} neu, {len(overlap)} Überlappung | " + f"{done} / {data.n_coverable} erfasst ({frac:.0%}), {k} Posen") + ax.set_title(title, fontsize=10) + ax.legend(loc="upper right", fontsize=7, framealpha=0.9) + + +def save_frames(data: StepData, outdir: Path) -> None: + import matplotlib + matplotlib.use("Agg") + import matplotlib.pyplot as plt + + outdir.mkdir(parents=True, exist_ok=True) + fig, ax = plt.subplots(figsize=(9, 8)) + for k in range(data.n_steps + 1): + draw_step(ax, data, k) + fig.tight_layout() + fig.savefig(outdir / f"step_{k:03d}.png", dpi=110) + plt.close(fig) + print(f" -> {data.n_steps + 1} Frames in {outdir}") + + +def interactive(data: StepData) -> None: + import matplotlib + try: + matplotlib.use("TkAgg") + except Exception: + pass + import matplotlib.pyplot as plt + + state = {"k": 0} + fig, ax = plt.subplots(figsize=(9, 8)) + + def redraw(): + draw_step(ax, data, state["k"]) + fig.canvas.draw_idle() + + def on_key(event): + if event.key in ("right", "n", " "): + state["k"] = min(state["k"] + 1, data.n_steps) + redraw() + elif event.key in ("left", "b"): + state["k"] = max(state["k"] - 1, 0) + redraw() + elif event.key == "home": + state["k"] = 0; redraw() + elif event.key == "end": + state["k"] = data.n_steps; redraw() + elif event.key == "s": + _OUT.mkdir(exist_ok=True) + f = _OUT / f"{data.scene.name}_step_{state['k']:03d}.png" + fig.savefig(f, dpi=120); print(f" gespeichert: {f}") + elif event.key == "q": + plt.close(fig) + + fig.canvas.mpl_connect("key_press_event", on_key) + print("Navigation: → / n = vor, ← / b = zurück, Home/End, s = speichern, q = schließen") + redraw() + plt.show() + + +def main() -> None: + ap = argparse.ArgumentParser(description="vpp2d — Schritt-für-Schritt-Debug-Viewer") + ap.add_argument("--scene", default="notched_box", choices=list(scene_mod.SCENES)) + ap.add_argument("--stl", default=None) + ap.add_argument("--stl-level", type=float, default=None) + ap.add_argument("--baseline", type=float, default=None) + ap.add_argument("--config", default=None) + ap.add_argument("--show", action="store_true", + help="interaktiver Navigator statt PNG-Frames") + args = ap.parse_args() + + data = build(args) + if args.show: + interactive(data) + else: + name = data.scene.name + save_frames(data, _OUT / f"steps_{name}_greedy") + + +if __name__ == "__main__": + main() diff --git a/student_code/260722_UAVViewPlanning/vpp2d/visibility.py b/student_code/260722_UAVViewPlanning/vpp2d/visibility.py new file mode 100644 index 0000000..f73a421 --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp2d/visibility.py @@ -0,0 +1,123 @@ +from __future__ import annotations + +import time +from dataclasses import dataclass, field + +import numpy as np +import scipy.sparse as sp + +from .candidates import Poses +from .config import Config +from .scene import Scene + + +@dataclass +class VisibilityMatrix: + V: sp.csr_matrix + stats: dict = field(default_factory=dict) + runtime: float = 0.0 + + @property + def shape(self) -> tuple[int, int]: + return self.V.shape + + def coverage_per_facet(self) -> np.ndarray: + return np.asarray(self.V.sum(axis=0)).ravel() + + def summary(self) -> str: + cov = self.coverage_per_facet() + reach = int((cov > 0).sum()) + n = self.V.shape[1] + return ( + f"Coverage-Matrix V: {self.V.shape[0]} Posen × {n} Facetten, " + f"{self.V.nnz:,} Sichtbarkeiten\n" + f" erreichbare Facetten: {reach}/{n} ({reach / max(1, n):.1%})\n" + f" Rechenzeit : {self.runtime:.2f} s" + ) + + +def _rays_clear(o: np.ndarray, cs: np.ndarray, edges: np.ndarray) -> np.ndarray: + o = np.asarray(o, float) + r = cs - o + a = edges[:, 0, :] + e = edges[:, 1, :] - a + diff = a - o + denom = r[:, 0:1] * e[:, 1] - r[:, 1:2] * e[:, 0] + num_t = diff[:, 0] * e[:, 1] - diff[:, 1] * e[:, 0] + num_s = diff[None, :, 0] * r[:, 1:2] - diff[None, :, 1] * r[:, 0:1] + with np.errstate(divide="ignore", invalid="ignore"): + t = num_t[None, :] / denom + s = num_s / denom + parallel = np.abs(denom) < 1e-12 + hit = (~parallel) & (t > 1e-6) & (t < 1 - 1e-4) & (s > -1e-9) & (s < 1 + 1e-9) + return ~hit.any(axis=1) + + +def compute_visibility(scene: Scene, poses: Poses, cfg: Config) -> VisibilityMatrix: + t0 = time.perf_counter() + C = scene.facets.centers + Nn = scene.facets.normals + edges = scene.edges + cos_fov = np.cos(cfg.fov_rad / 2.0) + cos_theta = np.cos(cfg.theta_max_rad) + + rows, cols = [], [] + stat = {"dist": 0, "incidence": 0, "fov": 0, "occlusion_single": 0, "dual": 0} + + for m in range(len(poses)): + p = poses.positions[m] + yaw = poses.yaws[m] + u = np.array([np.cos(yaw), np.sin(yaw)]) + right = np.array([u[1], -u[0]]) + p_cam = p + cfg.baseline * right + + r = C - p + dist = np.linalg.norm(r, axis=1) + ok = (dist >= cfg.d_min) & (dist <= cfg.d_max) + stat["dist"] += int(ok.sum()) + if not ok.any(): + continue + + rn = r / dist[:, None] + cos_inc = -(Nn * rn).sum(axis=1) + ok &= cos_inc >= cos_theta + stat["incidence"] += int(ok.sum()) + if not ok.any(): + continue + + cos_view = (rn * u).sum(axis=1) + ok &= cos_view >= cos_fov + r_cam = C - p_cam + dist_cam = np.linalg.norm(r_cam, axis=1) + cos_view_cam = (r_cam * u).sum(axis=1) / np.maximum(dist_cam, 1e-12) + ok &= cos_view_cam >= cos_fov + stat["fov"] += int(ok.sum()) + if not ok.any(): + continue + + idx = np.flatnonzero(ok) + clear_p = _rays_clear(p, C[idx], edges) + stat["occlusion_single"] += int(clear_p.sum()) + idx = idx[clear_p] + if idx.size == 0: + continue + if cfg.baseline > 0: + clear_c = _rays_clear(p_cam, C[idx], edges) + idx = idx[clear_c] + stat["dual"] += int(idx.size) + if idx.size == 0: + continue + + rows.append(np.full(idx.size, m, dtype=np.int64)) + cols.append(idx) + + if rows: + rows = np.concatenate(rows) + cols = np.concatenate(cols) + else: + rows = cols = np.empty(0, dtype=np.int64) + V = sp.csr_matrix( + (np.ones(len(rows), dtype=bool), (rows, cols)), + shape=(len(poses), len(scene.facets)), + ) + return VisibilityMatrix(V=V, stats=stat, runtime=time.perf_counter() - t0) diff --git a/student_code/260722_UAVViewPlanning/vpp3d/README.md b/student_code/260722_UAVViewPlanning/vpp3d/README.md new file mode 100644 index 0000000..17c5af4 --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp3d/README.md @@ -0,0 +1,299 @@ +# vpp3d — 3D-Portierung des alternativen View-Planning-Ansatzes + +Eigenständige, **vom restlichen Code getrennte** 3D-Portierung des 2D-Prototyps +[`../vpp2d`](../vpp2d/README.md). Dieselbe Pipeline und Constraint-Struktur, +aber im vollen Raum: die 4-DoF-Kinematik (x, y, z, yaw) ist hier vollständig +ausgeprägt. + +> **Fokus: reine Sichtlinie (line of sight)** für die Drohnen-Sensorabdeckung. +> Das innere Set Cover ist single-level über die Sichtbarkeitsmatrix. Ergänzt um +> die **Registrierungs-Überlappung** ([5], `overlap.py` / `setcover.py`) — als +> Konnektivität des flächenbasierten Registrierungsgraphen, entweder nachgelagert +> repariert oder gemeinsam mit der Abdeckung als **Connected Set Cover** exakt +> gelöst — und ein **LoS-Tracking-Modell** für die bodengebundenen mobilen +> Tracker (`tracking.py`). + +## Was gegenüber 2D hinzukommt + +| Aspekt | 2D (vpp2d) | 3D (vpp3d) | +|---|---|---| +| Primitiv | Polygon-Kantensegmente | **Mesh-Dreiecke** (zur Auflösung midpoint-unterteilt) | +| Pose | (x, y, yaw) | (x, y, z, yaw) — **Pitch geklemmt** auf den Gimbal-Bereich, Roll gesperrt | +| Sichtfeld | Kreissegment | **Sehpyramide** (horizontal × vertikal) | +| Kegel-Sampling | Inzidenzwinkel in der Ebene | Frontalstrahl + **Azimut-Ringe** um die Normale | +| Dedup-Schlüssel | (Voxel x,y) × Yaw-Bin | (Voxel x,y,z) × Yaw-Bin × **Pitch-Bin** | +| Raycast | analytisch (Kanten) | **Open3D** `RaycastingScene` gegen das Mesh | +| Visualisierung | matplotlib 2D | matplotlib-3D-PNG **+ Open3D-Interaktiv** | + +Der **Machbarkeitsfilter** nach der Pitch-Klemmung verwirft Posen, bei denen die +Facette aus dem vertikalen Sichtfeld fällt — dadurch sind *horizontale* Flächen +(z. B. die Würfeloberseite) physikalisch unabdeckbar (Strahlen müssten zu steil +geneigt sein). Das ist bewusst akzeptiert: Vollständigkeit gilt relativ zur +*erreichbaren* Fläche. + +## Pipeline (Spiegel des Bild-Entwurfs, in 3D) + +| Schritt | Modul | Inhalt | +|---|---|---| +| [1] Facetten als Primitiv | `scene.py` | Mesh-Dreiecke → Facetten (Mittelpunkt, Außennormale, Fläche) | +| [2] Posenregion je Facette samplen | `candidates.sample_pose_regions` | Kegel-Sampling um die Normale × Distanzschale | +| [3] Projektion auf 4-DoF-Raum + Dedup | `candidates.project_and_dedup` | Yaw exakt, Pitch geklemmt; Voxel/Yaw/Pitch-Dedup | +| [4] Coverage-Matrix per Raycast, duale Sicht | `visibility.py` | Projektor + Kamera, Open3D-Raycast | +| [5] Registrierungs-Überlappung | `overlap.py` | flächenbasierter Registrierungsgraph + Konnektivitäts-Reparatur (`ensure_connected`) | +| [5b] Lokale Pose-Verfeinerung | `refine.py` | gewählte Posen per Nelder-Mead auf ein glattes Qualitätsoptimum schieben (optional, `--refine`) | +| [6] Set Cover (Greedy + ILP + Connected) | `setcover.py` | Greedy/ILP (rein matrixbasiert) + `connected_set_cover_ilp` (Abdeckung + Zusammenhang gemeinsam) | +| Tracking-Standorte (LoS) | `tracking.py` | bodengebundene Tracker, Set Cover mit Sichtlinien-Raycast | +| [7] TSP-Tour | `sequencing.py` | zweistufig: TSP über Tracking-Standorte + offener TSP je Segment (NN + 2-opt, 3D-Distanzen) | + +### [5] Registrierungs-Überlappung (`overlap.py` / `setcover.py`) + +Set Cover garantiert per Default (`--k 1`) nur, dass jede erreichbare Facette von +*mindestens einem* Scan gesehen wird — nicht, dass sich die Scans zu *einem* +gemeinsamen Koordinatensystem registrieren lassen. Die ICP-Registrierung braucht, +dass benachbarte Scans gemeinsame Fläche teilen. + +Modelliert als **Registrierungsgraph**: Knoten = gewählte Scans, Kante (a, b), +wenn beide Scans mindestens `min_overlap` Anteil gemeinsam gesehener Facetten +teilen — **bezogen auf den kleineren der beiden Scans**. Bei uniformer +Diskretisierung ist die Facettenzahl proportional zur Fläche, d. h. `min_overlap` +ist ein echter **Flächen-Anteil**: `0.25` heißt „25 % der Fläche des kleineren +Scans werden auch vom Nachbarn gesehen". (Der frühere Ansatz maß das *äußere +Rand-Band* im Bild-Radius — 25 % Radius ≈ 44 % Fläche — und erzwang viele +unnötige Brücken-Posen.) + +Gefordert ist der **Zusammenhang** des Graphen (jeder Scan über eine Kette von +Paar-Registrierungen ins Weltsystem verkettbar). Zwei Wege: + +* **Zweistufig** — `overlap.ensure_connected` ergänzt nach dem Set Cover greedy + Brücken-Posen, bis der Graph zusammenhängt. Läuft fair nach Greedy *und* ILP + (`--method greedy|ilp`). +* **Gemeinsam (exakt)** — `setcover.connected_set_cover_ilp` (`--method + connected`) optimiert Abdeckung und Zusammenhang in *einem* CP-SAT-Modell + (Single-Commodity-Flow über die Overlap-Kanten). Näher am echten Optimum, weil + die Basisauswahl die Registrierbarkeit schon berücksichtigt statt sie + nachträglich zu reparieren; als exakte Referenz für kleine/mittlere Instanzen + gedacht. + +In der Visualisierung sind die Facetten, die **≥ 2 Scans** sehen (die tatsächliche +Registrierungs-Überlappung), **orange** hervorgehoben. + +### [5b] Lokale Pose-Verfeinerung (`refine.py`) + +Die Kandidatengenerierung [2/3] erzeugt Posen auf einem **diskreten Raster** +(feste Distanz-Stützstellen × Azimut-Ringe). Die vom Set Cover gewählten Posen +sitzen daher selten im Optimum ihres Footprints — oft etwas zu nah/fern, mit +schrägem Einfall oder am Rand des Sichtfelds. `refine.py` verschiebt jede +gewählte Pose einzeln per **Nelder-Mead** auf ein lokales Optimum einer glatten +Qualitäts-Score + + J = q_dist · q_incidence · q_frustum + +(Distanz-Glocke um `d_opt` · Einfalls-Smoothstep bis `θ_max` · Frustum- +Zentrierung). Die Orientierung ist kein freier Parameter, sondern wird je Schritt +auf den qualitätsgewichteten Facetten-Schwerpunkt ausgerichtet (Yaw exakt, Pitch +geklemmt, Roll = 0 — 4-DoF wie [2/3]). + +**Abdeckungsbewusst (Default):** Jede Pose darf sich nur so weit verschieben, +dass sie ihre vom Set Cover zugewiesenen Facetten (Zeile von V) weiterhin sieht — +eine Barriere bestraft jede zugewiesene Facette, die unter die Sichtbarkeit +rutscht. Ohne diese Einschränkung kollabieren benachbarte Posen aufeinander und +die Abdeckung bricht ein (Ablation `--refine-free`: `notched_box` fällt von +68,8 % auf 53,8 % erreichbarer Facetten). + +**Abdeckungsmaximierend (`--gain-weight g`):** Zusätzlich zu den zugewiesenen +Facetten werden benachbarte, noch erreichbare Facetten mit Gewicht `g` in die +Score aufgenommen — die Pose nimmt so weitere Facetten mit. Die Distanz-Glocke +`q_dist` verhindert dabei, dass sie zum Einsammeln entfernter Facetten auf +`d_max` driftet (dort → `q_dist = 0`, und die Barriere der zugewiesenen Facetten +schlägt zu). Auf `box`: weiche Abdeckung +8,9 % Facetten/Pose bei weiter +sinkendem `|d − d_opt|` (−36 %), also **kein** d_max-Drift. + +Die Score ist rein geometrisch (kein Raycast); nach der Verschiebung wird die +Sichtbarkeit [4] neu berechnet (maßgeblicher Verdeckungs-Check) — `run.py` gibt +die neu geraycastete Abdeckung vorher/nachher aus. Beispiel `box` +(abdeckungsbewusst): `|d − d_opt|` −66 %, Einfallswinkel 16,6° → 13,9°, Abdeckung +66,7 % → 66,4 % (nahezu erhalten). + +### Tracking mit Sichtlinie (`tracking.py`) + +Die bodengebundenen, mobilen Tracking-Roboter brauchen **freie Sichtlinie** zur +Drohne. Kandidaten-Standorte liegen auf einem Bodenraster über der Posen-Hülle; +ein Standort erfasst eine Pose, wenn die Drohne im Arbeitsabstand +`[range_min, range_max]` liegt **und** der Strahl Tracker → Drohne unverdeckt am +Messobjekt vorbeigeht (Open3D-Raycast). Minimale Standortmenge = Greedy-Set-Cover +über die Posen; jede Pose wird dem nächstgelegenen erfassenden Standort +zugeordnet (blaue Marker + Sichtlinien in der Visualisierung). + +## Aufruf + +Aus dem Verzeichnis `Code/` (venv mit numpy/scipy/open3d/ortools/matplotlib): + +```powershell +.venv\Scripts\python -m vpp3d.run # box, Greedy +.venv\Scripts\python -m vpp3d.run --scene notched_box --method both # Greedy vs. ILP +.venv\Scripts\python -m vpp3d.run --mesh Meshes/TestKorper1.stl --method both +.venv\Scripts\python -m vpp3d.run --mesh Meshes/EasyCube.ply --assume-convex --resolution 0.6 # Punktwolke -> Poisson +.venv\Scripts\python -m vpp3d.run --scene notched_box --baseline 0 # Ablation: Einzelsicht +.venv\Scripts\python -m vpp3d.run --mesh Meshes/TestKorper1.stl --show # + Open3D-Ansicht +.venv\Scripts\python -m vpp3d.run --scene box --refine # [5b] gewählte Posen lokal verfeinern +.venv\Scripts\python -m vpp3d.run --scene box --refine --gain-weight 0.3 # [5b] + Abdeckung maximieren (kein d_max-Drift) +.venv\Scripts\python -m vpp3d.run --scene notched_box --refine --refine-free # Ablation: Abdeckung bricht ein +``` + +Verfeinerungs-Optionen (`--refine`): `--refine-free` (Ablation ohne +Facetten-Zuweisung), `--gain-weight g` (Abdeckungsmaximierung, Default 0 = aus), +`--max-offset` (Suchradius je Pose, Default 0,5·d_opt), `--sigma-scale` (Breite +der Distanz-Glocke), `--coverage-weight` (Abdeckungs-Barriere, Default 4,0), +`--z-min` (Bodenfilter). + +Statische Ergebnis-PNGs (Kandidatenposen + Lösung mit Facettenstatus, gewählten +Posen und Route) landen in `vpp3d/output/`. `--show` öffnet zusätzlich die +interaktive Open3D-Ansicht. + +### Schritt-für-Schritt-Viewer (Greedy) + +`stepviz.py` zeigt den Aufbau der Abdeckung Pick für Pick (Pendant zu +`../vpp2d/stepviz.py`): je Schritt die gewählte Pose mit Blickrichtung, ihre +Sichtstrahlen und die **neu** (orange) vs. **überlappend** (blau) erfassten +Facetten; bereits abgedeckt = grün, offen = rot, unerreichbar = grau. + +```powershell +.venv\Scripts\python -m vpp3d.stepviz --scene notched_box # PNG-Frames je Schritt +.venv\Scripts\python -m vpp3d.stepviz --scene notched_box --show # interaktiver Navigator +.venv\Scripts\python -m vpp3d.stepviz --mesh Meshes/TestKorper1.stl +``` + +Im `--save`-Modus (Default) landen die Frames in +`vpp3d/output/steps__greedy/step_000.png …`. Im `--show`-Modus blättern +`→`/`n` und `←`/`b` durch die Schritte (Home/End = Anfang/Ende, `s` = Frame +speichern); die Kameraperspektive bleibt beim Blättern erhalten. + +### Interaktiver Inspektor (`inspector.py`) + +`inspector.py` ist der Haupt-Debug-Viewer für die fertige Lösung. Er führt die +komplette Pipeline ([2]–[7]) intern aus und zeigt das Meshobjekt mit korrekt +**gefärbten Facetten-Dreiecken** (kein Punktraster — echte Mesh-Flächen). + +```powershell +# Synthetische Szene +.venv\Scripts\python -m vpp3d.inspector +.venv\Scripts\python -m vpp3d.inspector --scene notched_box + +# Reales Mesh / Punktwolke +.venv\Scripts\python -m vpp3d.inspector --mesh Meshes/TestKorper1.stl +.venv\Scripts\python -m vpp3d.inspector --mesh Meshes/TestKorper1.stl --method both +.venv\Scripts\python -m vpp3d.inspector --mesh Meshes/EasyCube.ply --assume-convex + +# ILP statt Greedy; Connected Set Cover (Abdeckung + Zusammenhang gemeinsam) +.venv\Scripts\python -m vpp3d.inspector --method ilp --time-limit 120 +.venv\Scripts\python -m vpp3d.inspector --method connected --time-limit 120 +# Konnektivitäts-Reparatur deaktivieren +.venv\Scripts\python -m vpp3d.inspector --no-overlap + +# [5b] Lokale Pose-Verfeinerung (Sichtbarkeit wird danach neu geraycastet) +.venv\Scripts\python -m vpp3d.inspector --refine +.venv\Scripts\python -m vpp3d.inspector --refine --gain-weight 0.3 +``` + +#### CLI-Optionen + +| Option | Standard | Beschreibung | +|---|---|---| +| `--scene` | `box` | Synthetische Testszene: `box`, `notched_box` | +| `--mesh` | — | Reales Mesh oder Punktwolke (STL/PLY); ersetzt `--scene` | +| `--assume-convex` | aus | Normalen radial nach außen orientieren (für Punktwolken ohne Mesh-Normalen) | +| `--resolution` | Config | Facetten-Auflösung in m überschreiben — kleinere Werte → mehr Facetten → langsamere Berechnung | +| `--method` | `greedy` | Set-Cover-Solver: `greedy`, `ilp` oder `connected` (Connected Set Cover: Abdeckung + Zusammenhang gemeinsam) | +| `--k` | `1` | k-Coverage: jede Facette von ≥ k Posen sehen lassen (Redundanz für die Registrierung); Bedarf auf erreichbare Abdeckung gekappt | +| `--time-limit` | `60` | ILP-Zeitlimit in Sekunden | +| `--baseline` | Config | Clearance-Baseline in m; `0` = Einzelsicht-Ablation | +| `--no-overlap` | aus | Konnektivitäts-Reparatur (Schritt [5]) überspringen | +| `--refine` | aus | [5b] gewählte Posen lokal verfeinern; danach wird die Sichtbarkeit neu geraycastet (Einfärbung zeigt die verfeinerten Posen) | +| `--refine-free` | aus | Ablation zu `--refine`: ohne Facetten-Zuweisung (Abdeckung bricht ein) | +| `--gain-weight` | `0` | `--refine`: > 0 nimmt benachbarte Facetten mit Gewicht `g` in die Score auf (Abdeckungsmaximierung, kein d_max-Drift) | +| `--max-offset` | `0.5·d_opt` | `--refine`: max. Suchradius je Pose in m | +| `--sigma-scale` | `1.5` | `--refine`: Breite der Distanz-Glocke | +| `--coverage-weight` | `4.0` | `--refine`: Abdeckungs-Barriere je zugewiesener Facette (0 = aus) | +| `--z-min` | — | `--refine`: Bodenfilter, Posen nicht unter z_min schieben | +| `--config` | `config.toml` | Alternative Konfigurationsdatei | + +#### Tastenkürzel + +| Taste | Funktion | +|---|---| +| `O` / `3` | **Übersicht** — alle Plan-Posen, Gesamtabdeckungsstatus | +| `M` / `4` | **Mosaik** — jede Pose hat eine Farbe, ihr Footprint entsprechend; Registrierungs-Überlappung (≥2 Scans) gelb; Kandidaten ausgeblendet, Kamera-Pyramiden sichtbar | +| `C` / `1` | **Kandidaten** — alle Kandidatenposen, eine nach der anderen durchsteppen | +| `P` / `2` | **Plan** — nur gewählte Posen, mit FoV-Frustum der aktiven Pose | +| `N` / `→` | Nächste Pose (Kandidaten- und Plan-Modus) | +| `B` / `←` | Vorherige Pose | +| `S` | Screenshot → `vpp3d/output/inspector____.png` | +| `Q` / `Esc` | Viewer schließen | + +#### Facetten-Einfärbung je Modus + +| Farbe | Kandidaten | Plan | Übersicht | Mosaik | +|---|---|---|---|---| +| Hellgrün | Von akt. Pose sichtbar | Von akt. Pose sichtbar | Abgedeckt (Plan) | Farbe der zuständigen Plan-Pose | +| Blassgrün | — | Abgedeckt (andere Plan-Posen) | — | — | +| Rot | — | Coverable, nicht abgedeckt | Coverable, nicht abgedeckt | Coverable, nicht abgedeckt | +| Hellgrau | Coverable, nicht sichtbar | — | — | — | +| Grau | Unerreichbar | Unerreichbar | Unerreichbar | Unerreichbar | +| Gelb | — | — | — | Registrierungs-Überlappung (von ≥ 2 Scans gesehen) | + +Im **Mosaik-Modus** werden zusätzlich kleine **Kamera-Pyramiden** für jede Plan-Pose +eingeblendet — in der Palettenfarbe der jeweiligen Pose, mit der tatsächlichen +FoV-Öffnung skaliert. + +--- + +### Laufzeit-Logging (`runlog.py`) + +Jeder Aufruf von `run.py` und `inspector.py` legt automatisch einen Ordner +`vpp3d/output/runs/_/` an. Darin werden gespeichert: + +| Datei | Inhalt | +|---|---| +| `visibility_matrix.png` | Sichtbarkeitsmatrix der gewählten Posen × Facetten als Heatmap; Spalten nach Abdeckungsdichte sortiert; bei sehr großen Matrizen heruntergesampelt (max. 2000 × 800 px) | +| `_.png` | Kopie der Übersichts-PNG (nur `run.py`) | + +--- + +### Testszenen + +- `box` — konvexer 4-m-Würfel, 3D-Analogon zu **EasyCube** (alle Seitenflächen + frei einsehbar; Ober-/Unterseite horizontal → unerreichbar). +- `notched_box` — Quader (4,4 × 2 × 3,1 m) mit rechteckiger Tasche, 3D-Analogon + zu **TestKorper1**: erzeugt Selbstverdeckung (die duale Sicht verwirft Posen, + bei denen die Kamera durch die Taschenkante verdeckt wird) und 90°-Kanten. +- `--mesh ` — reales Mesh (STL/PLY). Punktwolken (z. B. `EasyCube.ply`) + werden per Poisson-Rekonstruktion zu einem Occluder-Mesh; `--assume-convex` + orientiert die Normalen radial nach außen. Für große Objekte (EasyCube.ply ist + ~11 × 11 × 6 m) die Facetten-Auflösung gröber wählen (`--resolution 0.6`), + sonst explodiert die Facetten- und Posenzahl. + +## Beispielergebnisse (Default-Config, duale Sicht, baseline = 0,4 m) + +| Szene | erreichbar | Greedy | ILP | Faktor | +|---|---|---|---|---| +| box | 8192/12288 (66,7 %) | 44 | 36 | 1,22 | +| notched_box | 16312/28672 (56,9 %) | 64 | 53 | 1,21 | +| TestKorper1.stl | 5120/7168 (71,4 %) | 39 | 29 | 1,34 | + +Zum Vergleich liefert die große Pipeline für TestKorper1 Greedy 37 / ILP 30 — +der alternative Ansatz reproduziert die Größenordnung. Die Einzelsicht-Ablation +(`--baseline 0`) braucht für `notched_box` 62 statt 64 Scans, weil die +Kamera-Verdeckungsprüfung an der Tasche entfällt. + +## Nicht enthalten / Abgrenzung + +- Kein bi-level Set Cover: Tracking-Standorte werden *nach* der Posenwahl + bestimmt (nachgelagert), nicht gemeinsam mit ihr optimiert. Die + *Sequenzierung* koppelt beide jedoch bereits zweistufig (äußere Tour über die + Standorte, innere offene TSP je Segment), sodass die Drohne nicht mehr + zwischen Tracking-Bereichen hin- und herspringt. +- Kollision nur als 1-NN-Clearance-Proxy; echte Freiraumprüfung (ESDF/OctoMap) + erst im ROS2/Gazebo-Teil. +- Euklidische Distanzen in der Sequenzierung (keine kollisionsfreie Roadmap). +- Occluder ist das grobe Eingangs-Mesh; nur die Facetten werden zur Auflösung + unterteilt (kein Crack/T-Stoß, da uniform unterteilt). diff --git a/student_code/260722_UAVViewPlanning/vpp3d/__init__.py b/student_code/260722_UAVViewPlanning/vpp3d/__init__.py new file mode 100644 index 0000000..e1b5a34 --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp3d/__init__.py @@ -0,0 +1,25 @@ +from .config import Config, load_config +from .scene import Scene, Facets, build_scene, box, notched_box, from_file, 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, connected_set_cover_ilp, +) +from .overlap import ( + OverlapResult, ensure_connected, registration_graph, overlap_adjacency, +) +from .sequencing import Route, sequence_route +from .refine import ( + RefineInfo, pose_quality, refine_poses, target_facets_from_visibility, +) + +__all__ = [ + "Config", "load_config", + "Scene", "Facets", "build_scene", "box", "notched_box", "from_file", "SCENES", + "Poses", "sample_pose_regions", "project_and_dedup", + "VisibilityMatrix", "compute_visibility", + "CoverResult", "greedy_set_cover", "ilp_set_cover", "connected_set_cover_ilp", + "OverlapResult", "ensure_connected", "registration_graph", "overlap_adjacency", + "Route", "sequence_route", + "RefineInfo", "pose_quality", "refine_poses", "target_facets_from_visibility", +] diff --git a/student_code/260722_UAVViewPlanning/vpp3d/candidates.py b/student_code/260722_UAVViewPlanning/vpp3d/candidates.py new file mode 100644 index 0000000..1174896 --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp3d/candidates.py @@ -0,0 +1,148 @@ +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 + pitches: 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: + cp = np.cos(self.pitches) + return np.column_stack([ + cp * np.cos(self.yaws), + cp * np.sin(self.yaws), + np.sin(self.pitches), + ]) + + +def _tangent_basis(n: np.ndarray) -> tuple[np.ndarray, np.ndarray]: + ref = np.tile(np.array([0.0, 0.0, 1.0]), (len(n), 1)) + ref[np.abs(n[:, 2]) > 0.9] = np.array([1.0, 0.0, 0.0]) + t1 = np.cross(n, ref) + t1 /= np.maximum(np.linalg.norm(t1, axis=1, keepdims=True), 1e-12) + return t1, np.cross(n, t1) + + +def _cone_directions(Nn: np.ndarray, t1: np.ndarray, t2: np.ndarray, + cfg: Config) -> np.ndarray: + rings = [] + for frac in cfg.cone_fractions: + alpha = frac * cfg.theta_max_rad + if frac == 0.0: + rings.append(Nn[None, :, :]) + continue + az = np.linspace(0.0, 2 * np.pi, cfg.n_azimuth, endpoint=False) + ring = (np.cos(alpha) * Nn[None, :, :] + + np.sin(alpha) * (np.cos(az)[:, None, None] * t1[None, :, :] + + np.sin(az)[:, None, None] * t2[None, :, :])) + rings.append(ring) + return np.concatenate(rings, axis=0) + + +def sample_pose_regions(scene: Scene, cfg: Config) -> Poses: + C = scene.facets.centers + Nn = scene.facets.normals + t1, t2 = _tangent_basis(Nn) + dists = (np.array([cfg.d_opt]) if cfg.n_distance == 1 + else np.linspace(cfg.d_min, cfg.d_max, cfg.n_distance)) + + dirs = _cone_directions(Nn, t1, t2, cfg) + D, N, _ = dirs.shape + T = len(dists) + + positions = (C[None, None, :, :] + + dirs[:, None, :, :] * dists[None, :, None, None]).reshape(-1, 3) + vd = np.broadcast_to(-dirs[:, None, :, :], (D, T, N, 3)).reshape(-1, 3) + yaws = np.arctan2(vd[:, 1], vd[:, 0]) + pitches = np.arcsin(np.clip(vd[:, 2], -1.0, 1.0)) + seed = np.broadcast_to(scene.facets.ids[None, None, :], (D, T, N)).reshape(-1).copy() + + return Poses(positions, yaws, pitches, + np.arange(len(positions), dtype=np.int64), seed) + + +def _clearance_mask(positions: np.ndarray, scene: Scene, safety: np.ndarray) -> np.ndarray: + dist, _ = cKDTree(scene.facets.centers).query(positions, k=1) + return dist >= safety + + +def _vertical_fov_ok(positions: np.ndarray, yaws: np.ndarray, pitches: np.ndarray, + seed_centers: np.ndarray, cfg: Config) -> np.ndarray: + cp = np.cos(pitches) + fwd = np.column_stack([cp * np.cos(yaws), cp * np.sin(yaws), np.sin(pitches)]) + right = np.cross(fwd, np.array([0.0, 0.0, 1.0])) + right /= np.maximum(np.linalg.norm(right, axis=1, keepdims=True), 1e-12) + up = np.cross(right, fwd) + u = seed_centers - positions + x = np.einsum("ij,ij->i", u, fwd) + z = np.einsum("ij,ij->i", u, up) + return (x > 0.0) & (np.abs(z) <= x * np.tan(0.5 * cfg.fov_v_rad)) + + +def dedup_keys(pos: np.ndarray, yaw: np.ndarray, pitch: np.ndarray, + cfg: Config) -> np.ndarray: + return np.column_stack([ + np.floor((pos - pos.min(axis=0)) / cfg.voxel).astype(np.int64), + np.floor(np.mod(yaw, 2 * np.pi) / cfg.yaw_bin_rad).astype(np.int64), + np.floor(pitch / cfg.pitch_bin_rad).astype(np.int64), + ]) + + +def project_and_dedup(raw: Poses, scene: Scene, cfg: Config) -> tuple[Poses, dict]: + keep = _clearance_mask(raw.positions, scene, cfg.safety_distance) + pos, yaw, pitch, seed = (raw.positions[keep], raw.yaws[keep], + raw.pitches[keep], raw.seed_facet[keep]) + n_after_clear = len(pos) + + if cfg.use_ground_plane: + gok = pos[:, 2] >= scene.bounds[0][2] + cfg.ground_clearance + pos, yaw, pitch, seed = pos[gok], yaw[gok], pitch[gok], seed[gok] + n_after_ground = len(pos) + + pitch = np.clip(pitch, cfg.pitch_min_rad, cfg.pitch_max_rad) + feas = _vertical_fov_ok(pos, yaw, pitch, scene.facets.centers[seed], cfg) + pos, yaw, pitch, seed = pos[feas], yaw[feas], pitch[feas], seed[feas] + n_after_feas = len(pos) + + if n_after_feas == 0: + empty = Poses(np.empty((0, 3)), np.empty(0), np.empty(0), + np.empty(0, np.int64), np.empty(0, np.int64)) + return empty, {"n_raw": len(raw), "n_after_clearance": n_after_clear, + "n_after_ground": n_after_ground, + "n_after_feasibility": 0, "n_after_dedup": 0, "dedup_ratio": 0.0} + + _, first_idx = np.unique(dedup_keys(pos, yaw, pitch, cfg), axis=0, + return_index=True) + first_idx = np.sort(first_idx) + + poses = Poses( + positions=pos[first_idx], + yaws=yaw[first_idx], + pitches=pitch[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_ground": n_after_ground, + "n_after_feasibility": n_after_feas, + "n_after_dedup": len(poses), + "dedup_ratio": len(poses) / max(1, n_after_feas), + } + return poses, stats diff --git a/student_code/260722_UAVViewPlanning/vpp3d/config.py b/student_code/260722_UAVViewPlanning/vpp3d/config.py new file mode 100644 index 0000000..7671f62 --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp3d/config.py @@ -0,0 +1,131 @@ +from __future__ import annotations + +import math +import tomllib +from dataclasses import dataclass, replace +from pathlib import Path + +_DEFAULT_PATH = Path(__file__).with_name("config.toml") + + +def _rad(deg_field: str): + return property(lambda self: math.radians(getattr(self, deg_field))) + + +@dataclass(frozen=True) +class Config: + d_min: float + d_max: float + d_opt: float + + fov_h_deg: float + fov_v_deg: float + + baseline: float + + theta_max_deg: float + + pitch_min_deg: float + pitch_max_deg: float + + safety_distance: float + use_ground_plane: bool + ground_clearance: float + + min_overlap: float + erosion: float + + track_range_min: float + track_range_max: float + track_grid_step: float + track_margin: float + + n_azimuth: int + cone_fractions: tuple[float, ...] + n_distance: int + voxel: float + yaw_bin_deg: float + pitch_bin_deg: float + resolution: float + + fov_h_rad = _rad("fov_h_deg") + fov_v_rad = _rad("fov_v_deg") + theta_max_rad = _rad("theta_max_deg") + pitch_min_rad = _rad("pitch_min_deg") + pitch_max_rad = _rad("pitch_max_deg") + yaw_bin_rad = _rad("yaw_bin_deg") + pitch_bin_rad = _rad("pitch_bin_deg") + + def summary(self) -> str: + return ( + "Config(3D):\n" + f" Arbeitsabstand : [{self.d_min}, {self.d_max}] m (d_opt={self.d_opt})\n" + f" FoV (h x v) : {self.fov_h_deg}° x {self.fov_v_deg}° / Inzidenz ≤{self.theta_max_deg}°\n" + f" Pitch-Klemmung : [{self.pitch_min_deg}°, {self.pitch_max_deg}°] (Roll gesperrt)\n" + f" Basislinie : {self.baseline} m " + f"({'duale Sicht' if self.baseline > 0 else 'Einzelsicht'})\n" + f" Overlap (Reg.) : {self.min_overlap:.0%} gemeinsame Flaeche (kleinerer Scan)" + + (f" | Erosion {self.erosion:.2f} m" if self.erosion > 0 else "") + + "\n" + f" Bodenfilter : {'an' if self.use_ground_plane else 'aus'} " + f"(z_boden + {self.ground_clearance} m Bodenabstand)\n" + f" Tracking : LoS-Standorte, Reichweite [{self.track_range_min}, " + f"{self.track_range_max}] m, Raster {self.track_grid_step} m\n" + f" Sampling : {len(self.cone_fractions)}×{self.n_azimuth} Kegel-Richtungen " + f"× {self.n_distance}×Distanz, Dedup {self.voxel:.2f} m / " + f"{self.yaw_bin_deg}° / {self.pitch_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", {}) + orient = raw.get("orientation", {}) + trk = raw.get("tracking", {}) + + return Config( + d_min=d_min, + d_max=d_max, + d_opt=d_opt, + fov_h_deg=float(raw["fov"]["fov_h_deg"]), + fov_v_deg=float(raw["fov"]["fov_v_deg"]), + baseline=float(raw.get("sensor", {}).get("baseline", 0.0)), + theta_max_deg=float(raw["incidence"]["theta_max_deg"]), + pitch_min_deg=float(orient.get("pitch_min_deg", -10.0)), + pitch_max_deg=float(orient.get("pitch_max_deg", 10.0)), + safety_distance=float(raw["drone"]["safety_distance"]), + use_ground_plane=bool(raw["drone"].get("use_ground_plane", True)), + ground_clearance=float(raw["drone"].get("ground_clearance", + raw["drone"]["safety_distance"])), + min_overlap=float(raw["registration"]["min_overlap"]), + erosion=float(raw["registration"].get("erosion", 0.0)), + track_range_min=float(trk.get("range_min", 1.0)), + track_range_max=float(trk.get("range_max", 8.0)), + track_grid_step=float(trk.get("grid_step", 0.5)), + track_margin=float(trk.get("margin", 2.0)), + n_azimuth=int(smp.get("n_azimuth", 8)), + cone_fractions=tuple(float(x) for x in smp.get("cone_fractions", [0.0, 0.5, 0.85])), + n_distance=int(smp.get("n_distance", 2)), + voxel=float(smp.get("voxel", 0.15 * d_opt)), + yaw_bin_deg=float(smp.get("yaw_bin_deg", 15.0)), + pitch_bin_deg=float(smp.get("pitch_bin_deg", 10.0)), + resolution=float(smp.get("resolution", 0.25)), + ) + + +def with_overrides(cfg: Config, args) -> Config: + if getattr(args, "baseline", None) is not None: + cfg = replace(cfg, baseline=args.baseline) + if getattr(args, "resolution", None) is not None: + cfg = replace(cfg, resolution=args.resolution) + if getattr(args, "no_ground_plane", False): + cfg = replace(cfg, use_ground_plane=False) + return cfg diff --git a/student_code/260722_UAVViewPlanning/vpp3d/config.toml b/student_code/260722_UAVViewPlanning/vpp3d/config.toml new file mode 100644 index 0000000..29ffcae --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp3d/config.toml @@ -0,0 +1,98 @@ +# Constraints des 3D-Solver-Skeletts (vpp3d). +# +# Eigenstaendige Konfiguration des alternativen Ansatzes, 3D-Portierung von +# ../vpp2d/config.toml. Bewusst getrennt von der grossen Pipeline-Config +# (../config.toml), damit der Prototyp self-contained bleibt. Distanzen in +# Metern, Winkel in Grad. +# +# Gegenueber dem 2D-Prototyp kommt die dritte Dimension hinzu: die Drohne ist +# 4-DoF unteraktuiert (x, y, z, yaw). Roll ist gesperrt, Pitch wird auf den +# Gimbal-Bereich geklemmt. Das Sichtfeld ist eine Pyramide (horizontal x +# vertikal), die duale Sicht prueft Projektor UND Kamera per Raycast gegen das +# Dreiecksnetz. +# +# Kein Tracking-/Standortmodell: Repositionierung wird nicht optimiert. + +[working_distance] +# Arbeitsvolumen des Streifenprojektors: nahe/ferne Distanzgrenze [m]. +d_min = 2.8 +d_max = 3.2 +# Soll-Standoff [m] fuer die Kandidatenplatzierung. Auskommentiert -> Mittelwert. +# d_opt = 3.0 + +[fov] +# Sichtfeld der Sensorpyramide [Grad], volle Oeffnungswinkel (horizontal/vertikal). +fov_h_deg = 40.0 +fov_v_deg = 30.0 + +[sensor] +# Basislinie zwischen Projektor und Kamera des Streifenlichtsystems [m]. +# Ein Scan ist nur gueltig, wenn *beide* (Projektor UND Kamera) die Facette +# unverdeckt und im Sichtfeld sehen ("duale Sicht"). baseline = 0 -> klassische +# Einzelsicht (Ablation). Die Kamera sitzt um baseline seitlich (entlang der +# right-Achse des Sensorrahmens) versetzt. +baseline = 0.4 + +[incidence] +# Maximaler Einfallswinkel zwischen Sichtstrahl und Facetten-Normale [Grad]. +theta_max_deg = 60.0 + +[orientation] +# Erlaubter Anstellbereich (Pitch) des Sensors [Grad]. Schritt [3] klemmt den +# idealen (frontalen) Pitch auf [pitch_min, pitch_max] und verwirft Posen, bei +# denen die Facette nach der Klemmung aus dem vertikalen Sichtfeld faellt. +# pitch < 0 : Sensor schaut nach unten (Drohne ueber dem Objekt) +# pitch > 0 : Sensor schaut nach oben +pitch_min_deg = -10.0 +pitch_max_deg = 10.0 + +[drone] +# Mindest-Sicherheitsabstand der Drohne zum Messobjekt [m]. +safety_distance = 1.5 +# Bodenebene-Filter: niedrigstes z des Messobjekts gilt als Boden; Posen muessen +# mindestens ground_clearance darueber liegen. Mit false deaktivieren. +use_ground_plane = true +ground_clearance = 1.0 # Mindesthoehe der Drohne ueber der Bodenebene [m] + +[sampling] +# Abtastung der zulaessigen Posenregion je Facette (Schritt [2]). +# Kegel-Sampling um die Facetten-Normale: ein frontaler Strahl plus Azimut-Ringe +# bei Bruchteilen von theta_max (cone_fractions) mit je n_azimuth Richtungen, +# kombiniert mit Distanz-Stuetzstellen in [d_min, d_max]. +n_azimuth = 8 +cone_fractions = [0.0, 0.5, 0.85] +n_distance = 2 +# Dedup-Aufloesung der Projektion (Schritt [3]): Ortsraster [m], Yaw- und +# Pitch-Bin [Grad]. Auskommentiertes voxel -> 0.15 * d_opt. +# voxel = 0.45 +yaw_bin_deg = 15.0 +pitch_bin_deg = 10.0 +# Facetten-Aufloesung der Szene: Soll-Kantenlaenge der Dreiecke [m]. +resolution = 0.25 + +[registration] +# Overlap-Schwelle des Registrierungsgraphen (overlap.py / setcover.py): +# Flaechen-Anteil, den zwei Scans teilen muessen, damit sie als paarweise +# registrierbar gelten (Kante im Graphen) -- bezogen auf den *kleineren* der +# beiden Scans. Bei uniformer Diskretisierung ist die Facettenzahl proportional +# zur Flaeche, d. h. min_overlap = 0.25 heisst "25 % der Flaeche des kleineren +# Scans werden auch vom Nachbarn gesehen". Der Registrierungsgraph muss +# zusammenhaengend sein; ist er es nicht, ergaenzt ensure_connected Bruecken +# (bzw. connected_set_cover_ilp erzwingt den Zusammenhang direkt). +min_overlap = 0.25 +# Footprint-Erosion [m] (erosion.py): > 0 erodiert jeden Kandidaten-Footprint +# vor dem Set Cover um diesen Rand. Benachbarte Scans ueberlappen dann per +# Konstruktion um ein Band ~2*erosion (Side-Lap-Prinzip); auf zusammenhaengenden +# Flaechen haengt der Registrierungsgraph zusammen, ensure_connected wird zum +# reinen Sicherheitsnetz (0 Bruecken erwartet). 0 = aus. CLI-Override: --erode. +erosion = 0.0 + +[tracking] +# Bodengebundene, mobile Tracking-Roboter (kabelgebundene Sensoren). Jede +# gewaehlte Drohnenpose muss von >= 1 Standort mit *freier Sichtlinie* (LoS, +# Raycast gegen das Messobjekt) und im Arbeitsabstand erfasst werden. Minimale +# Standortmenge = Set Cover (tracking.py). +range_min = 1.0 # min. Abstand Tracker <-> Drohne [m] +range_max = 8.0 # max. Abstand Tracker <-> Drohne [m] +grid_step = 0.5 # Aufloesung des Boden-Kandidatenrasters [m] +margin = 2.0 # Rand, um den das Raster ueber die Posen-Huelle hinausgeht [m] diff --git a/student_code/260722_UAVViewPlanning/vpp3d/erosion.py b/student_code/260722_UAVViewPlanning/vpp3d/erosion.py new file mode 100644 index 0000000..6ed076b --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp3d/erosion.py @@ -0,0 +1,95 @@ +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np +import scipy.sparse as sp +from scipy.spatial import cKDTree + + +@dataclass +class ErosionResult: + V: sp.csr_matrix + erosion: float + radius: float + n_iter: int + nnz_before: int + nnz_after: int + reachable_before: int + reachable_after: int + n_empty: int + + def summary(self) -> str: + drop = 1.0 - self.nnz_after / max(1, self.nnz_before) + return ( + f"Footprint-Erosion (Rand {self.erosion:.2f} m ~= {self.n_iter} " + f"Ring(e) a {self.radius:.2f} m):\n" + f" Sichtbarkeiten : {self.nnz_before:,} -> " + f"{self.nnz_after:,} (-{drop:.0%})\n" + f" erreichbare Facetten : {self.reachable_before:,} -> " + f"{self.reachable_after:,} (Planung; real gilt das volle V)\n" + f" leere Footprints : {self.n_empty:,} von {self.V.shape[0]:,}" + ) + + +def _facet_adjacency(centers: np.ndarray, radius: float) -> sp.csr_matrix: + pairs = cKDTree(centers).query_pairs(radius, output_type="ndarray") + n = len(centers) + loops = np.arange(n, dtype=np.int64) + i = np.concatenate([pairs[:, 0], pairs[:, 1], loops]) + j = np.concatenate([pairs[:, 1], pairs[:, 0], loops]) + return sp.csr_matrix((np.ones(len(i), dtype=np.int32), (i, j)), shape=(n, n)) + + +def erode_footprints( + V: sp.csr_matrix, + centers: np.ndarray, + resolution: float, + erosion: float, +) -> ErosionResult: + radius = float(resolution) + n_iter = max(1, int(round(erosion / radius))) + adj = _facet_adjacency(np.asarray(centers, dtype=np.float64), radius) + deg = np.asarray(adj.sum(axis=0)).ravel() + + Vc = V.tocsr().astype(np.int32) + nnz_before = int(Vc.nnz) + reach_before = int((np.asarray(Vc.sum(axis=0)).ravel() > 0).sum()) + + Ve = Vc + for _ in range(n_iter): + D = (Ve @ adj).tocoo() + keep = D.data == deg[D.col] + Ve = sp.csr_matrix( + (np.ones(int(keep.sum()), dtype=np.int32), + (D.row[keep], D.col[keep])), + shape=V.shape, + ) + + return ErosionResult( + V=Ve.astype(bool).tocsr(), + erosion=float(erosion), + radius=radius, + n_iter=n_iter, + nnz_before=nnz_before, + nnz_after=int(Ve.nnz), + reachable_before=reach_before, + reachable_after=int((np.asarray(Ve.sum(axis=0)).ravel() > 0).sum()), + n_empty=int((np.diff(Ve.indptr) == 0).sum()), + ) + + +def cover_residual( + V: sp.csr_matrix, selected: np.ndarray +) -> tuple[np.ndarray, int]: + from .setcover import greedy_set_cover + + Vc = V.tocsr() + reach = np.asarray(Vc.sum(axis=0)).ravel() > 0 + cov = (np.asarray(Vc[selected].sum(axis=0)).ravel() > 0 + if len(selected) else np.zeros(Vc.shape[1], dtype=bool)) + residual = np.flatnonzero(reach & ~cov) + if not len(residual): + return np.empty(0, dtype=np.int64), 0 + extra = greedy_set_cover(Vc[:, residual]).poses + return np.setdiff1d(extra, selected), int(len(residual)) diff --git a/student_code/260722_UAVViewPlanning/vpp3d/inspector.py b/student_code/260722_UAVViewPlanning/vpp3d/inspector.py new file mode 100644 index 0000000..217360a --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp3d/inspector.py @@ -0,0 +1,594 @@ +from __future__ import annotations + +import argparse +import colorsys +import math +import sys +from pathlib import Path + +import numpy as np + +try: + sys.stdout.reconfigure(encoding="utf-8") +except (AttributeError, ValueError): + pass + +from .config import load_config, with_overrides +from .scene import scene_from_args +from .candidates import sample_pose_regions, project_and_dedup +from .visibility import compute_visibility +from .setcover import greedy_set_cover, ilp_set_cover, connected_set_cover_ilp +from .erosion import erode_footprints, cover_residual +from .overlap import ensure_connected +from .refine import refine_selected_poses +from .tracking import plan_tracking +from .sequencing import sequence_route +from .runlog import RunLog +from .run import add_common_args + +_OUT = Path(__file__).with_name("output") + +_KEY_N, _KEY_B = 78, 66 +_KEY_RIGHT, _KEY_LEFT = 262, 263 +_KEY_C, _KEY_P, _KEY_O, _KEY_M = 67, 80, 79, 77 +_KEY_1, _KEY_2, _KEY_3, _KEY_4 = 49, 50, 51, 52 +_KEY_S, _KEY_Q, _KEY_ESC = 83, 81, 256 + +_C_VIS = np.array([0.18, 0.76, 0.36]) +_C_CORE = np.array([0.00, 0.42, 0.18]) +_C_PLAN_COV = np.array([0.60, 0.88, 0.65]) +_C_OPEN = np.array([0.85, 0.22, 0.15]) +_C_UNREACH = np.array([0.42, 0.42, 0.42]) +_C_BG = np.array([0.78, 0.78, 0.78]) +_C_CUR = np.array([1.00, 0.50, 0.00]) +_C_OTHER = np.array([0.30, 0.30, 0.30]) +_C_CAND = np.array([0.28, 0.60, 0.90]) +_C_PLAN_PT = np.array([0.10, 0.10, 0.10]) +_C_FRUSTUM = np.array([0.00, 0.40, 1.00]) +_C_ARROW = np.array([0.10, 0.10, 0.10]) +_C_ROUTE = np.array([1.00, 0.55, 0.00]) +_C_OVERLAP = np.array([1.00, 0.90, 0.00]) +_C_TRACKER = np.array([0.00, 0.80, 0.80]) + +_FRUSTUM_LINES = np.array([ + [0, 1], [0, 2], [0, 3], [0, 4], + [1, 2], [2, 3], [3, 4], [4, 1], + [5, 6], [6, 7], [7, 8], [8, 5], + [1, 5], [2, 6], [3, 7], [4, 8], +], dtype=np.int32) + + +def _distinct_colors(n: int) -> np.ndarray: + golden = 0.6180339887498949 + cols = np.empty((max(1, n), 3)) + for i in range(max(1, n)): + h = (i * golden) % 1.0 + s = (0.90, 0.60, 0.75)[i % 3] + v = (0.95, 0.75, 0.85, 0.65)[i % 4] + cols[i] = colorsys.hsv_to_rgb(h, s, v) + return cols + + +def _lineset(pts, lines, color=None): + import open3d as o3d + ls = o3d.geometry.LineSet( + o3d.utility.Vector3dVector(np.asarray(pts, dtype=float)), + o3d.utility.Vector2iVector(np.asarray(lines, dtype=np.int32)), + ) + if color is not None: + ls.paint_uniform_color(list(color)) + return ls + + +def _set_points(ls, pts) -> None: + import open3d as o3d + ls.points = o3d.utility.Vector3dVector(np.asarray(pts, dtype=float)) + + +def _pose_frame(yaw: float, pitch: float): + cp, sp = math.cos(pitch), math.sin(pitch) + cy, sy = math.cos(yaw), math.sin(yaw) + fwd = np.array([cp * cy, cp * sy, sp]) + right = np.array([-sy, cy, 0.0]) + return fwd, right, np.cross(right, fwd) + + +def _rect(ctr: np.ndarray, a: np.ndarray, b: np.ndarray) -> list[np.ndarray]: + return [ctr + a + b, ctr - a + b, ctr - a - b, ctr + a - b] + + +def _make_facet_mesh(scene): + import open3d as o3d + V, T = scene.facet_verts, scene.facet_tris + mesh = o3d.geometry.TriangleMesh( + o3d.utility.Vector3dVector(V[T].reshape(-1, 3)), + o3d.utility.Vector3iVector(np.arange(len(T) * 3, dtype=np.int32).reshape(-1, 3)), + ) + mesh.compute_vertex_normals() + return mesh + + +def _set_facet_colors(mesh, colors: np.ndarray) -> None: + import open3d as o3d + mesh.vertex_colors = o3d.utility.Vector3dVector(np.repeat(colors, 3, axis=0)) + + +def _frustum_points(pos: np.ndarray, yaw: float, pitch: float, cfg) -> np.ndarray: + fwd, right, up = _pose_frame(yaw, pitch) + th = math.tan(cfg.fov_h_rad / 2) + tv = math.tan(cfg.fov_v_rad / 2) + return np.vstack([pos.reshape(1, 3)] + + [_rect(pos + d * fwd, d * th * right, d * tv * up) + for d in (cfg.d_min, cfg.d_max)]) + + +def _make_ground_grid(z_ground: float, lo: np.ndarray, hi: np.ndarray, + step: float = 1.0): + margin = max(step, 0.5) + x0, x1 = lo[0] - margin, hi[0] + margin + y0, y1 = lo[1] - margin, hi[1] + margin + pts: list[list[float]] = [] + for y in np.arange(y0, y1 + 1e-9, step): + pts += [[x0, y, z_ground], [x1, y, z_ground]] + for x in np.arange(x0, x1 + 1e-9, step): + pts += [[x, y0, z_ground], [x, y1, z_ground]] + lines = [[i, i + 1] for i in range(0, len(pts), 2)] + return _lineset(pts, lines, (0.60, 0.60, 0.60)) + + +def _make_camera_pyramids(poses, selected_rows: np.ndarray, + palette: np.ndarray, cfg): + import open3d as o3d + depth = 0.18 * cfg.d_opt + th = np.tan(cfg.fov_h_rad / 2) * depth + tv = np.tan(cfg.fov_v_rad / 2) * depth + + pts: list[np.ndarray] = [] + idx: list[list[int]] = [] + col: list[list[float]] = [] + for k, row in enumerate(selected_rows): + pos = poses.positions[row] + fwd, right, up = _pose_frame(float(poses.yaws[row]), + float(poses.pitches[row])) + base = len(pts) + pts.append(pos) + pts.extend(_rect(pos + depth * fwd, th * right, tv * up)) + c = palette[k].tolist() + for i in range(4): + idx.append([base, base + 1 + i]); col.append(c) + for i in range(4): + idx.append([base + 1 + i, base + 1 + (i + 1) % 4]); col.append(c) + + ls = _lineset(pts, idx) + ls.colors = o3d.utility.Vector3dVector(np.array(col, dtype=float)) + return ls + + +def _make_tracking_geom(tracking, plan_pos: np.ndarray): + import open3d as o3d + + if tracking is None or len(tracking.stations) == 0: + return None, None + + pcd = o3d.geometry.PointCloud( + o3d.utility.Vector3dVector(tracking.stations)) + pcd.paint_uniform_color(_C_TRACKER.tolist()) + + pts: list[np.ndarray] = [] + idx: list[list[int]] = [] + for i, st_idx in enumerate(tracking.assignment): + if st_idx < 0 or i >= len(plan_pos): + continue + pts += [tracking.stations[st_idx], plan_pos[i]] + idx.append([len(pts) - 2, len(pts) - 1]) + + if not idx: + return pcd, None + return pcd, _lineset(pts, idx, _C_TRACKER) + + +def _overlay_core(colors: np.ndarray, V_eroded, row: int) -> None: + if V_eroded is None: + return + core = V_eroded.getrow(row).indices + if len(core): + colors[core] = _C_CORE + + +def _colors_candidates(vis, coverable: np.ndarray, cur_row: int, + V_eroded=None) -> np.ndarray: + colors = np.tile(_C_BG, (len(coverable), 1)) + colors[~coverable] = _C_UNREACH + visible = vis.V.getrow(cur_row).indices + if len(visible): + colors[visible] = _C_VIS + _overlay_core(colors, V_eroded, cur_row) + return colors + + +def _colors_plan(vis, coverable: np.ndarray, selected_rows: np.ndarray, + cur_step: int, V_eroded=None) -> np.ndarray: + plan_covered = np.asarray(vis.V[selected_rows].sum(axis=0)).ravel() > 0 + colors = np.tile(_C_UNREACH, (len(coverable), 1)) + colors[coverable & ~plan_covered] = _C_OPEN + colors[coverable & plan_covered] = _C_PLAN_COV + visible = vis.V.getrow(selected_rows[cur_step]).indices + if len(visible): + colors[visible] = _C_VIS + _overlay_core(colors, V_eroded, selected_rows[cur_step]) + return colors + + +def _colors_overview(vis, coverable: np.ndarray, + selected_rows: np.ndarray) -> np.ndarray: + plan_covered = np.asarray(vis.V[selected_rows].sum(axis=0)).ravel() > 0 + colors = np.tile(_C_UNREACH, (len(coverable), 1)) + colors[coverable & ~plan_covered] = _C_OPEN + colors[coverable & plan_covered] = np.array([0.19, 0.64, 0.33]) + return colors + + +def _colors_mosaic(vis, coverable: np.ndarray, selected_rows: np.ndarray, + palette: np.ndarray, + overlap_facets: np.ndarray | None = None) -> np.ndarray: + V_sel = vis.V[selected_rows].tocsr() + coverage_count = np.asarray(V_sel.sum(axis=0)).ravel().astype(int) + + colors = np.tile(_C_UNREACH, (len(coverable), 1)) + colors[coverable & (coverage_count == 0)] = _C_OPEN + + coo = V_sel.tocoo() + if coo.nnz: + order = np.lexsort((coo.row, coo.col)) + col_s, row_s = coo.col[order], coo.row[order] + first = np.ones(len(col_s), dtype=bool) + first[1:] = col_s[1:] != col_s[:-1] + colors[col_s[first]] = palette[row_s[first]] + + if overlap_facets is not None and len(overlap_facets): + colors[overlap_facets] = _C_OVERLAP + return colors + + +def run_inspector(scene, poses, vis, selected_rows: np.ndarray, + route, cfg, overlap=None, tracking=None, + V_eroded=None) -> None: + import open3d as o3d + + coverable = vis.coverage_per_facet() > 0 + fc = scene.facets + n_cand = len(poses) + n_plan = len(selected_rows) + plan_pos = poses.positions[selected_rows] + palette = _distinct_colors(n_plan) + + extent = float(np.ptp(fc.centers, axis=0).max()) + r_sphere = max(0.04, 0.016 * extent) + arrow_len = 0.70 * cfg.d_opt + + overlap_ids = overlap.overlap_facets if overlap is not None else None + + lo, hi = scene.bounds + ls_ground = _make_ground_grid(float(lo[2]), lo, hi, + step=max(0.5, round(extent / 10, 1))) + + facet_mesh = _make_facet_mesh(scene) + + pcd_cand = o3d.geometry.PointCloud( + o3d.utility.Vector3dVector(poses.positions)) + pcd_cand.paint_uniform_color(_C_CAND.tolist()) + + pcd_step = o3d.geometry.PointCloud( + o3d.utility.Vector3dVector(poses.positions)) + pcd_step.paint_uniform_color(_C_OTHER.tolist()) + + if len(route.order) > 1: + ls_route = _lineset(plan_pos[route.order], + [[i, i + 1] for i in range(len(route.order) - 1)], + _C_ROUTE) + else: + ls_route = None + + sphere = o3d.geometry.TriangleMesh.create_sphere(radius=r_sphere) + sphere.compute_vertex_normals() + sphere.paint_uniform_color(_C_CUR.tolist()) + + ls_arrow = _lineset(np.zeros((2, 3)), [[0, 1]], _C_ARROW) + ls_frustum = _lineset(np.zeros((9, 3)), _FRUSTUM_LINES, _C_FRUSTUM) + ls_cameras = _make_camera_pyramids(poses, selected_rows, palette, cfg) + + pcd_tracker, ls_tracker_lines = _make_tracking_geom(tracking, plan_pos) + + state = { + "mode": "overview", + "cur": 0, + "pose_shown": False, + "sphere_pos": np.zeros(3), + "cand_shown": True, + "cam_shown": False, + "trk_shown": False, + } + + def _set_shown(v, key: str, geoms, show: bool) -> None: + if state[key] == show: + return + fn = v.add_geometry if show else v.remove_geometry + for gm in geoms: + if gm is not None: + fn(gm, reset_bounding_box=False) + state[key] = show + + def _set_layers(v, cand: bool, cameras: bool, trk: bool) -> None: + _set_shown(v, "cand_shown", [pcd_cand], cand) + _set_shown(v, "cam_shown", [ls_cameras], cameras) + _set_shown(v, "trk_shown", [pcd_tracker, ls_tracker_lines], trk) + + def _show_pose(v, pose_row: int) -> None: + pos = poses.positions[pose_row] + sphere.translate(pos - state["sphere_pos"]) + state["sphere_pos"] = pos.copy() + _set_points(ls_arrow, np.vstack([pos, pos + arrow_len * poses.view_dirs[pose_row]])) + _set_points(ls_frustum, _frustum_points(pos, float(poses.yaws[pose_row]), + float(poses.pitches[pose_row]), cfg)) + for gm in (sphere, ls_arrow, ls_frustum): + if state["pose_shown"]: + v.update_geometry(gm) + else: + v.add_geometry(gm, reset_bounding_box=False) + state["pose_shown"] = True + + def _hide_pose(v) -> None: + if state["pose_shown"]: + for gm in (sphere, ls_arrow, ls_frustum): + v.remove_geometry(gm, reset_bounding_box=False) + state["pose_shown"] = False + + + def _refresh_candidates(v) -> None: + _set_layers(v, cand=True, cameras=False, trk=False) + cur = state["cur"] + _set_facet_colors(facet_mesh, + _colors_candidates(vis, coverable, cur, V_eroded)) + pcd_step.points = o3d.utility.Vector3dVector(poses.positions) + colors_p = np.tile(_C_OTHER, (n_cand, 1)) + colors_p[cur] = _C_CUR + pcd_step.colors = o3d.utility.Vector3dVector(colors_p) + v.update_geometry(facet_mesh) + v.update_geometry(pcd_step) + _show_pose(v, cur) + n_vis = vis.V.getrow(cur).nnz + print(f" Kandidat {cur + 1:>5}/{n_cand} " + f"({poses.positions[cur, 0]:.2f}, " + f"{poses.positions[cur, 1]:.2f}, " + f"{poses.positions[cur, 2]:.2f}) " + f"Yaw {np.degrees(poses.yaws[cur]):.0f}° " + f"Pitch {np.degrees(poses.pitches[cur]):.0f}° " + f"→ {n_vis} Facetten sichtbar") + + def _refresh_plan(v) -> None: + _set_layers(v, cand=True, cameras=False, trk=False) + cur = state["cur"] + _set_facet_colors(facet_mesh, + _colors_plan(vis, coverable, selected_rows, cur, + V_eroded)) + colors_p = np.tile(_C_OTHER, (n_plan, 1)) + colors_p[cur] = _C_CUR + pcd_step.points = o3d.utility.Vector3dVector(plan_pos) + pcd_step.colors = o3d.utility.Vector3dVector(colors_p) + v.update_geometry(facet_mesh) + v.update_geometry(pcd_step) + _show_pose(v, selected_rows[cur]) + n_vis = vis.V.getrow(selected_rows[cur]).nnz + print(f" Plan-Pose {cur + 1:>4}/{n_plan} " + f"(Pose-ID {selected_rows[cur]}) → {n_vis} Facetten sichtbar") + + def _refresh_overview(v) -> None: + _set_layers(v, cand=True, cameras=False, trk=False) + _set_facet_colors(facet_mesh, + _colors_overview(vis, coverable, selected_rows)) + pcd_step.points = o3d.utility.Vector3dVector(plan_pos) + pcd_step.colors = o3d.utility.Vector3dVector( + np.tile(_C_PLAN_PT, (n_plan, 1))) + v.update_geometry(facet_mesh) + v.update_geometry(pcd_step) + _hide_pose(v) + n_cov = int( + np.asarray(vis.V[selected_rows].sum(axis=0)).ravel().astype(bool).sum()) + print(f" Übersicht: {n_plan} Plan-Posen, " + f"{n_cov}/{int(coverable.sum())} Facetten abgedeckt " + f"({n_cov / max(1, int(coverable.sum())):.1%}), " + f"Route {route.length:.1f} m") + + def _refresh_mosaic(v) -> None: + _set_layers(v, cand=False, cameras=True, trk=True) + _set_facet_colors(facet_mesh, + _colors_mosaic(vis, coverable, selected_rows, palette, + overlap_ids)) + pcd_step.points = o3d.utility.Vector3dVector(plan_pos) + pcd_step.colors = o3d.utility.Vector3dVector(palette[:n_plan]) + v.update_geometry(facet_mesh) + v.update_geometry(pcd_step) + _hide_pose(v) + n_yellow = int(len(overlap_ids)) if overlap_ids is not None else 0 + n_trk = len(tracking.stations) if tracking is not None else 0 + print(f" Mosaik: {n_plan} Posen, {n_yellow} Ueberlappungs-Facetten gelb, " + f"{n_trk} Tracking-Standorte") + + + def _step(delta: int): + def cb(v, action, mods) -> bool: + if action not in (1, 2): + return False + m = state["mode"] + if m == "candidates": + state["cur"] = (state["cur"] + delta) % n_cand + _refresh_candidates(v) + elif m == "plan": + state["cur"] = (state["cur"] + delta) % n_plan + _refresh_plan(v) + return False + return cb + + def _mode_cb(mode: str, label: str, refresh_fn): + def cb(v, action, mods) -> bool: + if action != 1: + return False + state["mode"] = mode + state["cur"] = 0 + print(f"\n {label}") + refresh_fn(v) + return False + return cb + + def _screenshot(v, action, mods) -> bool: + if action != 1: + return False + import time as _time + _OUT.mkdir(exist_ok=True) + fname = (_OUT / f"inspector_{scene.name}_{state['mode']}" + f"_{state['cur']:04d}_{int(_time.time())}.png") + v.capture_screen_image(str(fname), do_render=True) + print(f" Screenshot: {fname}") + return False + + def _quit(v, action, mods) -> bool: + if action == 1: + v.close() + return False + + viz = o3d.visualization.VisualizerWithKeyCallback() + viz.create_window(window_name=f"vpp3d Inspector — {scene.name}", + width=1280, height=900) + + viz.add_geometry(facet_mesh) + viz.add_geometry(pcd_step) + viz.add_geometry(pcd_cand) + viz.add_geometry(ls_ground) + if ls_route is not None: + viz.add_geometry(ls_route) + + ro = viz.get_render_option() + ro.point_size = 5.0 + ro.background_color = np.array([0.95, 0.95, 0.95]) + ro.mesh_show_back_face = True + + bindings = [ + ((_KEY_N, _KEY_RIGHT), _step(+1)), + ((_KEY_B, _KEY_LEFT), _step(-1)), + ((_KEY_C, _KEY_1), _mode_cb("candidates", "[C] Kandidaten-Modus", _refresh_candidates)), + ((_KEY_P, _KEY_2), _mode_cb("plan", "[P] Plan-Modus", _refresh_plan)), + ((_KEY_O, _KEY_3), _mode_cb("overview", "[O] Übersicht", _refresh_overview)), + ((_KEY_M, _KEY_4), _mode_cb("mosaic", "[M] Mosaik", _refresh_mosaic)), + ((_KEY_S,), _screenshot), + ((_KEY_Q, _KEY_ESC), _quit), + ] + for keys, cb in bindings: + for key in keys: + viz.register_key_action_callback(key, cb) + + _refresh_overview(viz) + viz.reset_view_point(True) + + print() + print(" Tasten: C/1=Kandidaten P/2=Plan O/3=Übersicht M/4=Mosaik") + print(" N/→ vor B/← zurück S=Screenshot Q=schließen") + print() + + viz.run() + viz.destroy_window() + + +def build_and_run(args) -> None: + cfg = with_overrides(load_config(args.config), args) + print(cfg.summary()) + print("=" * 64) + + scene = scene_from_args(args, cfg) + print(scene.summary()) + + raw = sample_pose_regions(scene, cfg) + poses, d = project_and_dedup(raw, scene, cfg) + ground_part = (f"→ Boden {d['n_after_ground']:,} " + if cfg.use_ground_plane else "") + print(f"[2/3] Posen: {d['n_raw']:,} roh → Clearance {d['n_after_clearance']:,} " + f"{ground_part}→ Dedup {d['n_after_dedup']:,}") + + vis = compute_visibility(scene, poses, cfg) + print(vis.summary()) + + erosion = cfg.erosion if args.erode is None else args.erode + V_solve, ero = vis.V, None + if erosion > 0: + ero = erode_footprints(vis.V, scene.facets.centers, cfg.resolution, + erosion) + V_solve = ero.V + print("[5a] " + ero.summary()) + + if args.method == "ilp": + cover = ilp_set_cover(V_solve, k=args.k, time_limit=args.time_limit) + elif args.method == "connected": + cover = connected_set_cover_ilp(V_solve, cfg.min_overlap, k=args.k, + time_limit=args.time_limit) + else: + cover = greedy_set_cover(V_solve, k=args.k) + print(cover.summary()) + + if erosion > 0 and len(cover.poses): + extra, n_residual = cover_residual(vis.V, cover.poses) + if n_residual: + cover.poses = np.concatenate([cover.poses, extra]) + print(f" Rest-Abdeckung (volles V): +{len(extra)} Posen fuer " + f"{n_residual} Rand-Facetten") + + selected_rows = cover.poses + ov = None + if not args.no_overlap: + ov = ensure_connected(vis, cover.poses, cfg.min_overlap) + selected_rows = ov.selected + print(ov.summary()) + + if args.refine: + _, _, info = refine_selected_poses( + vis, poses, scene, selected_rows, cfg, + free=args.refine_free, gain_weight=args.gain_weight, + max_offset=args.max_offset, sigma_scale=args.sigma_scale, + coverage_weight=args.coverage_weight, z_min=args.z_min, + ) + print("-" * 64) + print("[5b] " + info.summary()) + vis = compute_visibility(scene, poses, cfg) + if erosion > 0: + ero = erode_footprints(vis.V, scene.facets.centers, + cfg.resolution, erosion) + n = vis.V.shape[1] + cov = int((np.asarray(vis.V[selected_rows].sum(axis=0)).ravel() > 0).sum()) + print(f" Abdeckung nach Verfeinerung (neu geraycastet): " + f"{cov}/{n} ({cov / max(1, n):.1%})") + + trk = None + if not args.no_tracking: + trk = plan_tracking(scene, poses.positions[selected_rows], cfg) + print(trk.summary()) + + route = sequence_route(poses.positions[selected_rows], trk) + print(route.summary()) + + log = RunLog(scene.name) + log.save_matrix_png(vis, selected_rows) + + run_inspector(scene, poses, vis, selected_rows, route, cfg, + overlap=ov, tracking=trk, + V_eroded=ero.V if ero is not None else None) + + +def main() -> None: + ap = argparse.ArgumentParser( + description="vpp3d — interaktiver 3D-Inspektor") + add_common_args(ap) + ap.add_argument("--method", default="greedy", + choices=["greedy", "ilp", "connected"]) + build_and_run(ap.parse_args()) + + +if __name__ == "__main__": + main() diff --git a/student_code/260722_UAVViewPlanning/vpp3d/overlap.py b/student_code/260722_UAVViewPlanning/vpp3d/overlap.py new file mode 100644 index 0000000..49568c8 --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp3d/overlap.py @@ -0,0 +1,129 @@ +from __future__ import annotations + +from dataclasses import dataclass, field + +import numpy as np +import scipy.sparse as sp +from scipy.sparse.csgraph import connected_components + +from .visibility import VisibilityMatrix + + +@dataclass +class OverlapResult: + selected: np.ndarray + added: np.ndarray + n_components: int + overlap_facets: np.ndarray + min_overlap: float + extra: dict = field(default_factory=dict) + + def summary(self) -> str: + ok = "zusammenhaengend" if self.n_components == 1 else ( + f"NICHT zusammenhaengend ({self.n_components} Komponenten)" + ) + return ( + f"Registrierungsgraph (min_overlap={self.min_overlap:.0%} Flaeche, " + f"kleinerer Scan):\n" + f" Posen : {len(self.selected):,} " + f"(+{len(self.added):,} Bruecken ergaenzt)\n" + f" Ueberlappungs-Facetten : {len(self.overlap_facets):,} " + f"(von >= 2 Scans gesehen)\n" + f" Graph : {ok}" + ) + + +def overlap_fractions(V: sp.csr_matrix, rows: np.ndarray) -> np.ndarray: + Vs = V[rows].astype(np.int64) + inter = np.asarray((Vs @ Vs.T).todense()) + sizes = np.asarray(Vs.sum(axis=1)).ravel() + return inter / np.maximum(np.minimum.outer(sizes, sizes), 1) + + +def registration_graph( + V: sp.csr_matrix, rows: np.ndarray, min_overlap: float +) -> np.ndarray: + adj = overlap_fractions(V, rows) >= min_overlap + np.fill_diagonal(adj, False) + return adj + + +def overlap_adjacency( + V: sp.csr_matrix, min_overlap: float +) -> tuple[np.ndarray, sp.csr_matrix]: + Vc = V.tocsr().astype(np.int64) + sizes_all = np.asarray(Vc.sum(axis=1)).ravel() + nodes = np.flatnonzero(sizes_all > 0).astype(np.int64) + n = len(nodes) + if n == 0: + return nodes, sp.csr_matrix((0, 0), dtype=np.float32) + + Vn = Vc[nodes] + inter = (Vn @ Vn.T).tocoo() + sizes = sizes_all[nodes] + + upper = inter.row < inter.col + a, b, val = inter.row[upper], inter.col[upper], inter.data[upper] + frac = val / np.maximum(np.minimum(sizes[a], sizes[b]), 1) + edge = frac >= min_overlap + a, b, frac = a[edge], b[edge], frac[edge].astype(np.float32) + + adj = sp.csr_matrix((frac, (a, b)), shape=(n, n), dtype=np.float32) + return nodes, adj + adj.T + + +def _overlap_facets(V: sp.csr_matrix, rows: np.ndarray) -> np.ndarray: + if len(rows) == 0: + return np.empty(0, dtype=np.int64) + cov = np.asarray(V[rows].sum(axis=0)).ravel() + return np.flatnonzero(cov >= 2).astype(np.int64) + + +def ensure_connected( + vis: VisibilityMatrix, + selected: np.ndarray, + min_overlap: float, +) -> OverlapResult: + V = vis.V.tocsr() + sel = list(np.asarray(selected, dtype=np.int64)) + added: list[int] = [] + + while True: + rows = np.asarray(sel, dtype=np.int64) + adj = registration_graph(V, rows, min_overlap) + n_comp, labels = connected_components(sp.csr_matrix(adj), directed=False) + if n_comp <= 1: + break + + pool = np.setdiff1d(np.arange(V.shape[0]), rows, assume_unique=False) + if len(pool) == 0: + break + + Vp = V[pool].astype(np.int64) + Vs = V[rows].astype(np.int64) + inter = np.asarray((Vp @ Vs.T).todense()) + size_p = np.asarray(Vp.sum(axis=1)).ravel() + size_s = np.asarray(Vs.sum(axis=1)).ravel() + frac = inter / np.maximum(np.minimum.outer(size_p, size_s), 1) + link = frac >= min_overlap + + comp_onehot = np.eye(n_comp, dtype=bool)[labels] + comps_touched = (link @ comp_onehot.astype(np.int64)) > 0 + n_touched = comps_touched.sum(axis=1) + + best = int(np.lexsort((-frac.sum(axis=1), -n_touched))[0]) + if n_touched[best] < 2: + break + sel.append(int(pool[best])) + added.append(int(pool[best])) + + rows = np.asarray(sel, dtype=np.int64) + adj = registration_graph(V, rows, min_overlap) + n_comp, _ = connected_components(sp.csr_matrix(adj), directed=False) + return OverlapResult( + selected=rows, + added=np.asarray(added, dtype=np.int64), + n_components=int(n_comp), + overlap_facets=_overlap_facets(V, rows), + min_overlap=float(min_overlap), + ) diff --git a/student_code/260722_UAVViewPlanning/vpp3d/refine.py b/student_code/260722_UAVViewPlanning/vpp3d/refine.py new file mode 100644 index 0000000..046c62b --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp3d/refine.py @@ -0,0 +1,293 @@ +from __future__ import annotations + +from dataclasses import dataclass, field + +import numpy as np +from scipy.optimize import minimize +from scipy.spatial import cKDTree + +from .candidates import Poses +from .config import Config +from .scene import Facets + +_EPS = 1e-9 +_PENALTY = 1.0e6 + + +def _aim(position: np.ndarray, target: np.ndarray, cfg: Config): + look = target - position + horiz = float(np.hypot(look[0], look[1])) + yaw = float(np.arctan2(look[1], look[0])) if horiz > _EPS else 0.0 + pitch = float(np.clip(np.arctan2(look[2], max(horiz, _EPS)), + cfg.pitch_min_rad, cfg.pitch_max_rad)) + cp, sp = np.cos(pitch), np.sin(pitch) + return yaw, pitch, np.array([cp * np.cos(yaw), cp * np.sin(yaw), sp]) + + +def pose_quality( + position: np.ndarray, + facet_pos: np.ndarray, + facet_norm: np.ndarray, + cfg: Config, + *, + weights: np.ndarray | None = None, + sigma_scale: float = 1.5, + return_detail: bool = False, +): + position = np.asarray(position, dtype=np.float64).reshape(3) + n = len(facet_pos) + wt = np.ones(n) if weights is None else np.asarray(weights, dtype=np.float64) + w = facet_pos - position + d = np.linalg.norm(w, axis=1) + u = w / np.maximum(d, _EPS)[:, None] + + cos_inc = -np.einsum("ij,ij->i", facet_norm, u) + cos_tmax = np.cos(cfg.theta_max_rad) + gate_di = (d >= cfg.d_min) & (d <= cfg.d_max) & (cos_inc >= cos_tmax) + + sig_near = max((cfg.d_opt - cfg.d_min) / sigma_scale, _EPS) + sig_far = max((cfg.d_max - cfg.d_opt) / sigma_scale, _EPS) + sigma = np.where(d < cfg.d_opt, sig_near, sig_far) + q_dist = np.exp(-0.5 * ((d - cfg.d_opt) / sigma) ** 2) + + c = np.clip((cos_inc - cos_tmax) / max(1.0 - cos_tmax, _EPS), 0.0, 1.0) + q_inc = c * c * (3.0 - 2.0 * c) + + w_di = np.where(gate_di, q_dist * q_inc * wt, 0.0) + detail = {"yaw": 0.0, "pitch": 0.0, "forward": np.array([1.0, 0.0, 0.0]), + "q": np.zeros(n)} + if not w_di.any(): + return (0.0, detail) if return_detail else 0.0 + + target = (w_di[:, None] * facet_pos).sum(axis=0) / w_di.sum() + yaw, pitch, fwd = _aim(position, target, cfg) + + right = np.cross(fwd, np.array([0.0, 0.0, 1.0])) + nr = np.linalg.norm(right) + right = right / nr if nr > _EPS else np.array([0.0, 1.0, 0.0]) + up = np.cross(right, fwd) + x = (u @ fwd) * d + y = (u @ right) * d + z = (u @ up) * d + x_safe = np.where(x > _EPS, x, _EPS) + ry = y / (x_safe * np.tan(0.5 * cfg.fov_h_rad)) + rz = z / (x_safe * np.tan(0.5 * cfg.fov_v_rad)) + gate_f = (x > _EPS) & (np.abs(ry) <= 1.0) & (np.abs(rz) <= 1.0) + q_frust = np.clip(1.0 - ry ** 2, 0.0, 1.0) * np.clip(1.0 - rz ** 2, 0.0, 1.0) + + q = np.where(gate_di & gate_f, w_di * q_frust, 0.0) + J = float(q.sum()) + if return_detail: + return J, {"yaw": yaw, "pitch": pitch, "forward": fwd, "q": q} + return J + + +@dataclass +class RefineInfo: + j_before: np.ndarray + j_after: np.ndarray + displacement: np.ndarray + soft_cov_before: np.ndarray + soft_cov_after: np.ndarray + mean_inc_before: np.ndarray + mean_inc_after: np.ndarray + d_err_before: np.ndarray + d_err_after: np.ndarray + n_iter: np.ndarray = field(default_factory=lambda: np.empty(0)) + + def summary(self) -> str: + def pct(a, b): + a, b = float(np.nanmean(a)), float(np.nanmean(b)) + return f"{a:.3g} -> {b:.3g} ({(b - a) / max(abs(a), 1e-9):+.1%})" + ang_b = float(np.degrees(np.nanmean(self.mean_inc_before))) + ang_a = float(np.degrees(np.nanmean(self.mean_inc_after))) + return ( + f"Lokale Verfeinerung: {len(self.j_before):,} Posen\n" + f" Score J : {pct(self.j_before, self.j_after)}\n" + f" Weiche Abdeckung: {pct(self.soft_cov_before, self.soft_cov_after)} Facetten/Pose\n" + f" Einfallswinkel : {ang_b:.1f}° -> {ang_a:.1f}° (kleiner = frontaler)\n" + f" |d - d_opt| : {pct(self.d_err_before, self.d_err_after)} m\n" + f" Verschiebung : Ø {np.mean(self.displacement):.3g} m, " + f"max {np.max(self.displacement, initial=0.0):.3g} m" + ) + + +def _pose_metrics(position, facet_pos, facet_norm, cfg): + _, det = pose_quality(position, facet_pos, facet_norm, cfg, return_detail=True) + vis = det["q"] > 0.0 + n = int(vis.sum()) + if n == 0: + return 0, float("nan"), float("nan") + w = facet_pos[vis] - position + d = np.linalg.norm(w, axis=1) + u = w / np.maximum(d, _EPS)[:, None] + cos_inc = np.clip(-np.einsum("ij,ij->i", facet_norm[vis], u), -1.0, 1.0) + return n, float(np.mean(np.arccos(cos_inc))), float(np.mean(np.abs(d - cfg.d_opt))) + + +def refine_poses( + poses: Poses, + facets: Facets, + cfg: Config, + *, + target_facets: list[np.ndarray] | None = None, + max_offset: float | None = None, + init_step: float | None = None, + sigma_scale: float = 1.5, + coverage_weight: float = 4.0, + gain_weight: float = 0.0, + z_min: float | None = None, + max_iter: int = 120, +) -> tuple[Poses, RefineInfo]: + max_offset = 0.5 * cfg.d_opt if max_offset is None else float(max_offset) + init_step = 0.15 * cfg.d_opt if init_step is None else float(init_step) + P, Nn = facets.centers, facets.normals + tree = cKDTree(P) + search_r = cfg.d_max + max_offset + + S = len(poses) + new_pos = poses.positions.copy() + new_yaw = poses.yaws.copy() + new_pitch = poses.pitches.copy() + + j_before = np.zeros(S) + j_after = np.zeros(S) + disp = np.zeros(S) + sc_b = np.zeros(S, dtype=np.int64) + sc_a = np.zeros(S, dtype=np.int64) + inc_b = np.full(S, np.nan) + inc_a = np.full(S, np.nan) + de_b = np.full(S, np.nan) + de_a = np.full(S, np.nan) + n_iter = np.zeros(S, dtype=np.int64) + + for j in range(S): + base = poses.positions[j] + if target_facets is not None: + assigned = np.asarray(target_facets[j], dtype=np.int64) + n_assigned = len(assigned) + if gain_weight > 0.0 and n_assigned: + nearby = np.asarray(tree.query_ball_point(base, r=search_r), + dtype=np.int64) + extra = np.setdiff1d(nearby, assigned, assume_unique=False) + idx = np.concatenate([assigned, extra]) + wt = np.concatenate([np.ones(n_assigned), + np.full(len(extra), float(gain_weight))]) + else: + idx, wt = assigned, None + else: + idx = np.asarray(tree.query_ball_point(base, r=search_r), dtype=np.int64) + wt = None + n_assigned = 0 + if len(idx) == 0: + j_before[j] = j_after[j] = 0.0 + new_pos[j] = base + continue + nbr_pos, nbr_norm = P[idx], Nn[idx] + + guard = (target_facets is not None and coverage_weight > 0.0 + and n_assigned > 0) + + def objective(offset, base=base, nbr_pos=nbr_pos, nbr_norm=nbr_norm, + wt=wt, guard=guard, n_assigned=n_assigned): + pos = base + offset + J, det = pose_quality(pos, nbr_pos, nbr_norm, cfg, weights=wt, + sigma_scale=sigma_scale, return_detail=True) + pen = 0.0 + r = float(np.linalg.norm(offset)) + if r > max_offset: + pen += (r - max_offset) + clear = float(tree.query(pos, k=1)[0]) + if clear < cfg.safety_distance: + pen += (cfg.safety_distance - clear) + if z_min is not None and pos[2] < z_min: + pen += (z_min - pos[2]) + cov_pen = (coverage_weight * int(np.count_nonzero(det["q"][:n_assigned] <= 0.0)) + if guard else 0.0) + return -J + cov_pen + _PENALTY * pen + + sc_b[j], inc_b[j], de_b[j] = _pose_metrics(base, nbr_pos, nbr_norm, cfg) + j_before[j] = -objective(np.zeros(3)) + + res = minimize( + objective, np.zeros(3), method="Nelder-Mead", + options={"initial_simplex": np.vstack([np.zeros(3), init_step * np.eye(3)]), + "maxiter": max_iter, "xatol": 1e-3, "fatol": 1e-4}, + ) + n_iter[j] = int(res.nit) + + if -res.fun > j_before[j]: + pos = base + res.x + j_after[j] = -res.fun + else: + pos = base.copy() + j_after[j] = j_before[j] + new_pos[j] = pos + disp[j] = float(np.linalg.norm(pos - base)) + sc_a[j], inc_a[j], de_a[j] = _pose_metrics(pos, nbr_pos, nbr_norm, cfg) + + _, det = pose_quality(pos, nbr_pos, nbr_norm, cfg, weights=wt, + return_detail=True) + new_yaw[j], new_pitch[j] = det["yaw"], det["pitch"] + + refined = Poses( + positions=new_pos, + yaws=new_yaw, + pitches=new_pitch, + ids=poses.ids.copy(), + seed_facet=poses.seed_facet.copy(), + ) + info = RefineInfo( + j_before=j_before, j_after=j_after, displacement=disp, + soft_cov_before=sc_b, soft_cov_after=sc_a, + mean_inc_before=inc_b, mean_inc_after=inc_a, + d_err_before=de_b, d_err_after=de_a, n_iter=n_iter, + ) + return refined, info + + +def target_facets_from_visibility(V, rows: np.ndarray) -> list[np.ndarray]: + import scipy.sparse as sp + + Vsel = sp.csr_matrix(V)[rows] + return [Vsel.indices[Vsel.indptr[k]:Vsel.indptr[k + 1]] + for k in range(Vsel.shape[0])] + + +def subset_poses(poses: Poses, rows: np.ndarray) -> Poses: + rows = np.asarray(rows, dtype=np.int64) + return Poses( + positions=poses.positions[rows], + yaws=poses.yaws[rows], + pitches=poses.pitches[rows], + ids=poses.ids[rows], + seed_facet=(poses.seed_facet[rows] if len(poses.seed_facet) + else poses.seed_facet), + ) + + +def refine_selected_poses( + vis, poses: Poses, scene, selected: np.ndarray, cfg: Config, + *, + free: bool = False, + gain_weight: float = 0.0, + max_offset: float | None = None, + sigma_scale: float = 1.5, + coverage_weight: float = 4.0, + z_min: float | None = None, +) -> tuple[Poses, Poses, RefineInfo]: + sel = np.asarray(selected, dtype=np.int64) + before = subset_poses(poses, sel) + targets = None if free else target_facets_from_visibility(vis.V, sel) + + refined, info = refine_poses( + before, scene.facets, cfg, + target_facets=targets, + max_offset=max_offset, sigma_scale=sigma_scale, + coverage_weight=coverage_weight, gain_weight=gain_weight, z_min=z_min, + ) + + poses.positions[sel] = refined.positions + poses.yaws[sel] = refined.yaws + poses.pitches[sel] = refined.pitches + return before, refined, info diff --git a/student_code/260722_UAVViewPlanning/vpp3d/render_figures.py b/student_code/260722_UAVViewPlanning/vpp3d/render_figures.py new file mode 100644 index 0000000..3def7cf --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp3d/render_figures.py @@ -0,0 +1,340 @@ +from __future__ import annotations + +import argparse +import sys +from pathlib import Path + +import numpy as np + +try: + sys.stdout.reconfigure(encoding="utf-8") +except (AttributeError, ValueError): + pass + +from .config import load_config +from . import scene as scene_mod +from .candidates import sample_pose_regions, project_and_dedup +from .visibility import compute_visibility +from .setcover import ilp_set_cover, greedy_set_cover +from .overlap import ensure_connected +from .tracking import plan_tracking +from .sequencing import sequence_route +from . import inspector as insp + +_FIG_DIR = Path(__file__).resolve().parents[2] / "LaTex" / "abbildung" +_SCRATCH = Path(__file__).with_name("output") / "figtest" + +TEXTWIDTH_PT = 469.47 +BODY_PT = 11.0 +_FONT_PATH = r"C:\Windows\Fonts\arial.ttf" + +_WHITE = np.array([255, 255, 255], dtype=np.uint8) + + +def build_solution(scene, cfg, method: str = "ilp", time_limit: float = 60.0): + raw = sample_pose_regions(scene, cfg) + poses, _ = project_and_dedup(raw, scene, cfg) + vis = compute_visibility(scene, poses, cfg) + + if method == "ilp": + cover = ilp_set_cover(vis.V, k=1, time_limit=time_limit) + else: + cover = greedy_set_cover(vis.V, k=1) + + ov = ensure_connected(vis, cover.poses, cfg.min_overlap) + selected = ov.selected + trk = plan_tracking(scene, poses.positions[selected], cfg) + route = sequence_route(poses.positions[selected], trk) + return poses, vis, selected, route, ov, trk + + +def mosaic_geometries(scene, poses, vis, selected, route, ov, trk, cfg): + import open3d as o3d + + coverable = vis.coverage_per_facet() > 0 + palette = insp._distinct_colors(len(selected)) + plan_pos = poses.positions[selected] + overlap_ids = ov.overlap_facets if ov is not None else None + + geoms = [] + + facet_mesh = insp._make_facet_mesh(scene) + insp._set_facet_colors( + facet_mesh, + insp._colors_mosaic(vis, coverable, selected, palette, overlap_ids)) + geoms.append(facet_mesh) + + pcd_poses = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(plan_pos)) + pcd_poses.colors = o3d.utility.Vector3dVector(palette[:len(selected)]) + geoms.append(pcd_poses) + + geoms.append(insp._make_camera_pyramids(poses, selected, palette, cfg)) + + if len(route.order) > 1: + geoms.append(insp._lineset( + plan_pos[route.order], + [[i, i + 1] for i in range(len(route.order) - 1)], + insp._C_ROUTE)) + + pcd_trk, ls_trk = insp._make_tracking_geom(trk, plan_pos) + for g in (pcd_trk, ls_trk): + if g is not None: + geoms.append(g) + + lo, hi = scene.bounds + extent = float(np.ptp(scene.facets.centers, axis=0).max()) + geoms.append(insp._make_ground_grid( + float(lo[2]), lo, hi, step=max(0.5, round(extent / 10, 1)))) + + return geoms + + +def capture(geoms, *, width: int, height: int, front, up, zoom: float, + lookat=None, point_size: float = 9.0) -> np.ndarray: + import open3d as o3d + + viz = o3d.visualization.Visualizer() + viz.create_window(width=width, height=height, visible=False) + for g in geoms: + viz.add_geometry(g) + + ro = viz.get_render_option() + ro.background_color = np.array([1.0, 1.0, 1.0]) + ro.point_size = point_size + ro.line_width = 2.0 + ro.mesh_show_back_face = True + ro.light_on = True + + vc = viz.get_view_control() + vc.set_up(up) + vc.set_front(front) + if lookat is not None: + vc.set_lookat(lookat) + vc.set_zoom(zoom) + + for _ in range(5): + viz.poll_events() + viz.update_renderer() + + buf = viz.capture_screen_float_buffer(do_render=True) + viz.destroy_window() + return (np.asarray(buf) * 255.0 + 0.5).astype(np.uint8) + + +def autocrop(img: np.ndarray, pad_frac: float = 0.015) -> np.ndarray: + non_white = np.any(img < 250, axis=2) + if not non_white.any(): + return img + ys, xs = np.where(non_white) + y0, y1, x0, x1 = ys.min(), ys.max() + 1, xs.min(), xs.max() + 1 + img = img[y0:y1, x0:x1] + pad = int(round(pad_frac * max(img.shape[:2]))) + if pad: + img = np.pad(img, ((pad, pad), (pad, pad), (0, 0)), + constant_values=255) + return img + + +def _resize_to_height(img: np.ndarray, h: int) -> np.ndarray: + if img.shape[0] == h: + return img + from PIL import Image + w = max(1, int(round(img.shape[1] * h / img.shape[0]))) + return np.asarray(Image.fromarray(img).resize((w, h), Image.LANCZOS)) + + +def hconcat(imgs, gap_frac: float = 0.02) -> np.ndarray: + h = min(im.shape[0] for im in imgs) + imgs = [_resize_to_height(im, h) for im in imgs] + gap = int(round(gap_frac * h)) + sep = np.full((h, gap, 3), 255, np.uint8) + row = [] + for i, im in enumerate(imgs): + if i: + row.append(sep) + row.append(im) + return np.concatenate(row, axis=1) + + +def _font(px: int): + from PIL import ImageFont + return ImageFont.truetype(_FONT_PATH, px) + + +def _legend_items(): + pal = insp._distinct_colors(6) + return [ + ("multi", pal, "Scan-Footprint (Farbe je Pose)"), + ("swatch", insp._C_OVERLAP, "Registrierungs-Überlappung (≥ 2 Scans)"), + ("swatch", insp._C_UNREACH, "unerreichbar (horizontale Fläche)"), + ("line", insp._C_ROUTE, "Flugroute (TSP)"), + ("line", insp._C_TRACKER, "Tracking-Standort + Sichtlinie"), + ] + + +def draw_legend(width: int, font_px: int, items) -> np.ndarray: + from PIL import Image, ImageDraw + + font = _font(font_px) + sw = int(round(font_px * 1.15)) + gap_sym = int(round(font_px * 0.45)) + gap_item = int(round(font_px * 1.6)) + row_h = int(round(font_px * 1.9)) + pad_y = int(round(font_px * 0.6)) + + probe = ImageDraw.Draw(Image.new("RGB", (1, 1))) + + def text_w(s): + return int(probe.textlength(s, font=font)) + + widths = [sw + gap_sym + text_w(txt) for _, _, txt in items] + rows, cur, cur_w = [], [], 0 + for it, w in zip(items, widths): + add = w + (gap_item if cur else 0) + if cur and cur_w + add > width: + rows.append((cur, cur_w)); cur, cur_w = [], 0 + add = w + cur.append((it, w)); cur_w += add + if cur: + rows.append((cur, cur_w)) + + H = pad_y * 2 + row_h * len(rows) + canvas = Image.new("RGB", (width, H), (255, 255, 255)) + d = ImageDraw.Draw(canvas) + + for r, (row, row_w) in enumerate(rows): + x = (width - row_w) // 2 + cy = pad_y + row_h * r + row_h // 2 + for (art, color, txt), w in row: + top = cy - sw // 2 + if art == "line": + col = tuple(int(c * 255) for c in color) + d.line([(x, cy), (x + sw, cy)], fill=col, + width=max(3, font_px // 8)) + d.rectangle([x + sw // 2 - sw // 6, cy - sw // 6, + x + sw // 2 + sw // 6, cy + sw // 6], fill=col) + elif art == "multi": + n = len(color) + cw = sw / n + for k in range(n): + col = tuple(int(c * 255) for c in color[k]) + d.rectangle([x + int(k * cw), top, + x + int((k + 1) * cw), top + sw], fill=col) + d.rectangle([x, top, x + sw, top + sw], outline=(60, 60, 60), + width=max(1, font_px // 22)) + else: + col = tuple(int(c * 255) for c in color) + d.rectangle([x, top, x + sw, top + sw], fill=col, + outline=(60, 60, 60), width=max(1, font_px // 22)) + tx = x + sw + gap_sym + d.text((tx, cy), txt, font=font, fill=(20, 20, 20), anchor="lm") + x += w + gap_item + + return np.asarray(canvas) + + +def _panel_label(img: np.ndarray, text: str, font_px: int) -> np.ndarray: + from PIL import Image, ImageDraw + im = Image.fromarray(img) + d = ImageDraw.Draw(im) + m = int(round(font_px * 0.5)) + d.text((m, m), text, font=_font(font_px), fill=(20, 20, 20), anchor="lt") + return np.asarray(im) + + +def compose(panels, out_path: Path, *, labels=None) -> None: + from PIL import Image + + if labels: + pw = int(np.mean([p.shape[1] for p in panels])) + lab_px = max(12, int(round(BODY_PT * pw / TEXTWIDTH_PT))) + panels = [_panel_label(p, lab, lab_px) for p, lab in zip(panels, labels)] + + body = panels[0] if len(panels) == 1 else hconcat(panels) + W = body.shape[1] + font_px = max(12, int(round(BODY_PT * W / TEXTWIDTH_PT))) + legend = draw_legend(W, font_px, _legend_items()) + final = np.concatenate([body, legend], axis=0) + + out_path.parent.mkdir(parents=True, exist_ok=True) + Image.fromarray(final).save(out_path) + print(f" -> {out_path} ({final.shape[1]}x{final.shape[0]} px, " + f"Legende {font_px}px ~ {BODY_PT:.0f}pt @ \\textwidth)") + + +def _load_scene(spec, cfg): + if spec["kind"] == "synthetic": + return scene_mod.SCENES[spec["name"]](cfg.resolution) + return scene_mod.from_file(spec["mesh"], cfg.resolution, + assume_convex=spec.get("assume_convex", False), + name=spec["name"]) + + +SCENES = { + "box": dict( + kind="synthetic", name="box", out="box_ilp.png", + front=(0.6, -0.8, 0.35), up=(0, 0, 1), zoom=0.62, point_size=11.0), + "notched_box": dict( + kind="synthetic", name="notched_box", out="notched_box_ilp.png", + front=(0.75, -0.55, 0.38), up=(0, 0, 1), zoom=0.6, point_size=10.0), + "TestKorper1": dict( + kind="mesh", name="TestKorper1", mesh="Meshes/TestKorper1.stl", + out="TestKorper1_ilp.png", + front=(0.75, -0.55, 0.38), up=(0, 0, 1), zoom=0.6, point_size=10.0), +} + + +def render_panel(key: str, cfg, *, width: int, height: int, + method: str) -> np.ndarray: + spec = SCENES[key] + print(f"[{key}] Szene laden ...") + scene = _load_scene(spec, cfg) + print(f"[{key}] Loesung berechnen ({method}) ...") + poses, vis, selected, route, ov, trk = build_solution(scene, cfg, method) + print(f"[{key}] {len(selected)} Posen, {len(ov.overlap_facets)} " + f"Overlap-Facetten, {len(trk.stations)} Tracking-Standorte") + geoms = mosaic_geometries(scene, poses, vis, selected, route, ov, trk, cfg) + img = capture(geoms, width=width, height=height, front=spec["front"], + up=spec["up"], zoom=spec["zoom"], + point_size=spec.get("point_size", 9.0)) + return autocrop(img) + + +FIGURES = [ + ("box_ilp.png", ["box"], None), + ("notched_testkorper_ilp.png", ["notched_box", "TestKorper1"], + ["(a)", "(b)"]), +] + + +def main() -> None: + ap = argparse.ArgumentParser(description="Evaluationsabbildungen rendern") + ap.add_argument("--only", choices=list(SCENES), default=None, + help="nur eine Szene rendern (Einzelbild, ohne Montage)") + ap.add_argument("--method", default="ilp", choices=["ilp", "greedy"]) + ap.add_argument("--width", type=int, default=1920) + ap.add_argument("--height", type=int, default=1440) + ap.add_argument("--scratch", action="store_true", + help="in output/figtest statt LaTex/abbildung schreiben") + args = ap.parse_args() + + cfg = load_config() + out_dir = _SCRATCH if args.scratch else _FIG_DIR + + if args.only: + panel = render_panel(args.only, cfg, width=args.width, + height=args.height, method=args.method) + compose([panel], out_dir / SCENES[args.only]["out"]) + return + + cache = {} + for fname, keys, labels in FIGURES: + panels = [cache.setdefault( + k, render_panel(k, cfg, width=args.width, + height=args.height, method=args.method)) + for k in keys] + compose(panels, out_dir / fname, labels=labels) + + +if __name__ == "__main__": + main() diff --git a/student_code/260722_UAVViewPlanning/vpp3d/run.py b/student_code/260722_UAVViewPlanning/vpp3d/run.py new file mode 100644 index 0000000..3dc8f0a --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp3d/run.py @@ -0,0 +1,223 @@ +from __future__ import annotations + +import argparse +import sys +from pathlib import Path + +try: + sys.stdout.reconfigure(encoding="utf-8") +except (AttributeError, ValueError): + pass + +import numpy as np + +from .config import load_config, with_overrides +from . import scene as scene_mod +from .scene import scene_from_args +from .candidates import sample_pose_regions, project_and_dedup +from .visibility import compute_visibility +from .setcover import greedy_set_cover, ilp_set_cover, connected_set_cover_ilp +from .erosion import erode_footprints, cover_residual +from .overlap import ensure_connected +from .refine import refine_selected_poses +from .tracking import plan_tracking +from .sequencing import sequence_route +from .viz import plot_overview_png, show_open3d +from .runlog import RunLog + +_OUT = Path(__file__).with_name("output") + + +def add_common_args(ap: argparse.ArgumentParser) -> None: + ap.add_argument("--scene", default="box", choices=list(scene_mod.SCENES), + help="synthetische Testszene") + ap.add_argument("--mesh", default=None, + help="statt Szene: reales Mesh/Punktwolke (STL/PLY)") + ap.add_argument("--assume-convex", action="store_true", + help="Normalen radial nach aussen orientieren (Punktwolken)") + ap.add_argument("--resolution", type=float, default=None, + help="Facetten-Aufloesung [m] ueberschreiben (groesser = " + "weniger Facetten; noetig fuer grosse Objekte wie EasyCube)") + ap.add_argument("--k", type=int, default=1, + help="k-Coverage: jede Facette von mind. k Posen sehen lassen " + "(Redundanz fuer die Registrierung; Default 1 = " + "reine Vollabdeckung). Bedarf wird auf die erreichbare " + "Abdeckung gekappt.") + ap.add_argument("--baseline", type=float, default=None, + help="Basislinie ueberschreiben (0 = Einzelsicht-Ablation)") + ap.add_argument("--time-limit", type=float, default=60.0, help="Zeitlimit ILP [s]") + ap.add_argument("--erode", type=float, default=None, + help="[5a] Footprint-Erosion [m]: Overlap per Konstruktion " + "— Kandidaten-Footprints vor dem Set Cover um diesen " + "Rand erodieren (benachbarte Scans ueberlappen um " + "~2*erode). Ueberschreibt registration.erosion aus " + "der config.toml; 0 = aus.") + ap.add_argument("--no-overlap", action="store_true", + help="Konnektivitaets-Reparatur des Registrierungsgraphen " + "ueberspringen") + ap.add_argument("--no-tracking", action="store_true", + help="LoS-Tracking-Standorte ueberspringen") + ap.add_argument("--refine", action="store_true", + help="[5b] gewaehlte Posen lokal auf ein Qualitaetsoptimum " + "verschieben (glatte Score, abdeckungsbewusst)") + ap.add_argument("--refine-free", action="store_true", + help="ABLATION zu --refine: ohne Facetten-Zuweisung optimieren " + "(Posen kollabieren, Abdeckung bricht ein)") + ap.add_argument("--gain-weight", type=float, default=0.0, + help="--refine: > 0 nimmt benachbarte Facetten mit Gewicht g in " + "die Score auf (Abdeckungsmaximierung; d_max-Drift durch " + "die Distanz-Glocke gebremst; Default 0 = aus)") + ap.add_argument("--max-offset", type=float, default=None, + help="--refine: max. Suchradius je Pose [m] (Default 0.5·d_opt)") + ap.add_argument("--sigma-scale", type=float, default=1.5, + help="--refine: Breite der Distanz-Glocke (groesser = toleranter)") + ap.add_argument("--coverage-weight", type=float, default=4.0, + help="--refine: Abdeckungs-Barriere je zugewiesener Facette " + "(0 = aus)") + ap.add_argument("--z-min", type=float, default=None, + help="--refine: Bodenfilter, Posen nicht unter z_min schieben") + ap.add_argument("--no-ground-plane", action="store_true", + help="Bodenebene-Filter deaktivieren (Posen duerfen unter " + "z_boden + safety_distance liegen)") + ap.add_argument("--config", default=None, help="alternative config.toml") + + +def _apply_refinement(vis, poses, scene, selected, cfg, args): + before, refined, info = refine_selected_poses( + vis, poses, scene, selected, cfg, + free=args.refine_free, gain_weight=args.gain_weight, + max_offset=args.max_offset, sigma_scale=args.sigma_scale, + coverage_weight=args.coverage_weight, z_min=args.z_min, + ) + print("-" * 64) + print("[5b] " + info.summary()) + + vis_before = compute_visibility(scene, before, cfg) + vis_after = compute_visibility(scene, refined, cfg) + cov_b = int((np.asarray(vis_before.V.sum(axis=0)).ravel() > 0).sum()) + cov_a = int((np.asarray(vis_after.V.sum(axis=0)).ravel() > 0).sum()) + n = vis.V.shape[1] + print(f" Abdeckung (Teilmenge, neu geraycastet): {cov_b}/{n} ({cov_b / max(1, n):.1%})" + f" -> {cov_a}/{n} ({cov_a / max(1, n):.1%})") + return poses + + +def run(args) -> None: + cfg = with_overrides(load_config(args.config), args) + print(cfg.summary()) + print("=" * 64) + + scene = scene_from_args(args, cfg) + print(scene.summary()) + + raw = sample_pose_regions(scene, cfg) + poses, d = project_and_dedup(raw, scene, cfg) + ground_part = (f"-> Boden {d['n_after_ground']:,} " + if cfg.use_ground_plane else "") + print(f"[2/3] Posen: roh {d['n_raw']:,} -> Clearance {d['n_after_clearance']:,} " + f"{ground_part}" + f"-> Machbarkeit {d['n_after_feasibility']:,} -> Dedup {d['n_after_dedup']:,} " + f"(Faktor {d['n_after_feasibility'] / max(1, d['n_after_dedup']):.1f})") + + vis = compute_visibility(scene, poses, cfg) + print(vis.summary()) + if cfg.baseline > 0: + s = vis.stats + lost = s.get("occlusion_proj", 0) - s.get("dual", 0) + print(f" duale Sicht verwirft {lost:,} Paare, die die Einzelsicht " + f"zulassen wuerde (Kamera verdeckt).") + + erosion = cfg.erosion if args.erode is None else args.erode + V_solve = vis.V + if erosion > 0: + ero = erode_footprints(vis.V, scene.facets.centers, cfg.resolution, + erosion) + V_solve = ero.V + print("-" * 64) + print("[5a] " + ero.summary()) + + method_map = {"both": ["greedy", "ilp"], + "all": ["greedy", "ilp", "connected"]} + methods = method_map.get(args.method, [args.method]) + + def _solve(meth): + if meth == "greedy": + return greedy_set_cover(V_solve, k=args.k) + if meth == "ilp": + return ilp_set_cover(V_solve, k=args.k, time_limit=args.time_limit) + return connected_set_cover_ilp(V_solve, cfg.min_overlap, k=args.k, + time_limit=args.time_limit) + + results = {} + for meth in methods: + res = _solve(meth) + results[meth] = res + print("-" * 64) + print(res.summary()) + if erosion > 0 and len(res.poses): + extra, n_residual = cover_residual(vis.V, res.poses) + if n_residual: + res.poses = np.concatenate([res.poses, extra]) + print(f" Rest-Abdeckung (volles V) : +{len(extra)} " + f"Posen fuer {n_residual} Rand-Facetten") + cov = np.asarray(vis.V[res.poses].sum(axis=0)).ravel() > 0 + reach = np.asarray(vis.V.sum(axis=0)).ravel() > 0 + n_real = int((cov & reach).sum()) + print(f" reale Abdeckung (volles V) : " + f"{n_real}/{int(reach.sum())} " + f"({n_real / max(1, int(reach.sum())):.1%})") + + if "greedy" in results and "ilp" in results: + g, il = results["greedy"], results["ilp"] + print("-" * 64) + print(f"Greedy vs. ILP: Posen {len(g.poses)} vs. {len(il.poses)} " + f"(Faktor {len(g.poses) / max(1, len(il.poses)):.2f})") + + _OUT.mkdir(exist_ok=True) + log = RunLog(scene.name) + for meth, res in results.items(): + selected = res.poses + ov = None + if not args.no_overlap: + ov = ensure_connected(vis, res.poses, cfg.min_overlap) + selected = ov.selected + print("-" * 64) + print(ov.summary()) + + if args.refine: + poses = _apply_refinement(vis, poses, scene, selected, cfg, args) + + trk = None + if not args.no_tracking: + trk = plan_tracking(scene, poses.positions[selected], cfg) + print("-" * 64) + print(trk.summary()) + + route = sequence_route(poses.positions[selected], trk) + print(f"[7] {meth}: {route.summary()}") + png = _OUT / f"{scene.name}_{meth}.png" + plot_overview_png(scene, poses, vis, res, route, cfg, str(png), + show=False, selected=selected, overlap=ov, tracking=trk) + print(f" -> {png}") + log.save_matrix_png(vis, selected) + log.copy_png(png) + if args.show: + show_open3d(scene, poses, vis, res, route, cfg, + selected=selected, overlap=ov, tracking=trk) + + +def main() -> None: + ap = argparse.ArgumentParser(description="vpp3d — 3D-Prototyp (alternativer VPP-Ansatz)") + add_common_args(ap) + ap.add_argument("--method", default="greedy", + choices=["greedy", "ilp", "connected", "both", "all"], + help="greedy/ilp: Set Cover + Konnektivitaets-Reparatur; " + "connected: gemeinsames Connected-Set-Cover-ILP " + "(Abdeckung + Zusammenhang exakt); both=greedy+ilp, " + "all=greedy+ilp+connected") + ap.add_argument("--show", action="store_true", help="Open3D-Ansicht der Loesung") + run(ap.parse_args()) + + +if __name__ == "__main__": + main() diff --git a/student_code/260722_UAVViewPlanning/vpp3d/runlog.py b/student_code/260722_UAVViewPlanning/vpp3d/runlog.py new file mode 100644 index 0000000..d32c278 --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp3d/runlog.py @@ -0,0 +1,67 @@ +from __future__ import annotations + +import datetime +import shutil +from pathlib import Path + +import numpy as np + +_RUNS_ROOT = Path(__file__).with_name("output") / "runs" + + +class RunLog: + def __init__(self, scene_name: str = "scene"): + ts = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + self.dir = _RUNS_ROOT / f"{ts}_{scene_name}" + self.dir.mkdir(parents=True, exist_ok=True) + self.scene_name = scene_name + print(f" RunLog → {self.dir}") + + + def save_matrix_png(self, vis, selected_rows: np.ndarray) -> Path: + import matplotlib + matplotlib.use("Agg") + import matplotlib.pyplot as plt + + V_sel = np.asarray(vis.V[selected_rows].todense()).astype(np.uint8) + n_poses, n_facets = V_sel.shape + + col_order = np.argsort(-V_sel.sum(axis=0)) + V_disp = V_sel[:, col_order] + + MAX_W, MAX_H = 2000, 800 + step_c = max(1, n_facets // MAX_W) + step_r = max(1, n_poses // MAX_H) + V_disp = V_disp[::step_r, ::step_c] + + ds_c = f" (1:{step_c})" if step_c > 1 else "" + ds_r = f" (1:{step_r})" if step_r > 1 else "" + + fig_w = max(8, min(20, V_disp.shape[1] / 80)) + fig_h = max(3, min(10, V_disp.shape[0] / 15 + 1.2)) + fig, ax = plt.subplots(figsize=(fig_w, fig_h)) + ax.imshow(V_disp, aspect="auto", interpolation="nearest", + cmap="Greens", vmin=0, vmax=1) + ax.set_xlabel(f"Facetten ({n_facets}{ds_c}, sortiert nach Abdeckung)") + ax.set_ylabel(f"Gewaehlte Posen ({n_poses}{ds_r})") + ax.set_title( + f"Sichtbarkeitsmatrix {self.scene_name} — " + f"{n_poses} Posen × {n_facets} Facetten " + f"({int(V_sel.sum())} sichtbare Eintraege)" + ) + fig.tight_layout() + + path = self.dir / "visibility_matrix.png" + fig.savefig(path, dpi=120) + plt.close(fig) + print(f" Matrix PNG → {path}") + return path + + + def copy_png(self, src: "str | Path") -> Path | None: + src = Path(src) + if src.exists(): + dst = self.dir / src.name + shutil.copy2(src, dst) + return dst + return None diff --git a/student_code/260722_UAVViewPlanning/vpp3d/scene.py b/student_code/260722_UAVViewPlanning/vpp3d/scene.py new file mode 100644 index 0000000..f954d4b --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp3d/scene.py @@ -0,0 +1,256 @@ +from __future__ import annotations + +from dataclasses import dataclass, field + +import numpy as np + + +@dataclass +class Facets: + centers: np.ndarray + normals: np.ndarray + areas: np.ndarray + ids: np.ndarray + + def __len__(self) -> int: + return len(self.ids) + + +@dataclass +class Scene: + mesh: object + facets: Facets + name: str = "scene" + facet_verts: np.ndarray = field(default_factory=lambda: np.empty((0, 3))) + facet_tris: np.ndarray = field(default_factory=lambda: np.empty((0, 3), dtype=np.int64)) + + @property + def vertices(self) -> np.ndarray: + return np.asarray(self.mesh.vertices) + + @property + def triangles(self) -> np.ndarray: + return np.asarray(self.mesh.triangles) + + @property + def bounds(self) -> tuple[np.ndarray, np.ndarray]: + v = self.vertices + return v.min(axis=0), v.max(axis=0) + + def summary(self) -> str: + lo, hi = self.bounds + ext = hi - lo + return ( + f"Scene '{self.name}': {len(self.triangles)} Occluder-Dreiecke, " + f"{len(self.facets)} Facetten\n" + f" Ausdehnung: {ext[0]:.1f} × {ext[1]:.1f} × {ext[2]:.1f} m" + ) + + +def _edge_points_2d(pa: np.ndarray, pb: np.ndarray, res: float) -> np.ndarray: + seg = max(1, int(round(np.linalg.norm(pb - pa) / res))) + if seg < 2: + return np.empty((0, 2)) + t = np.arange(1, seg)[:, None] / seg + return pa[None] + t * (pb - pa)[None] + + +def _facets_from_triangles(verts: np.ndarray, tris: np.ndarray, + resolution: float, + assume_convex: bool = False) -> tuple: + from scipy.spatial import Delaunay, cKDTree + + all_centers: list[np.ndarray] = [] + all_normals: list[np.ndarray] = [] + all_areas: list[np.ndarray] = [] + all_fv: list[np.ndarray] = [] + all_ft: list[np.ndarray] = [] + vtx_off = 0 + + for i in range(len(tris)): + a = verts[tris[i, 0]].astype(float) + b = verts[tris[i, 1]].astype(float) + c = verts[tris[i, 2]].astype(float) + + n_raw = np.cross(b - a, c - a) + ln = np.linalg.norm(n_raw) + if ln < 1e-12: + continue + n_hat = n_raw / ln + + u_ax = (b - a) / np.linalg.norm(b - a) + v_ax = np.cross(n_hat, u_ax) + v_ax = v_ax / np.linalg.norm(v_ax) + + pa = np.zeros(2) + pb = np.array([np.dot(b - a, u_ax), np.dot(b - a, v_ax)]) + pc = np.array([np.dot(c - a, u_ax), np.dot(c - a, v_ax)]) + + boundary = [pa[None], pb[None], pc[None], + _edge_points_2d(pa, pb, resolution), + _edge_points_2d(pb, pc, resolution), + _edge_points_2d(pc, pa, resolution)] + bpts = np.vstack([p for p in boundary if len(p)]) + + p2 = np.stack([pa, pb, pc]) + interior = np.empty((0, 2)) + us = np.arange(p2[:, 0].min(), p2[:, 0].max() + resolution, resolution) + vs = np.arange(p2[:, 1].min(), p2[:, 1].max() + resolution, resolution) + if len(us) and len(vs): + ug, vg = np.meshgrid(us, vs) + cand = np.stack([ug.ravel(), vg.ravel()], axis=1) + T = np.array([[pb[0] - pa[0], pc[0] - pa[0]], + [pb[1] - pa[1], pc[1] - pa[1]]]) + lam = np.linalg.solve(T, (cand - pa).T).T + l1 = 1.0 - lam[:, 0] - lam[:, 1] + cand = cand[(l1 > 0) & (lam[:, 0] > 0) & (lam[:, 1] > 0)] + if len(cand): + d, _ = cKDTree(bpts).query(cand) + interior = cand[d > 0.5 * resolution] + + pts2d = np.vstack([bpts, interior]) if len(interior) else bpts + + if len(pts2d) < 3: + ctrs2d = ((pa + pb + pc) / 3.0)[None] + ctrs3d = a + ctrs2d[:, 0:1] * u_ax + ctrs2d[:, 1:2] * v_ax + all_fv.append(np.stack([a, b, c])) + all_ft.append(np.array([[0, 1, 2]], dtype=np.int64) + vtx_off) + vtx_off += 3 + all_centers.append(ctrs3d) + all_normals.append(n_hat[None]) + all_areas.append(np.array([0.5 * ln])) + continue + + simp = Delaunay(pts2d).simplices + d01 = pts2d[simp[:, 1]] - pts2d[simp[:, 0]] + d02 = pts2d[simp[:, 2]] - pts2d[simp[:, 0]] + cw = (d01[:, 0] * d02[:, 1] - d01[:, 1] * d02[:, 0]) < 0 + simp[cw] = simp[cw][:, [0, 2, 1]] + pts3d = a + pts2d[:, 0:1] * u_ax + pts2d[:, 1:2] * v_ax + + tri3 = pts3d[simp] + cr = np.cross(tri3[:, 1] - tri3[:, 0], tri3[:, 2] - tri3[:, 0]) + + all_fv.append(pts3d) + all_ft.append(simp.astype(np.int64) + vtx_off) + vtx_off += len(pts3d) + + all_centers.append(tri3.mean(axis=1)) + all_normals.append(np.tile(n_hat, (len(simp), 1))) + all_areas.append(0.5 * np.linalg.norm(cr, axis=1)) + + centers = np.vstack(all_centers) + normals = np.vstack(all_normals) + + if assume_convex: + centroid = centers.mean(axis=0) + flip = np.einsum("ij,ij->i", normals, centers - centroid) < 0 + normals[flip] *= -1.0 + + facets = Facets( + centers=centers, + normals=normals, + areas=np.concatenate(all_areas), + ids=np.arange(len(centers), dtype=np.int64), + ) + fv = np.vstack(all_fv) if all_fv else np.empty((0, 3)) + ft = np.vstack(all_ft).astype(np.int64) if all_ft else np.empty((0, 3), dtype=np.int64) + return facets, fv, ft + + +def _make_mesh(verts: np.ndarray, tris: np.ndarray): + import open3d as o3d + mesh = o3d.geometry.TriangleMesh( + o3d.utility.Vector3dVector(np.asarray(verts, float)), + o3d.utility.Vector3iVector(np.asarray(tris, np.int32)), + ) + mesh.compute_vertex_normals() + mesh.compute_triangle_normals() + return mesh + + +def build_scene(verts: np.ndarray, tris: np.ndarray, resolution: float, + name: str = "scene", assume_convex: bool = False) -> Scene: + facets, V, T = _facets_from_triangles(verts, tris, resolution, assume_convex) + return Scene(mesh=_make_mesh(verts, tris), facets=facets, name=name, + facet_verts=V, facet_tris=T.astype(np.int64)) + + +def _prism(poly2d: np.ndarray, z0: float, z1: float) -> tuple[np.ndarray, np.ndarray]: + m = len(poly2d) + bottom = np.column_stack([poly2d, np.full(m, z0)]) + top = np.column_stack([poly2d, np.full(m, z1)]) + verts = np.vstack([bottom, top]) + tris = [] + for i in range(m): + j = (i + 1) % m + tris.append([i, j, j + m]) + tris.append([i, j + m, i + m]) + for i in range(1, m - 1): + tris.append([0, i + 1, i]) + for i in range(1, m - 1): + tris.append([m, m + i, m + i + 1]) + return verts, np.asarray(tris, dtype=np.int64) + + +def box(resolution: float, size: float = 4.0, name: str = "box") -> Scene: + h = size / 2.0 + poly = np.array([[-h, -h], [h, -h], [h, h], [-h, h]], dtype=float) + return build_scene(*_prism(poly, -h, h), resolution, name) + + +def notched_box(resolution: float, w: float = 4.4, h: float = 2.0, d: float = 3.1, + notch: float = 1.2, name: str = "notched_box") -> Scene: + cy = h / 2.0 + poly = np.array([ + [0.0, 0.0], + [w, 0.0], + [w, cy - notch / 2], + [w - notch, cy - notch / 2], + [w - notch, cy + notch / 2], + [w, cy + notch / 2], + [w, h], + [0.0, h], + ], dtype=float) + return build_scene(*_prism(poly, 0.0, d), resolution, name) + + +def from_file(path: str, resolution: float, assume_convex: bool = False, + poisson_depth: int = 8, name: str | None = None) -> Scene: + import open3d as o3d + + stem = path.replace("\\", "/").split("/")[-1].rsplit(".", 1)[0] + nm = name or stem + + mesh = o3d.io.read_triangle_mesh(path) + if len(mesh.triangles) > 0: + return build_scene(np.asarray(mesh.vertices), np.asarray(mesh.triangles), + resolution, nm, assume_convex) + + pcd = o3d.io.read_point_cloud(path) + nn = np.asarray(pcd.compute_nearest_neighbor_distance()) + pcd.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid( + radius=3.0 * float(np.median(nn)), max_nn=30)) + if assume_convex: + pcd.orient_normals_towards_camera_location(pcd.get_center()) + pcd.normals = o3d.utility.Vector3dVector(-np.asarray(pcd.normals)) + else: + pcd.orient_normals_consistent_tangent_plane(30) + rec, densities = o3d.geometry.TriangleMesh.create_from_point_cloud_poisson( + pcd, depth=poisson_depth) + dens = np.asarray(densities) + rec.remove_vertices_by_mask(dens < np.quantile(dens, 0.02)) + return build_scene(np.asarray(rec.vertices), np.asarray(rec.triangles), + resolution, nm, assume_convex=False) + + +SCENES = { + "box": box, + "notched_box": notched_box, +} + + +def scene_from_args(args, cfg) -> Scene: + if getattr(args, "mesh", None): + return from_file(args.mesh, cfg.resolution, assume_convex=args.assume_convex) + return SCENES[args.scene](cfg.resolution) diff --git a/student_code/260722_UAVViewPlanning/vpp3d/sequencing.py b/student_code/260722_UAVViewPlanning/vpp3d/sequencing.py new file mode 100644 index 0000000..9d9beb0 --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp3d/sequencing.py @@ -0,0 +1,122 @@ +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np + + +def _nearest_neighbor(D: np.ndarray, start: int) -> np.ndarray: + n = len(D) + visited = np.zeros(n, dtype=bool) + order = np.empty(n, dtype=np.int64) + order[0] = start + visited[start] = True + for i in range(1, n): + d = D[order[i - 1]].copy() + d[visited] = np.inf + order[i] = int(np.argmin(d)) + visited[order[i]] = True + return order + + +def _two_opt(order: np.ndarray, D: np.ndarray, max_rounds: int = 12) -> np.ndarray: + order = order.copy() + n = len(order) + for _ in range(max_rounds): + improved = False + for i in range(n - 2): + for j in range(i + 2, n): + a, b, c = order[i], order[i + 1], order[j] + if j + 1 < n: + d = order[j + 1] + d_old = D[a, b] + D[c, d] + d_new = D[a, c] + D[b, d] + else: + d_old = D[a, b] + d_new = D[a, c] + if d_new + 1e-12 < d_old: + order[i + 1 : j + 1] = order[i + 1 : j + 1][::-1] + improved = True + if not improved: + break + return order + + +def solve_tsp_path(points: np.ndarray, start: int = 0) -> np.ndarray: + n = len(points) + if n <= 2: + others = [i for i in range(n) if i != start] + return np.asarray([start] + others, dtype=np.int64) + D = np.linalg.norm(points[:, None] - points[None, :], axis=2) + return _two_opt(_nearest_neighbor(D, start), D) + + +@dataclass +class Route: + order: np.ndarray + length: float + station_ids: np.ndarray = None + n_repositions: int = 0 + + def __post_init__(self) -> None: + if self.station_ids is None: + self.station_ids = np.full(len(self.order), -1, dtype=np.int64) + + def summary(self) -> str: + txt = f"Route: {len(self.order)} Posen, Flugweg {self.length:.1f} m" + if self.n_repositions or (self.station_ids >= 0).any(): + n_seg = int(self.station_ids.max()) + 1 if (self.station_ids >= 0).any() else 0 + txt += (f", {n_seg} Tracking-Segmente " + f"({self.n_repositions} Repositionierungen)") + return txt + + +def sequence_route(pose_positions: np.ndarray, tracking=None) -> Route: + positions = np.asarray(pose_positions, dtype=np.float64).reshape(-1, 3) + P = len(positions) + if P == 0: + return Route(np.empty(0, np.int64), 0.0, np.empty(0, np.int64), 0) + + stations = (np.asarray(tracking.stations, dtype=np.float64).reshape(-1, 3) + if tracking is not None else np.empty((0, 3))) + + if len(stations) == 0: + order = solve_tsp_path(positions, start=0) + length = float(np.linalg.norm(np.diff(positions[order], axis=0), axis=1).sum()) + return Route(order, length, np.full(P, -1, np.int64), 0) + + assignment = np.asarray(tracking.assignment, dtype=np.int64).reshape(-1).copy() + + untracked = assignment < 0 + if untracked.any(): + d = np.linalg.norm( + stations[:, None, :2] - positions[None, untracked, :2], axis=2) + assignment[untracked] = np.argmin(d, axis=0) + + counts = np.bincount(assignment, minlength=len(stations)) + st_order = solve_tsp_path(stations, start=int(np.argmax(counts))) + + route_idx: list[int] = [] + station_per_pose: list[int] = [] + visited = 0 + prev_end: np.ndarray | None = None + for st in st_order: + members = np.flatnonzero(assignment == st) + if len(members) == 0: + continue + pts = positions[members] + start = 0 if prev_end is None else int( + np.argmin(np.linalg.norm(pts - prev_end, axis=1))) + local = solve_tsp_path(pts, start=start) + route_idx.extend(members[local].tolist()) + station_per_pose.extend([visited] * len(members)) + visited += 1 + prev_end = pts[local[-1]] + + order = np.asarray(route_idx, dtype=np.int64) + length = float(np.linalg.norm(np.diff(positions[order], axis=0), axis=1).sum()) + + station_ids = np.asarray(station_per_pose, dtype=np.int64) + station_ids[untracked[order]] = -1 + + return Route(order, length, station_ids, max(0, visited - 1)) diff --git a/student_code/260722_UAVViewPlanning/vpp3d/setcover.py b/student_code/260722_UAVViewPlanning/vpp3d/setcover.py new file mode 100644 index 0000000..23cd5bc --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp3d/setcover.py @@ -0,0 +1,265 @@ +from __future__ import annotations + +import time +from dataclasses import dataclass, field + +import numpy as np +import scipy.sparse as sp +from scipy.sparse.csgraph import connected_components, breadth_first_order + + +@dataclass +class CoverResult: + poses: np.ndarray + method: str + k: int = 1 + n_coverable: int = 0 + n_covered: int = 0 + runtime: float = 0.0 + optimal: bool = False + extra: dict = field(default_factory=dict) + + def summary(self) -> str: + opt = " (Optimum bewiesen)" if self.optimal else "" + kstr = f", k={self.k}" if self.k > 1 else "" + return ( + f"Set Cover [{self.method}{opt}{kstr}]:\n" + f" Drohnenposen (Scans) : {len(self.poses)}\n" + f" Abdeckung erreichbarer Facetten : " + f"{self.n_covered}/{self.n_coverable} " + f"({self.n_covered / max(1, self.n_coverable):.1%})\n" + f" Rechenzeit : {self.runtime:.2f} s" + ) + + +def _demand(V: sp.csr_matrix, k: int) -> np.ndarray: + avail = np.asarray(V.sum(axis=0)).ravel().astype(np.int64) + return np.minimum(int(k), avail) + + +def greedy_set_cover( + V: sp.csr_matrix, k: int = 1, trace: list | None = None +) -> CoverResult: + t0 = time.perf_counter() + Vc = V.tocsr().astype(np.int64) + demand = _demand(Vc, k) + coverable = demand > 0 + deficit = demand.copy() + chosen = np.zeros(Vc.shape[0], dtype=bool) + selected: list[int] = [] + + while deficit.any(): + gain = np.asarray(Vc @ (deficit > 0).astype(np.int64)).ravel() + gain[chosen] = 0 + j = int(np.argmax(gain)) + if gain[j] == 0: + break + seen = Vc.indices[Vc.indptr[j]:Vc.indptr[j + 1]] + if trace is not None: + open_seen = deficit[seen] > 0 + trace.append({ + "pose": int(j), + "covered_before": (coverable & (deficit == 0)).copy(), + "new_facets": seen[open_seen].copy(), + "overlap_facets": seen[~open_seen].copy(), + }) + chosen[j] = True + selected.append(j) + deficit[seen] = np.maximum(deficit[seen] - 1, 0) + + return CoverResult( + poses=np.asarray(selected, dtype=np.int64), + method="greedy", + k=int(k), + n_coverable=int(coverable.sum()), + n_covered=int((coverable & (deficit == 0)).sum()), + runtime=time.perf_counter() - t0, + ) + + +def ilp_set_cover( + V: sp.csr_matrix, k: int = 1, time_limit: float = 120.0 +) -> CoverResult: + from ortools.sat.python import cp_model + + t0 = time.perf_counter() + Vcsc = V.tocsc() + M, N = V.shape + demand = _demand(V, k) + + model = cp_model.CpModel() + x = [model.new_bool_var(f"x{j}") for j in range(M)] + for i in np.flatnonzero(demand): + rows = Vcsc.indices[Vcsc.indptr[i]:Vcsc.indptr[i + 1]] + model.add(sum(x[j] for j in rows) >= int(demand[i])) + model.minimize(sum(x)) + + solver = cp_model.CpSolver() + solver.parameters.max_time_in_seconds = float(time_limit) + solver.parameters.num_workers = 8 + status = solver.solve(model) + if status not in (cp_model.OPTIMAL, cp_model.FEASIBLE): + raise RuntimeError(f"CP-SAT ohne Loesung ({solver.status_name(status)})") + + sel = np.asarray([j for j in range(M) if solver.value(x[j])], dtype=np.int64) + times = np.asarray(V.tocsr()[sel].sum(axis=0)).ravel() if sel.size else np.zeros(N) + return CoverResult( + poses=sel, + method="ilp", + k=int(k), + n_coverable=int((demand > 0).sum()), + n_covered=int(((demand > 0) & (times >= demand)).sum()), + runtime=time.perf_counter() - t0, + optimal=status == cp_model.OPTIMAL, + extra={"solver_status": solver.status_name(status)}, + ) + + +def _sparsify_topk(adj: sp.csr_matrix, max_degree: int) -> sp.csr_matrix: + A = adj.tocsr() + n = A.shape[0] + rows: list[np.ndarray] = [] + cols: list[np.ndarray] = [] + for i in range(n): + s, e = A.indptr[i], A.indptr[i + 1] + idx, dat = A.indices[s:e], A.data[s:e] + if len(idx) > max_degree: + idx = idx[np.argpartition(dat, -max_degree)[-max_degree:]] + rows.append(np.full(len(idx), i, dtype=np.int64)) + cols.append(idx.astype(np.int64)) + r = np.concatenate(rows) if rows else np.empty(0, np.int64) + c = np.concatenate(cols) if cols else np.empty(0, np.int64) + m = sp.csr_matrix((np.ones(len(r), dtype=bool), (r, c)), shape=(n, n)) + return (m + m.T).astype(bool) + + +def _hint_from_warm(model, warm_local, adj_s, x, y, g, arc_var, n) -> None: + w = len(warm_local) + x_h = np.zeros(n, np.int64) + y_h = np.zeros(n, np.int64) + g_h = np.zeros(n, np.int64) + arc_h = {key: 0 for key in arc_var} + if w >= 1: + x_h[warm_local] = 1 + if w == 1: + r = int(warm_local[0]); y_h[r] = 1; g_h[r] = 1 + elif w >= 2: + sub = adj_s[warm_local][:, warm_local] + order, preds = breadth_first_order(sub, 0, directed=False, + return_predecessors=True) + subtree = np.ones(w, np.int64) + for node in order[::-1]: + p = preds[node] + if p >= 0: + subtree[p] += subtree[node] + root = int(warm_local[0]); y_h[root] = 1; g_h[root] = int(w) + for node in order: + p = preds[node] + if p < 0: + continue + key = (int(warm_local[p]), int(warm_local[node])) + if key in arc_h: + arc_h[key] = int(subtree[node]) + for j in range(n): + model.add_hint(x[j], int(x_h[j])) + model.add_hint(y[j], int(y_h[j])) + model.add_hint(g[j], int(g_h[j])) + for key, var in arc_var.items(): + model.add_hint(var, int(arc_h[key])) + + +def connected_set_cover_ilp( + V: sp.csr_matrix, min_overlap: float, k: int = 1, time_limit: float = 120.0, + max_degree: int = 8, +) -> CoverResult: + from ortools.sat.python import cp_model + + from .overlap import overlap_adjacency, ensure_connected + from .visibility import VisibilityMatrix + + t0 = time.perf_counter() + Vcsr = V.tocsr() + N = Vcsr.shape[1] + demand = _demand(Vcsr, k) + + nodes, adj = overlap_adjacency(Vcsr, min_overlap) + n = len(nodes) + if n == 0: + return CoverResult(poses=np.empty(0, np.int64), method="connected", + k=int(k), runtime=time.perf_counter() - t0) + + Vn = Vcsr[nodes].tocsc() + g2l = {int(gid): li for li, gid in enumerate(nodes)} + + warm = ensure_connected(VisibilityMatrix(V=Vcsr), + greedy_set_cover(Vcsr, k=k).poses, min_overlap) + warm_local = np.array([g2l[int(p)] for p in warm.selected], dtype=np.int64) + cap = max(1, len(warm_local)) + + adj_s = _sparsify_topk(adj, max_degree) + if len(warm_local) > 1: + sub = adj[warm_local][:, warm_local].tocoo() + wr = warm_local[sub.row]; wc = warm_local[sub.col] + extra = sp.csr_matrix((np.ones(len(wr), bool), (wr, wc)), shape=(n, n)) + adj_s = (adj_s + extra + extra.T).astype(bool) + n_comp, comp = connected_components(adj_s, directed=False) + + model = cp_model.CpModel() + x = [model.new_bool_var(f"x{j}") for j in range(n)] + y = [model.new_bool_var(f"y{j}") for j in range(n)] + g = [model.new_int_var(0, cap, f"g{j}") for j in range(n)] + + for i in np.flatnonzero(demand): + rows_i = Vn.indices[Vn.indptr[i]:Vn.indptr[i + 1]] + model.add(sum(x[j] for j in rows_i) >= int(demand[i])) + + for j in range(n): + model.add(y[j] <= x[j]) + model.add(g[j] <= cap * y[j]) + for c in range(n_comp): + members = np.flatnonzero(comp == c) + model.add(sum(y[int(j)] for j in members) <= 1) + + edges = sp.triu(adj_s, k=1).tocoo() + inflow: list[list] = [[] for _ in range(n)] + outflow: list[list] = [[] for _ in range(n)] + arc_var: dict[tuple[int, int], object] = {} + for u, v in zip(edges.row.tolist(), edges.col.tolist()): + f_uv = model.new_int_var(0, cap, f"f_{u}_{v}") + f_vu = model.new_int_var(0, cap, f"f_{v}_{u}") + model.add(f_uv <= cap * x[u]); model.add(f_uv <= cap * x[v]) + model.add(f_vu <= cap * x[u]); model.add(f_vu <= cap * x[v]) + outflow[u].append(f_uv); inflow[v].append(f_uv) + outflow[v].append(f_vu); inflow[u].append(f_vu) + arc_var[(u, v)] = f_uv; arc_var[(v, u)] = f_vu + + for j in range(n): + model.add(g[j] + sum(inflow[j]) - sum(outflow[j]) == x[j]) + + model.minimize(sum(x)) + model.add(sum(x) <= cap) + + _hint_from_warm(model, warm_local, adj_s, x, y, g, arc_var, n) + + solver = cp_model.CpSolver() + solver.parameters.max_time_in_seconds = float(time_limit) + solver.parameters.num_workers = 8 + status = solver.solve(model) + if status not in (cp_model.OPTIMAL, cp_model.FEASIBLE): + raise RuntimeError(f"CP-SAT ohne Loesung ({solver.status_name(status)})") + + poses = nodes[[j for j in range(n) if solver.value(x[j])]] + times = np.asarray(Vcsr[poses].sum(axis=0)).ravel() if len(poses) else np.zeros(N) + return CoverResult( + poses=poses.astype(np.int64), + method="connected", + k=int(k), + n_coverable=int((demand > 0).sum()), + n_covered=int(((demand > 0) & (times >= demand)).sum()), + runtime=time.perf_counter() - t0, + optimal=status == cp_model.OPTIMAL, + extra={"solver_status": solver.status_name(status), + "graph_components": int(n_comp), + "warm_poses": int(len(warm_local)), + "objective_bound": float(solver.best_objective_bound)}, + ) diff --git a/student_code/260722_UAVViewPlanning/vpp3d/stepfigs.py b/student_code/260722_UAVViewPlanning/vpp3d/stepfigs.py new file mode 100644 index 0000000..a627300 --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp3d/stepfigs.py @@ -0,0 +1,352 @@ +from __future__ import annotations + +import argparse +import sys +from dataclasses import replace +from pathlib import Path + +import numpy as np + +try: + sys.stdout.reconfigure(encoding="utf-8") +except (AttributeError, ValueError): + pass + +from .config import load_config, with_overrides +from . import scene as scene_mod +from .scene import scene_from_args +from .candidates import (Poses, sample_pose_regions, dedup_keys, _clearance_mask, + _cone_directions, _tangent_basis, _vertical_fov_ok) +from .visibility import compute_visibility +from . import inspector as insp +from .render_figures import capture + +_OUT = Path(__file__).with_name("output") / "praesentation" + +C_MESH = (0.86, 0.87, 0.89) +C_FACET = (0.10, 0.25, 0.60) +C_NORMAL = (0.95, 0.45, 0.00) +C_KEEP = (0.13, 0.66, 0.30) +C_DROP = (0.85, 0.20, 0.15) +C_RAW = (0.62, 0.64, 0.70) +C_RAY = (0.55, 0.60, 0.70) +C_SEED = (1.00, 0.45, 0.00) +C_PROJ = (0.05, 0.05, 0.05) +C_CAM = (0.00, 0.45, 0.90) + +VIEWS = { + "box": dict(front=(0.6, -0.8, 0.35), up=(0, 0, 1), zoom=0.52), + "notched_box": dict(front=(0.78, -0.52, 0.36), up=(0, 0, 1), zoom=0.52), +} +_VIEW_DEFAULT = VIEWS["notched_box"] + + +def _mesh(scene, color=C_MESH): + import open3d as o3d + m = o3d.geometry.TriangleMesh(scene.mesh) + m.paint_uniform_color(list(color)) + m.compute_vertex_normals() + return m + + +def _cloud(pts, colors): + import open3d as o3d + pts = np.asarray(pts, float).reshape(-1, 3) + if not len(pts): + return None + pc = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(pts)) + colors = np.asarray(colors, float) + if colors.ndim == 1: + colors = np.tile(colors, (len(pts), 1)) + pc.colors = o3d.utility.Vector3dVector(colors) + return pc + + +def _ball(p, r, color): + import open3d as o3d + b = o3d.geometry.TriangleMesh.create_sphere(radius=r, resolution=24) + b.translate(np.asarray(p, float)) + b.paint_uniform_color(list(color)) + b.compute_vertex_normals() + return b + + +def _arrows(origins, dirs, length, color, slim=0.06, resolution=8): + import open3d as o3d + origins = np.asarray(origins, float).reshape(-1, 3) + d = np.asarray(dirs, float).reshape(-1, 3) + if not len(d): + return None + + r = slim * length + tmpl = o3d.geometry.TriangleMesh.create_arrow( + cylinder_radius=r, cone_radius=2.2 * r, + cylinder_height=0.70 * length, cone_height=0.30 * length, + resolution=resolution, cylinder_split=1, cone_split=1) + V0 = np.asarray(tmpl.vertices) + T0 = np.asarray(tmpl.triangles) + + a = np.column_stack([-d[:, 1], d[:, 0], np.zeros(len(d))]) + c = d[:, 2] + K = np.zeros((len(d), 3, 3)) + K[:, 0, 1], K[:, 0, 2] = -a[:, 2], a[:, 1] + K[:, 1, 0], K[:, 1, 2] = a[:, 2], -a[:, 0] + K[:, 2, 0], K[:, 2, 1] = -a[:, 1], a[:, 0] + denom = np.maximum(1.0 + c, 1e-12) + R = np.eye(3) + K + (K @ K) / denom[:, None, None] + R[1.0 + c < 1e-9] = np.diag([1.0, -1.0, -1.0]) + + V = np.einsum("nij,kj->nki", R, V0) + origins[:, None, :] + T = T0[None] + (np.arange(len(d)) * len(V0))[:, None, None] + out = o3d.geometry.TriangleMesh( + o3d.utility.Vector3dVector(V.reshape(-1, 3)), + o3d.utility.Vector3iVector(T.reshape(-1, 3).astype(np.int32))) + out.paint_uniform_color(list(color)) + out.compute_vertex_normals() + return out + + +def _rays(origins, targets, color): + targets = np.asarray(targets, float).reshape(-1, 3) + if not len(targets): + return None + origins = np.broadcast_to(np.asarray(origins, float).reshape(-1, 3), + targets.shape) + k = len(targets) + return insp._lineset(np.vstack([origins, targets]), + [[i, i + k] for i in range(k)], color) + + +def _frustum(pos, yaw, pitch, cfg, color): + pts = insp._frustum_points(np.asarray(pos, float), yaw, pitch, cfg) + idx = ([[0, 5 + i] for i in range(4)] + + [[1 + i, 1 + (i + 1) % 4] for i in range(4)] + + [[5 + i, 5 + (i + 1) % 4] for i in range(4)]) + return insp._lineset(pts, idx, color) + + +def _anchor(scene, cfg): + lo, hi = scene.bounds + m = cfg.d_max + corners = np.array([[x, y, z] for x in (lo[0] - m, hi[0] + m) + for y in (lo[1] - m, hi[1] + m) + for z in (lo[2] - m, hi[2] + m)]) + return _cloud(corners, (1.0, 1.0, 1.0)) + + +def _lift(scene, offset_frac=0.006): + fc = scene.facets + return fc.centers + fc.normals * (offset_frac * _extent(scene)) + + +def _extent(scene) -> float: + lo, hi = scene.bounds + return float((hi - lo).max()) + + +def _crop(img: np.ndarray, frac: float) -> np.ndarray: + h, w = img.shape[:2] + ch, cw = int(h * frac), int(w * frac) + return img[(h - ch) // 2:(h + ch) // 2, (w - cw) // 2:(w + cw) // 2] + + +def _sub(n: int, k: int) -> np.ndarray: + if n <= k: + return np.arange(n) + return np.unique(np.linspace(0, n - 1, k).astype(np.int64)) + + +class Stages: + def __init__(self, scene, cfg): + raw = sample_pose_regions(scene, cfg) + self.raw = raw + self.clear = _clearance_mask(raw.positions, scene, cfg.safety_distance) + self.ground = (raw.positions[:, 2] >= scene.bounds[0][2] + cfg.ground_clearance + if cfg.use_ground_plane else np.ones(len(raw), bool)) + self.dist = self.clear & self.ground + + self.pitch = np.clip(raw.pitches, cfg.pitch_min_rad, cfg.pitch_max_rad) + idx = np.flatnonzero(self.dist) + self.feasible = np.zeros(len(raw), bool) + self.feasible[idx] = _vertical_fov_ok( + raw.positions[idx], raw.yaws[idx], self.pitch[idx], + scene.facets.centers[raw.seed_facet[idx]], cfg) + + surv = np.flatnonzero(self.feasible) + keys = dedup_keys(raw.positions[surv], raw.yaws[surv], + self.pitch[surv], cfg) + _, first = np.unique(keys, axis=0, return_index=True) + self.survivors = surv + self.reps = surv[np.sort(first)] + + def summary(self) -> str: + return (f" roh {len(self.raw):,} -> Abstand {int(self.dist.sum()):,} " + f"-> Machbarkeit {int(self.feasible.sum()):,} " + f"-> Dedup {len(self.reps):,}") + + +def _facet_ps(scene, cfg) -> float: + return float(np.clip(320.0 * cfg.resolution / _extent(scene), 3.0, 10.0)) + + +def fig_facetten(scene, cfg, st): + return [_mesh(scene), _cloud(_lift(scene), C_FACET)], _facet_ps(scene, cfg) + + +def fig_normalen(scene, cfg, st): + fc = scene.facets + pts = _lift(scene) + length = min(1.8 * cfg.resolution, 0.09 * _extent(scene)) + return [_mesh(scene), _cloud(pts, C_FACET), + _arrows(pts, fc.normals, length, C_NORMAL, + slim=0.045)], _facet_ps(scene, cfg) + + +def _seed_facet(scene, front) -> int: + fc = scene.facets + f = np.asarray(front, float) / np.linalg.norm(front) + cand = np.flatnonzero(fc.normals @ f > 0.5) + if not len(cand): + cand = np.arange(len(fc)) + target = fc.centers.mean(axis=0) + f * 0.5 * _extent(scene) + return int(cand[np.argmin(np.linalg.norm(fc.centers[cand] - target, axis=1))]) + + +def fig_kegel(scene, cfg, st): + fc = scene.facets + fi = _seed_facet(scene, VIEWS.get(scene.name, _VIEW_DEFAULT)["front"]) + c, n = fc.centers[fi], fc.normals[fi] + + t1, t2 = _tangent_basis(n[None, :]) + dirs = _cone_directions(n[None, :], t1, t2, cfg)[:, 0, :] + dists = (np.array([cfg.d_opt]) if cfg.n_distance == 1 + else np.linspace(cfg.d_min, cfg.d_max, cfg.n_distance)) + pos = (c[None, None, :] + dirs[:, None, :] * dists[None, :, None]).reshape(-1, 3) + + return [_mesh(scene), _ball(c, 0.03 * _extent(scene), C_SEED), + _rays(c, c + dirs * cfg.d_max, C_RAY), + _cloud(pos, C_KEEP)], 12.0 + + +def _thin(idx, k: int) -> np.ndarray: + idx = np.asarray(idx) + return idx[_sub(len(idx), k)] + + +def fig_dedup(scene, cfg, st): + pos = st.raw.positions + drop = _thin(np.setdiff1d(st.survivors, st.reps), 10_000) + return [_mesh(scene), _cloud(pos[drop], (0.78, 0.80, 0.85)), + _cloud(pos[st.reps], C_KEEP)], 5.0 + + +def fig_abstand(scene, cfg, st): + pos = st.raw.positions + lo, hi = scene.bounds + grid = insp._make_ground_grid(float(lo[2]), lo, hi, + step=max(0.5, round(_extent(scene) / 10, 1))) + return [_mesh(scene), grid, + _cloud(pos[_thin(np.flatnonzero(~st.dist), 40_000)], C_DROP), + _cloud(pos[_thin(np.flatnonzero(st.dist), 40_000)], C_KEEP)], 3.5 + + +def fig_dualsicht(scene, cfg, st, row=None, n_rays=45): + poses = Poses(st.raw.positions[st.reps], st.raw.yaws[st.reps], + st.pitch[st.reps], np.arange(len(st.reps)), + st.raw.seed_facet[st.reps]) + v_dual = compute_visibility(scene, poses, cfg) + v_single = compute_visibility(scene, poses, replace(cfg, baseline=0.0)) + diff = (v_single.V.astype(np.int8) - v_dual.V.astype(np.int8)).tocsr() + diff.eliminate_zeros() + + if row is None: + f = np.asarray(VIEWS.get(scene.name, _VIEW_DEFAULT)["front"], float) + w = (scene.facets.normals @ (f / np.linalg.norm(f)) > 0.3).astype(np.int32) + row = int(np.argmax((diff @ w) * (v_dual.V.astype(np.int32) @ w))) + lost, kept = diff[row].indices, v_dual.V[row].indices + print(f" Dualsicht: Pose #{row}, {len(kept)} Facetten dual, " + f"{len(lost)} nur vom Projektor") + + p = poses.positions[row] + yaw, pitch = float(poses.yaws[row]), float(poses.pitches[row]) + _, right, _ = insp._pose_frame(yaw, pitch) + p_cam = p + cfg.baseline * right + + colors = np.tile(np.asarray(C_MESH), (len(scene.facets), 1)) + colors[kept] = C_KEEP + colors[lost] = C_DROP + fmesh = insp._make_facet_mesh(scene) + insp._set_facet_colors(fmesh, colors) + + ctrs = scene.facets.centers + r = 0.03 * _extent(scene) + return [fmesh, + _frustum(p, yaw, pitch, cfg, C_PROJ), + _frustum(p_cam, yaw, pitch, cfg, C_CAM), + insp._lineset([p, p_cam], [[0, 1]], C_CAM), + _rays(p_cam, ctrs[lost[_sub(len(lost), n_rays)]], C_DROP), + _rays(p, ctrs[kept[_sub(len(kept), n_rays)]], C_KEEP), + _ball(p, r, C_PROJ), _ball(p_cam, r, C_CAM)], 6.0 + + +FIGURES = { + 1: ("1_facetten", fig_facetten), + 2: ("2_normalen", fig_normalen), + 3: ("3_kegelsampling", fig_kegel), + 4: ("4_dedup", fig_dedup), + 5: ("5_abstand", fig_abstand), + 6: ("6_dualsicht", fig_dualsicht), +} + + +def render(key: int, scene, cfg, st, args, out_dir: Path) -> None: + from PIL import Image + + name, builder = FIGURES[key] + print(f"[{key}] {name} ...") + geoms, point_size = builder(scene, cfg, st) + geoms = [g for g in geoms if g is not None] + [_anchor(scene, cfg)] + lo, hi = scene.bounds + img = _crop(capture(geoms, width=args.width, height=args.height, + point_size=point_size, lookat=0.5 * (lo + hi), + **VIEWS.get(scene.name, _VIEW_DEFAULT)), args.crop) + + out_dir.mkdir(parents=True, exist_ok=True) + path = out_dir / f"{scene.name}_{name}.png" + Image.fromarray(img).save(path) + print(f" -> {path} ({img.shape[1]}x{img.shape[0]} px)") + + +def main() -> None: + ap = argparse.ArgumentParser( + description="vpp3d — Folien-Abbildungen der Kandidatenkonstruktion") + ap.add_argument("--scene", default="notched_box", choices=list(scene_mod.SCENES)) + ap.add_argument("--mesh", default=None, help="statt Szene: Mesh/Punktwolke") + ap.add_argument("--assume-convex", action="store_true") + ap.add_argument("--resolution", type=float, default=0.08, + help="Facetten-Kantenlaenge [m] (fein fuer Folienbilder)") + ap.add_argument("--baseline", type=float, default=None) + ap.add_argument("--config", default=None) + ap.add_argument("--only", type=int, nargs="+", choices=list(FIGURES), + default=None, help="nur diese Abbildungen rendern") + ap.add_argument("--width", type=int, default=3000) + ap.add_argument("--height", type=int, default=2250) + ap.add_argument("--crop", type=float, default=0.62, + help="mittiger Bildausschnitt (1.0 = gesamtes Bild)") + ap.add_argument("--out", default=None, help="Zielverzeichnis") + args = ap.parse_args() + + cfg = with_overrides(load_config(args.config), args) + scene = scene_from_args(args, cfg) + print(scene.summary()) + + st = Stages(scene, cfg) + print(st.summary()) + + out_dir = Path(args.out) if args.out else _OUT + for key in (args.only or sorted(FIGURES)): + render(key, scene, cfg, st, args, out_dir) + + +if __name__ == "__main__": + main() diff --git a/student_code/260722_UAVViewPlanning/vpp3d/stepviz.py b/student_code/260722_UAVViewPlanning/vpp3d/stepviz.py new file mode 100644 index 0000000..71a0c7c --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp3d/stepviz.py @@ -0,0 +1,209 @@ +from __future__ import annotations + +import argparse +import sys +from pathlib import Path + +import numpy as np + +try: + sys.stdout.reconfigure(encoding="utf-8") +except (AttributeError, ValueError): + pass + +from .config import load_config, with_overrides +from . import scene as scene_mod +from .scene import scene_from_args +from .candidates import sample_pose_regions, project_and_dedup +from .visibility import compute_visibility +from .setcover import greedy_set_cover +from .viz import _draw_mesh, _equal_3d + +_OUT = Path(__file__).with_name("output") + +C_NONCOVER = "0.55" +C_OPEN = "#e34a33" +C_DONE = "#31a354" +C_NEW = "#fd8d3c" +C_OVERLAP = "#3182bd" + + +class StepData: + def __init__(self, scene, poses, vis, cfg, trace): + self.scene = scene + self.poses = poses + self.vis = vis + self.cfg = cfg + self.trace = trace + self.coverable = vis.coverage_per_facet() > 0 + self.n_coverable = int(self.coverable.sum()) + self.n_steps = len(trace) + + +def build(args) -> StepData: + cfg = with_overrides(load_config(args.config), args) + scene = scene_from_args(args, cfg) + print(scene.summary()) + + raw = sample_pose_regions(scene, cfg) + poses, _ = project_and_dedup(raw, scene, cfg) + vis = compute_visibility(scene, poses, cfg) + + trace: list = [] + res = greedy_set_cover(vis.V, trace=trace) + print(res.summary()) + print(f" -> {len(trace)} Greedy-Schritte aufgezeichnet.") + return StepData(scene, poses, vis, cfg, trace) + + +def draw_step(ax, data: StepData, k: int) -> None: + from mpl_toolkits.mplot3d.art3d import Poly3DCollection + + elev, azim = ax.elev, ax.azim + ax.clear() + sc = data.scene + fc = sc.facets + _draw_mesh(ax, sc, Poly3DCollection) + + if k == 0: + covered_before = np.zeros(len(fc), dtype=bool) + new = np.empty(0, dtype=int) + overlap = np.empty(0, dtype=int) + pose = None + else: + st = data.trace[k - 1] + covered_before = st["covered_before"] + new = st["new_facets"] + overlap = st["overlap_facets"] + pose = st["pose"] + + open_mask = data.coverable & ~covered_before + if k > 0: + open_mask[new] = False + unreach = ~data.coverable + if unreach.any(): + ax.scatter(fc.centers[unreach, 0], fc.centers[unreach, 1], fc.centers[unreach, 2], + c=C_NONCOVER, s=8, label="unerreichbar") + if covered_before.any(): + ax.scatter(fc.centers[covered_before, 0], fc.centers[covered_before, 1], + fc.centers[covered_before, 2], c=C_DONE, s=10, label="abgedeckt") + if open_mask.any(): + ax.scatter(fc.centers[open_mask, 0], fc.centers[open_mask, 1], + fc.centers[open_mask, 2], c=C_OPEN, s=10, label="offen") + + if pose is not None: + p = data.poses.positions[pose] + d = data.poses.view_dirs[pose] + for f, col, lw, al in ([(f, C_OVERLAP, 0.5, 0.6) for f in overlap] + + [(f, C_NEW, 0.7, 0.8) for f in new]): + c = fc.centers[f] + ax.plot([p[0], c[0]], [p[1], c[1]], [p[2], c[2]], + color=col, lw=lw, alpha=al) + if len(overlap): + ax.scatter(fc.centers[overlap, 0], fc.centers[overlap, 1], fc.centers[overlap, 2], + c=C_OVERLAP, s=40, edgecolor="white", lw=0.4, label="Ueberlappung") + if len(new): + ax.scatter(fc.centers[new, 0], fc.centers[new, 1], fc.centers[new, 2], + c=C_NEW, s=40, edgecolor="white", lw=0.4, label="neu erfasst") + ax.scatter([p[0]], [p[1]], [p[2]], c="black", s=45, marker="o") + ax.quiver(p[0], p[1], p[2], d[0], d[1], d[2], + length=0.6 * data.cfg.d_opt, color="black", linewidth=1.5, + normalize=True) + + done = int(covered_before.sum()) + (len(new) if k > 0 else 0) + frac = done / max(1, data.n_coverable) + if k == 0: + title = (f"Schritt 0 / {data.n_steps} — Start: 0 Posen, " + f"0 / {data.n_coverable} erfasst (0 %)") + else: + title = (f"Schritt {k} / {data.n_steps} — Pose #{pose}\n" + f"+{len(new)} neu, {len(overlap)} Ueberlappung | " + f"{done} / {data.n_coverable} erfasst ({frac:.0%}), {k} Posen") + ax.set_title(title, fontsize=10) + ax.legend(loc="upper right", fontsize=7, framealpha=0.9) + _equal_3d(ax, sc) + ax.view_init(elev=elev, azim=azim) + + +def save_frames(data: StepData, outdir: Path) -> None: + import matplotlib + matplotlib.use("Agg") + import matplotlib.pyplot as plt + + outdir.mkdir(parents=True, exist_ok=True) + fig = plt.figure(figsize=(10, 9)) + ax = fig.add_subplot(111, projection="3d") + ax.view_init(elev=22, azim=-60) + for k in range(data.n_steps + 1): + draw_step(ax, data, k) + fig.tight_layout() + fig.savefig(outdir / f"step_{k:03d}.png", dpi=110) + plt.close(fig) + print(f" -> {data.n_steps + 1} Frames in {outdir}") + + +def interactive(data: StepData) -> None: + import matplotlib + try: + matplotlib.use("TkAgg") + except Exception: + pass + import matplotlib.pyplot as plt + + state = {"k": 0} + fig = plt.figure(figsize=(10, 9)) + ax = fig.add_subplot(111, projection="3d") + ax.view_init(elev=22, azim=-60) + + def redraw(): + draw_step(ax, data, state["k"]) + fig.canvas.draw_idle() + + def on_key(event): + if event.key in ("right", "n", " "): + state["k"] = min(state["k"] + 1, data.n_steps) + redraw() + elif event.key in ("left", "b"): + state["k"] = max(state["k"] - 1, 0) + redraw() + elif event.key == "home": + state["k"] = 0; redraw() + elif event.key == "end": + state["k"] = data.n_steps; redraw() + elif event.key == "s": + _OUT.mkdir(exist_ok=True) + f = _OUT / f"{data.scene.name}_step_{state['k']:03d}.png" + fig.savefig(f, dpi=120); print(f" gespeichert: {f}") + elif event.key == "q": + plt.close(fig) + + fig.canvas.mpl_connect("key_press_event", on_key) + print("Navigation: → / n = vor, ← / b = zurueck, Home/End, s = speichern, q = schliessen") + redraw() + plt.show() + + +def main() -> None: + ap = argparse.ArgumentParser(description="vpp3d — Schritt-fuer-Schritt-Debug-Viewer") + ap.add_argument("--scene", default="notched_box", choices=list(scene_mod.SCENES)) + ap.add_argument("--mesh", default=None, + help="statt Szene: reales Mesh/Punktwolke (STL/PLY)") + ap.add_argument("--assume-convex", action="store_true", + help="Normalen radial nach aussen orientieren (Punktwolken)") + ap.add_argument("--resolution", type=float, default=None, + help="Facetten-Aufloesung [m] ueberschreiben") + ap.add_argument("--baseline", type=float, default=None) + ap.add_argument("--config", default=None) + ap.add_argument("--show", action="store_true", + help="interaktiver Navigator statt PNG-Frames") + args = ap.parse_args() + + data = build(args) + if args.show: + interactive(data) + else: + save_frames(data, _OUT / f"steps_{data.scene.name}_greedy") + + +if __name__ == "__main__": + main() diff --git a/student_code/260722_UAVViewPlanning/vpp3d/tracking.py b/student_code/260722_UAVViewPlanning/vpp3d/tracking.py new file mode 100644 index 0000000..86c3a9f --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp3d/tracking.py @@ -0,0 +1,89 @@ +from __future__ import annotations + +from dataclasses import dataclass, field + +import numpy as np + +from .config import Config +from .scene import Scene +from .visibility import ray_clear + + +@dataclass +class TrackingPlan: + stations: np.ndarray + assignment: np.ndarray + pose_positions: np.ndarray + n_untrackable: int = 0 + extra: dict = field(default_factory=dict) + + def summary(self) -> str: + txt = ( + f"Tracking (LoS): {len(self.stations):,} Standorte " + f"(= {max(0, len(self.stations) - 1):,} Repositionierungen)" + ) + if self.n_untrackable: + txt += (f"\n WARNUNG: {self.n_untrackable:,} Posen nicht trackbar " + f"(ausserhalb Reichweite oder rundum verdeckt)!") + return txt + + +def plan_tracking(scene: Scene, positions: np.ndarray, cfg: Config, + ray_tolerance: float | None = None) -> TrackingPlan: + import open3d as o3d + + positions = np.asarray(positions, dtype=np.float64).reshape(-1, 3) + S = len(positions) + if S == 0: + return TrackingPlan(np.empty((0, 3)), np.empty(0, np.int64), + positions, 0) + + if ray_tolerance is None: + ray_tolerance = max(0.02, 0.5 * cfg.resolution) + + ground = float(scene.bounds[0][2]) + lo = positions[:, :2].min(axis=0) - cfg.track_margin + hi = positions[:, :2].max(axis=0) + cfg.track_margin + xs = np.arange(lo[0], hi[0] + cfg.track_grid_step, cfg.track_grid_step) + ys = np.arange(lo[1], hi[1] + cfg.track_grid_step, cfg.track_grid_step) + gx, gy = np.meshgrid(xs, ys) + cand = np.column_stack([gx.ravel(), gy.ravel(), np.full(gx.size, ground)]) + T = len(cand) + + d3 = np.linalg.norm(cand[:, None, :] - positions[None, :, :], axis=2) + within = (d3 >= cfg.track_range_min) & (d3 <= cfg.track_range_max) + + rscene = o3d.t.geometry.RaycastingScene() + rscene.add_triangles(o3d.t.geometry.TriangleMesh.from_legacy(scene.mesh)) + covers = np.zeros((T, S), dtype=bool) + ti, pj = np.nonzero(within) + if len(ti): + los = ray_clear(rscene, cand[ti], positions[pj], ray_tolerance) + covers[ti[los], pj[los]] = True + + trackable = covers.any(axis=0) + + deficit = trackable.copy() + chosen: list[int] = [] + while deficit.any(): + gain = covers[:, deficit].sum(axis=1) + t = int(np.argmax(gain)) + if gain[t] == 0: + break + chosen.append(t) + deficit &= ~covers[t] + + assignment = np.full(S, -1, dtype=np.int64) + if chosen: + d_masked = np.where(covers[chosen], d3[chosen], np.inf) + nearest = np.argmin(d_masked, axis=0) + ok = np.isfinite(d_masked[nearest, np.arange(S)]) + assignment[ok] = nearest[ok] + + return TrackingPlan( + stations=cand[chosen] if chosen else np.empty((0, 3)), + assignment=assignment, + pose_positions=positions, + n_untrackable=int((~trackable).sum()), + extra={"n_candidates": T}, + ) diff --git a/student_code/260722_UAVViewPlanning/vpp3d/usage.md b/student_code/260722_UAVViewPlanning/vpp3d/usage.md new file mode 100644 index 0000000..d683a7d --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp3d/usage.md @@ -0,0 +1,258 @@ +# vpp3d — Usage + +Alle Befehle werden aus dem Verzeichnis `Code/` ausgeführt. + +--- + +## Einstiegspunkte + +| Modul | Zweck | +|---|---| +| `vpp3d.run` | Kompletter Pipeline-Durchlauf [1]–[7], erzeugt PNG | +| `vpp3d.inspector` | Interaktiver 3D-Viewer (Open3D), Tastatur-Navigation | +| `vpp3d.stepviz` | Schritt-für-Schritt Greedy-Ablauf (matplotlib) | + +--- + +## `vpp3d.run` — Kompletter Durchlauf + +``` +python -m vpp3d.run [OPTIONEN] +``` + +### Parameter + +| Flag | Typ | Default | Beschreibung | +|---|---|---|---| +| `--scene` | `box` \| `notched_box` | `box` | Synthetische Testszene | +| `--mesh PATH` | Pfad | — | Reales Mesh/Punktwolke statt Szene (STL, PLY) | +| `--assume-convex` | Flag | aus | Normalen radial nach außen (Punktwolken ohne Normalen) | +| `--resolution M` | float | aus config.toml | Facetten-Kantenlänge [m] überschreiben | +| `--method` | `greedy` \| `ilp` \| `both` | `greedy` | Set-Cover-Solver | +| `--k K` | int | `1` | k-Coverage: jede Facette von ≥ K Posen sehen lassen (Registrierungs-Redundanz); Bedarf auf erreichbare Abdeckung gekappt | +| `--baseline B` | float | aus config.toml | Kamera-Projektor-Basislinie [m]; `0` = Einzelsicht-Ablation | +| `--time-limit T` | float | `60.0` | ILP-Zeitlimit [s] | +| `--no-overlap` | Flag | aus | Overlap-Reparatur überspringen | +| `--no-tracking` | Flag | aus | Tracking-Standort-Planung überspringen | +| `--config PATH` | Pfad | `vpp3d/config.toml` | Alternative Konfigurationsdatei | +| `--show` | Flag | aus | Nach dem Lauf Open3D-Ansicht öffnen | + +### Beispiele + +```powershell +# Standard: Box-Szene, Greedy +python -m vpp3d.run + +# Notched-Box mit Greedy + ILP-Vergleich +python -m vpp3d.run --scene notched_box --method both --time-limit 120 + +# Reales Mesh, nur Greedy, Open3D-Ansicht danach +python -m vpp3d.run --mesh Meshes/TestKorper1.stl --show + +# Punktwolke (EasyCube), konvexe Normalenorientierung +python -m vpp3d.run --mesh Meshes/EasyCube.ply --assume-convex + +# Ablation: Einzelsicht (kein Projektor-Kamera-Paar) +python -m vpp3d.run --baseline 0 --scene notched_box + +# Feinere Facetten, ILP, kein Tracking +python -m vpp3d.run --resolution 0.15 --method ilp --no-tracking +``` + +Ausgabe: `vpp3d/output/_.png` + +--- + +## `vpp3d.inspector` — Interaktiver 3D-Viewer + +``` +python -m vpp3d.inspector [OPTIONEN] +``` + +Führt dieselbe Pipeline wie `vpp3d.run` aus und öffnet anschließend einen +interaktiven Open3D-Viewer mit Tastatur-Navigation. + +### Parameter + +Identisch mit `vpp3d.run`, außer: kein `--no-tracking`, kein `--show`, kein +`--method both` (nur `greedy` oder `ilp`). + +| Flag | Typ | Default | Beschreibung | +|---|---|---|---| +| `--scene` | `box` \| `notched_box` | `box` | Synthetische Testszene | +| `--mesh PATH` | Pfad | — | Reales Mesh/Punktwolke (STL, PLY) | +| `--assume-convex` | Flag | aus | Normalen-Orientierung für Punktwolken | +| `--resolution M` | float | aus config.toml | Facetten-Kantenlänge [m] | +| `--method` | `greedy` \| `ilp` | `greedy` | Set-Cover-Solver | +| `--k K` | int | `1` | k-Coverage: jede Facette von ≥ K Posen sehen lassen (Bedarf gekappt) | +| `--baseline B` | float | aus config.toml | Basislinie [m]; `0` = Einzelsicht | +| `--time-limit T` | float | `60.0` | ILP-Zeitlimit [s] | +| `--no-overlap` | Flag | aus | Overlap-Reparatur überspringen | +| `--config PATH` | Pfad | `vpp3d/config.toml` | Alternative Konfigurationsdatei | + +### Tastaturkürzel im Viewer + +| Taste | Modus / Aktion | +|---|---| +| `O` / `3` | **Übersicht** — alle Plan-Posen + TSP-Route; grün=abgedeckt, rot=offen, grau=unerreichbar | +| `C` / `1` | **Kandidaten-Modus** — alle Kandidatenposen durchsteppen; hellgrün=von dieser Pose sichtbar | +| `P` / `2` | **Plan-Modus** — nur gewählte Plan-Posen durchsteppen; FoV-Frustum + Footprint | +| `M` / `4` | **Mosaik** — jede Pose in eigener Farbe, ihr Footprint entsprechend; gelb=Registrierungs-Überlappung (≥2 Scans) | +| `N` / `→` | Nächste Pose (Kandidaten- und Plan-Modus) | +| `B` / `←` | Vorherige Pose (Kandidaten- und Plan-Modus) | +| `S` | Screenshot nach `vpp3d/output/inspector___.png` | +| `Q` / `Esc` | Schließen | + +### Beispiele + +```powershell +# Standard: Box-Szene, Greedy +python -m vpp3d.inspector + +# Notched-Box mit ILP (längere Rechenzeit, weniger Posen) +python -m vpp3d.inspector --scene notched_box --method ilp --time-limit 120 + +# Reales Mesh +python -m vpp3d.inspector --mesh Meshes/TestKorper1.stl + +# Feinere Auflösung, ohne Overlap-Reparatur +python -m vpp3d.inspector --resolution 0.15 --no-overlap +``` + +--- + +## `vpp3d.stepviz` — Greedy-Ablauf Schritt für Schritt + +``` +python -m vpp3d.stepviz [OPTIONEN] +``` + +Zeigt den Greedy-Set-Cover-Aufbau Pick für Pick: neu abgedeckte Facetten +(orange), Überlappung mit bereits abgedeckten (blau), fertig abgedeckte (grün). + +### Parameter + +| Flag | Typ | Default | Beschreibung | +|---|---|---|---| +| `--scene` | `box` \| `notched_box` | `notched_box` | Synthetische Testszene | +| `--mesh PATH` | Pfad | — | Reales Mesh/Punktwolke (STL, PLY) | +| `--assume-convex` | Flag | aus | Normalen-Orientierung für Punktwolken | +| `--resolution M` | float | aus config.toml | Facetten-Kantenlänge [m] | +| `--baseline B` | float | aus config.toml | Basislinie [m] | +| `--config PATH` | Pfad | `vpp3d/config.toml` | Alternative Konfigurationsdatei | +| `--show` | Flag | aus | Interaktiver Navigator statt PNG-Export | + +### Beispiele + +```powershell +# PNGs je Greedy-Schritt nach vpp3d/output/steps_notched_box_greedy/ +python -m vpp3d.stepviz --scene notched_box + +# Interaktiv navigieren (Pfeiltasten, n/b, s=Screenshot, q=schließen) +python -m vpp3d.stepviz --scene notched_box --show + +# Reales Mesh, interaktiv +python -m vpp3d.stepviz --mesh Meshes/TestKorper1.stl --show +``` + +--- + +## `vpp3d.stepfigs` — Folienbilder der Kandidatenkonstruktion + +``` +python -m vpp3d.stepfigs [OPTIONEN] +``` + +Rendert je Konstruktions-/Verwurfsschritt ein PNG **ohne Beschriftung** nach +`vpp3d/output/praesentation/` — alle mit identischer Kamera, also als Folge +überblendbar: + +| # | Datei | Inhalt | +|---|---|---| +| 1 | `…_1_facetten.png` | Facetten-Mittelpunkte (Abtastziele) | +| 2 | `…_2_normalen.png` | Außennormale **jeder** Facette als Pfeil (orange) | +| 3 | `…_3_kegelsampling.png` | Inzidenzkegel einer Facette + Kandidatenposen (grün) | +| 4 | `…_4_dedup.png` | redundante Posen blass, Repräsentant je Rasterzelle grün | +| 5 | `…_5_abstand.png` | Sicherheits-/Bodenabstand: verworfen rot, zulässig grün | +| 6 | `…_6_dualsicht.png` | eine Pose: dual gesehen grün, nur vom Projektor rot | + +`--resolution` ist hier auf **0,08 m** vorbelegt (feine Facetten fürs Bild, nicht +aus `config.toml`). Punktgrößen und Pfeillängen skalieren mit; die Rohposen­wolken +(Bild 4/5) werden für die Darstellung ausgedünnt — die Zahlen auf der Konsole +bleiben vollständig. + +Weitere Parameter neben denen von `stepviz`: `--only N [N …]` (nur einzelne +Bilder), `--width/--height` (Renderauflösung), `--crop` (fester Mittenausschnitt, +`1.0` = ganzes Bild), `--out` (Zielverzeichnis). + +```powershell +python -m vpp3d.stepfigs # alle sechs, notched_box +python -m vpp3d.stepfigs --scene box --only 3 6 +``` + +--- + +## `vpp3d/config.toml` — Konfiguration + +Zentrale Parameterdatei. Alle Winkel in Grad, Distanzen in Metern. +Mit `--config PATH` kann eine alternative Datei übergeben werden. + +```toml +[working_distance] +d_min = 2.8 # Nah-Grenze des Arbeitsabstands [m] +d_max = 3.2 # Fern-Grenze [m] +# d_opt = 3.0 # Soll-Standoff; auskommentiert → Mittelwert + +[fov] +fov_h_deg = 40.0 # Horizontales Sichtfeld [°] +fov_v_deg = 30.0 # Vertikales Sichtfeld [°] + +[sensor] +baseline = 0.4 # Projektor-Kamera-Basislinie [m]; 0 = Einzelsicht + +[incidence] +theta_max_deg = 60.0 # Max. Einfallswinkel Sichtstrahl/Normale [°] + +[orientation] +pitch_min_deg = -10.0 # Gimbal-Untergrenze [°] +pitch_max_deg = 10.0 # Gimbal-Obergrenze [°] + +[drone] +safety_distance = 1.5 # Mindestabstand Drohne/Objekt [m] + +[sampling] +n_azimuth = 8 # Azimut-Richtungen je Kegel-Ring +cone_fractions = [0.0, 0.5, 0.85] # Kegel-Ringe als Bruchteil von theta_max +n_distance = 2 # Distanz-Stützstellen in [d_min, d_max] +yaw_bin_deg = 15.0 # Dedup-Raster Yaw [°] +pitch_bin_deg = 10.0 # Dedup-Raster Pitch [°] +resolution = 0.25 # Facetten-Soll-Kantenlänge [m] +# voxel = 0.45 # Dedup-Ortsraster [m]; auskommentiert → 0.15 * d_opt + +[registration] +min_overlap = 0.25 # Overlap-Schwelle: gemeinsame Fläche (kleinerer Scan) + +[tracking] +range_min = 1.0 # Min. Tracker-Drohne-Abstand [m] +range_max = 8.0 # Max. Tracker-Drohne-Abstand [m] +grid_step = 0.5 # Rasterauflösung für Tracking-Standorte [m] +margin = 2.0 # Raster-Rand über Posen-Hülle [m] +``` + +--- + +## Schnellreferenz + +```powershell +# Einfachster Start +python -m vpp3d.run + +# Greedy vs. ILP vergleichen +python -m vpp3d.run --method both --time-limit 60 + +# Interaktiv erkunden +python -m vpp3d.inspector --scene notched_box + +# Greedy-Ablauf animieren +python -m vpp3d.stepviz --show +``` diff --git a/student_code/260722_UAVViewPlanning/vpp3d/visibility.py b/student_code/260722_UAVViewPlanning/vpp3d/visibility.py new file mode 100644 index 0000000..35a2853 --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp3d/visibility.py @@ -0,0 +1,148 @@ +from __future__ import annotations + +import time +from dataclasses import dataclass, field + +import numpy as np +import scipy.sparse as sp +from scipy.spatial import cKDTree + +from .candidates import Poses +from .config import Config +from .scene import Scene + +_EPS = 1e-12 + + +@dataclass +class VisibilityMatrix: + V: sp.csr_matrix + stats: dict = field(default_factory=dict) + runtime: float = 0.0 + + @property + def shape(self) -> tuple[int, int]: + return self.V.shape + + def coverage_per_facet(self) -> np.ndarray: + return np.asarray(self.V.sum(axis=0)).ravel() + + def summary(self) -> str: + cov = self.coverage_per_facet() + reach = int((cov > 0).sum()) + n = self.V.shape[1] + lines = [ + f"Coverage-Matrix V: {self.V.shape[0]:,} Posen × {n:,} Facetten, " + f"{self.V.nnz:,} Sichtbarkeiten", + f" erreichbare Facetten: {reach}/{n} ({reach / max(1, n):.1%})", + f" Rechenzeit : {self.runtime:.2f} s", + ] + if self.stats: + lines.append(" Filterstufen : " + " -> ".join( + f"{k}={v:,}" for k, v in self.stats.items())) + return "\n".join(lines) + + +def ray_clear(rscene, origins: np.ndarray, targets: np.ndarray, + tol: float, batch: int = 1_000_000) -> np.ndarray: + import open3d as o3d + + d = targets - origins + dd = np.linalg.norm(d, axis=1) + dirs = d / np.maximum(dd[:, None], _EPS) + ok = np.empty(len(origins), dtype=bool) + for s in range(0, len(origins), batch): + e = min(s + batch, len(origins)) + rays = o3d.core.Tensor( + np.hstack([origins[s:e], dirs[s:e]]).astype(np.float32)) + t_hit = rscene.cast_rays(rays)["t_hit"].numpy().astype(np.float64) + ok[s:e] = t_hit >= dd[s:e] - tol + return ok + + +def _frames(poses: Poses) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + fwd = poses.view_dirs + right = np.cross(fwd, np.array([0.0, 0.0, 1.0])) + right /= np.maximum(np.linalg.norm(right, axis=1, keepdims=True), _EPS) + return fwd, right, np.cross(right, fwd) + + +def _in_frustum(diff: np.ndarray, fwd: np.ndarray, right: np.ndarray, + up: np.ndarray, tan_h: float, tan_v: float) -> np.ndarray: + x = np.einsum("ij,ij->i", diff, fwd) + y = np.einsum("ij,ij->i", diff, right) + z = np.einsum("ij,ij->i", diff, up) + return (x > 0.0) & (np.abs(y) <= x * tan_h) & (np.abs(z) <= x * tan_v) + + +def compute_visibility(scene: Scene, poses: Poses, cfg: Config, + ray_tolerance: float | None = None, + ray_batch: int = 1_000_000) -> VisibilityMatrix: + import open3d as o3d + + t0 = time.perf_counter() + C = scene.facets.centers + Nn = scene.facets.normals + M, N = len(poses), len(scene.facets) + if M == 0 or N == 0: + return VisibilityMatrix(sp.csr_matrix((M, N), dtype=bool), + runtime=time.perf_counter() - t0) + + if ray_tolerance is None: + ray_tolerance = max(0.02, 0.5 * cfg.resolution) + tan_h = np.tan(0.5 * cfg.fov_h_rad) + tan_v = np.tan(0.5 * cfg.fov_v_rad) + cos_theta = np.cos(cfg.theta_max_rad) + + fwd, right, up = _frames(poses) + p_proj = poses.positions + p_cam = p_proj + cfg.baseline * right + + neigh = cKDTree(C).query_ball_point(p_proj, r=cfg.d_max) + counts = np.fromiter((len(l) for l in neigh), dtype=np.int64, count=M) + pose_idx = np.repeat(np.arange(M, dtype=np.int64), counts) + patch_idx = (np.concatenate([np.asarray(l, np.int64) for l in neigh if l]) + if counts.sum() else np.empty(0, np.int64)) + stats = {"d_max": int(len(pose_idx))} + + diff = C[patch_idx] - p_proj[pose_idx] + dist = np.linalg.norm(diff, axis=1) + keep = dist >= cfg.d_min + pose_idx, patch_idx, diff, dist = pose_idx[keep], patch_idx[keep], diff[keep], dist[keep] + stats["d_min"] = int(len(pose_idx)) + + u = diff / np.maximum(dist[:, None], _EPS) + keep = -np.einsum("ij,ij->i", Nn[patch_idx], u) >= cos_theta + pose_idx, patch_idx, diff, dist = pose_idx[keep], patch_idx[keep], diff[keep], dist[keep] + stats["incidence"] = int(len(pose_idx)) + + keep = _in_frustum(diff, fwd[pose_idx], right[pose_idx], up[pose_idx], tan_h, tan_v) + pose_idx, patch_idx, diff, dist = pose_idx[keep], patch_idx[keep], diff[keep], dist[keep] + stats["frustum_proj"] = int(len(pose_idx)) + + diff_c = C[patch_idx] - p_cam[pose_idx] + keep = _in_frustum(diff_c, fwd[pose_idx], right[pose_idx], up[pose_idx], tan_h, tan_v) + pose_idx, patch_idx = pose_idx[keep], patch_idx[keep] + stats["frustum_cam"] = int(len(pose_idx)) + + rscene = o3d.t.geometry.RaycastingScene() + rscene.add_triangles(o3d.t.geometry.TriangleMesh.from_legacy(scene.mesh)) + + if len(pose_idx): + clear_p = ray_clear(rscene, p_proj[pose_idx], C[patch_idx], + ray_tolerance, ray_batch) + pose_idx, patch_idx = pose_idx[clear_p], patch_idx[clear_p] + stats["occlusion_proj"] = int(len(pose_idx)) + if cfg.baseline > 0 and len(pose_idx): + clear_c = ray_clear(rscene, p_cam[pose_idx], C[patch_idx], + ray_tolerance, ray_batch) + pose_idx, patch_idx = pose_idx[clear_c], patch_idx[clear_c] + stats["dual"] = int(len(pose_idx)) + else: + stats["occlusion_proj"] = 0 + stats["dual"] = 0 + + V = sp.csr_matrix( + (np.ones(len(pose_idx), dtype=bool), (pose_idx, patch_idx)), + shape=(M, N)) + return VisibilityMatrix(V=V, stats=stats, runtime=time.perf_counter() - t0) diff --git a/student_code/260722_UAVViewPlanning/vpp3d/viz.py b/student_code/260722_UAVViewPlanning/vpp3d/viz.py new file mode 100644 index 0000000..848eda8 --- /dev/null +++ b/student_code/260722_UAVViewPlanning/vpp3d/viz.py @@ -0,0 +1,188 @@ +from __future__ import annotations + +import numpy as np + +C_COVERED = (0.19, 0.64, 0.33) +C_OPEN = (0.89, 0.29, 0.20) +C_UNREACH = (0.55, 0.55, 0.55) +C_OVERLAP = (1.0, 0.70, 0.0) +C_STATION = (0.10, 0.45, 0.90) + + +def _selected_ids(cover, selected): + return cover.poses if selected is None else np.asarray(selected, np.int64) + + +def _facet_status(scene, vis, cover, selected=None): + n = len(scene.facets) + coverable = vis.coverage_per_facet() > 0 + sel_rows = _selected_ids(cover, selected) + if len(sel_rows): + covered = np.asarray(vis.V.tocsr()[sel_rows].sum(axis=0)).ravel() > 0 + else: + covered = np.zeros(n, dtype=bool) + return covered, coverable & ~covered, ~coverable + + +def plot_overview_png(scene, poses, vis, cover, route, cfg, path: str, + show: bool = False, selected=None, + overlap=None, tracking=None) -> str: + import matplotlib + matplotlib.use("TkAgg" if show else "Agg") + import matplotlib.pyplot as plt + from mpl_toolkits.mplot3d.art3d import Poly3DCollection + + fc = scene.facets + sel_rows = _selected_ids(cover, selected) + covered, open_, unreach = _facet_status(scene, vis, cover, selected) + + overlap_mask = np.zeros(len(fc), dtype=bool) + if overlap is not None: + overlap_mask[overlap.overlap_facets] = True + covered = covered & ~overlap_mask + + fig = plt.figure(figsize=(15, 7)) + + ax1 = fig.add_subplot(1, 2, 1, projection="3d") + _draw_mesh(ax1, scene, Poly3DCollection) + pp = poses.positions + ax1.scatter(pp[:, 0], pp[:, 1], pp[:, 2], c="tab:green", s=4, alpha=0.4) + ax1.set_title(f"[2/3] Kandidatenposen nach Projektion+Dedup ({len(poses)})") + _equal_3d(ax1, scene) + + ax2 = fig.add_subplot(1, 2, 2, projection="3d") + _draw_mesh(ax2, scene, Poly3DCollection) + for mask, col, lbl in ((covered, C_COVERED, "abgedeckt"), + (overlap_mask, C_OVERLAP, "Registrierungs-Ueberlappung"), + (open_, C_OPEN, "offen"), + (unreach, C_UNREACH, "unerreichbar")): + if mask.any(): + ax2.scatter(fc.centers[mask, 0], fc.centers[mask, 1], fc.centers[mask, 2], + c=[col], s=8, label=lbl) + sp = poses.positions[sel_rows] + sd = poses.view_dirs[sel_rows] + if len(sp): + ax2.scatter(sp[:, 0], sp[:, 1], sp[:, 2], c="black", s=30, marker="o") + ax2.quiver(sp[:, 0], sp[:, 1], sp[:, 2], sd[:, 0], sd[:, 1], sd[:, 2], + length=0.6 * cfg.d_opt, color="black", linewidth=1.0, + normalize=True) + if len(route.order): + rp = sp[route.order] + ax2.plot(rp[:, 0], rp[:, 1], rp[:, 2], "-", color="tab:orange", lw=1.0) + + if tracking is not None and len(tracking.stations): + st = tracking.stations + ax2.scatter(st[:, 0], st[:, 1], st[:, 2], c=[C_STATION], s=70, + marker="^", edgecolor="black", label="Tracking-Standort") + for k, a in enumerate(tracking.assignment): + if a < 0: + continue + o, t = sp[k], st[a] + ax2.plot([o[0], t[0]], [o[1], t[1]], [o[2], t[2]], + "-", color=C_STATION, lw=0.4, alpha=0.5) + + n_add = 0 if overlap is None else len(overlap.added) + ax2.set_title(f"[5/6/7] {cover.method}: {len(sel_rows)} Posen " + f"(+{n_add} Bruecken), Weg {route.length:.0f} m") + ax2.legend(loc="upper right", fontsize=8) + _equal_3d(ax2, scene) + + fig.suptitle(f"vpp3d — {scene.name} — {cover.method}", fontsize=14) + fig.tight_layout(rect=(0, 0, 1, 0.97)) + fig.savefig(path, dpi=120) + if show: + plt.show() + plt.close(fig) + return path + + +def _draw_mesh(ax, scene, Poly3DCollection) -> None: + coll = Poly3DCollection(scene.vertices[scene.triangles], alpha=0.12, + facecolor="0.6", edgecolor="0.4", linewidths=0.2) + ax.add_collection3d(coll) + ax.set_xlabel("x [m]"); ax.set_ylabel("y [m]"); ax.set_zlabel("z [m]") + + +def _equal_3d(ax, scene) -> None: + lo, hi = scene.bounds + c = 0.5 * (lo + hi) + r = 0.5 * float((hi - lo).max()) + 1.0 + ax.set_xlim(c[0] - r, c[0] + r) + ax.set_ylim(c[1] - r, c[1] + r) + ax.set_zlim(c[2] - r, c[2] + r) + try: + ax.set_box_aspect((1, 1, 1)) + except Exception: + pass + + +def show_open3d(scene, poses, vis, cover, route, cfg, selected=None, + overlap=None, tracking=None) -> None: + import open3d as o3d + + def lineset(pts, idx, color): + ls = o3d.geometry.LineSet( + o3d.utility.Vector3dVector(np.asarray(pts, dtype=float)), + o3d.utility.Vector2iVector(np.asarray(idx))) + ls.paint_uniform_color(list(color)) + return ls + + geoms = [] + mesh = o3d.geometry.TriangleMesh(scene.mesh) + mesh.paint_uniform_color([0.8, 0.8, 0.82]) + mesh.compute_vertex_normals() + geoms.append(mesh) + + sel_rows = _selected_ids(cover, selected) + covered, open_, unreach = _facet_status(scene, vis, cover, selected) + fc = scene.facets + colors = np.empty((len(fc), 3)) + colors[covered] = C_COVERED + colors[open_] = C_OPEN + colors[unreach] = C_UNREACH + if overlap is not None: + colors[overlap.overlap_facets] = C_OVERLAP + pc = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(fc.centers)) + pc.colors = o3d.utility.Vector3dVector(colors) + geoms.append(pc) + + sp = poses.positions[sel_rows] + sd = poses.view_dirs[sel_rows] + r = max(0.05, 0.04 * cfg.d_opt) + line_pts, line_idx = [], [] + for k, (p, d) in enumerate(zip(sp, sd)): + ball = o3d.geometry.TriangleMesh.create_sphere(radius=r) + ball.translate(p) + ball.paint_uniform_color([0.0, 0.0, 0.0]) + ball.compute_vertex_normals() + geoms.append(ball) + line_pts += [p, p + d * 0.6 * cfg.d_opt] + line_idx.append([2 * k, 2 * k + 1]) + if line_pts: + geoms.append(lineset(line_pts, line_idx, (0.1, 0.1, 0.1))) + + if len(route.order) > 1: + rp = sp[route.order] + geoms.append(lineset(rp, [[i, i + 1] for i in range(len(rp) - 1)], + (1.0, 0.55, 0.0))) + + if tracking is not None and len(tracking.stations): + st = tracking.stations + box = max(0.1, 0.06 * cfg.d_opt) + for a_st in st: + cube = o3d.geometry.TriangleMesh.create_box(box, box, box) + cube.translate(a_st - box / 2.0) + cube.paint_uniform_color(list(C_STATION)) + cube.compute_vertex_normals() + geoms.append(cube) + los_pts, los_idx = [], [] + for k, a in enumerate(tracking.assignment): + if a < 0: + continue + los_pts += [sp[k], st[a]] + los_idx.append([len(los_pts) - 2, len(los_pts) - 1]) + if los_pts: + geoms.append(lineset(los_pts, los_idx, C_STATION)) + + o3d.visualization.draw_geometries( + geoms, window_name=f"vpp3d — {scene.name} — {cover.method}")