Claude: über nacht arbeiten. Pipeline verbessern
This commit is contained in:
chk
500
doc/pipeline.tex
Normal file
500
doc/pipeline.tex
Normal file
@@ -0,0 +1,500 @@
|
||||
\documentclass[11pt,a4paper]{article}
|
||||
|
||||
\usepackage[utf8]{inputenc}
|
||||
\usepackage[T1]{fontenc}
|
||||
\usepackage[ngerman]{babel}
|
||||
\usepackage[margin=2.4cm]{geometry}
|
||||
\usepackage{amsmath,amssymb,bm,mathtools}
|
||||
\usepackage{booktabs}
|
||||
\usepackage{xcolor}
|
||||
\usepackage{listings}
|
||||
\usepackage{graphicx}
|
||||
\usepackage{enumitem}
|
||||
\usepackage[hidelinks]{hyperref}
|
||||
|
||||
\definecolor{codebg}{rgb}{0.96,0.96,0.97}
|
||||
\definecolor{kw}{rgb}{0.2,0.3,0.7}
|
||||
\lstset{
|
||||
basicstyle=\ttfamily\small,
|
||||
backgroundcolor=\color{codebg},
|
||||
frame=single, rulecolor=\color{gray!40},
|
||||
breaklines=true, columns=fullflexible,
|
||||
keepspaces=true, showstringspaces=false,
|
||||
numberstyle=\tiny, xleftmargin=4pt, xrightmargin=4pt, aboveskip=6pt, belowskip=6pt,
|
||||
}
|
||||
|
||||
\newcommand{\R}{\mathbb{R}}
|
||||
\newcommand{\vx}{\mathbf{x}}
|
||||
\newcommand{\vX}{\mathbf{X}}
|
||||
\newcommand{\vu}{\mathbf{u}}
|
||||
\newcommand{\vn}{\mathbf{n}}
|
||||
\newcommand{\vp}{\mathbf{p}}
|
||||
\newcommand{\vt}{\mathbf{t}}
|
||||
\newcommand{\vq}{\mathbf{q}}
|
||||
\newcommand{\va}{\hat{\mathbf{a}}}
|
||||
\newcommand{\vY}{\mathbf{Y}}
|
||||
\newcommand{\vv}{\mathbf{v}}
|
||||
\newcommand{\vr}{\mathbf{r}}
|
||||
\newcommand{\vd}{\mathbf{d}}
|
||||
\newcommand{\norm}[1]{\left\lVert #1 \right\rVert}
|
||||
|
||||
\title{\textbf{Roboter-Pose-Erkennung aus Mehrkamera-ArUco-Aufnahmen}\\[2pt]
|
||||
\large Pipeline, mathematische Grundlagen und Konfiguration}
|
||||
\author{appRobotRendering --- technische Dokumentation}
|
||||
\date{\today}
|
||||
|
||||
\begin{document}
|
||||
\maketitle
|
||||
|
||||
\begin{abstract}
|
||||
\noindent
|
||||
Diese Dokumentation beschreibt eine Bildverarbeitungs-Pipeline, die aus
|
||||
synchronen Fotos eines Roboterarms von mehreren kalibrierten Kameras die
|
||||
Gelenkstellung (sieben Achsen $x,y,z,a,b,c,e$) rekonstruiert. Auf dem Roboter
|
||||
und einer Referenzplatte (\emph{Board}) sind ArUco-Marker angebracht. Aus den
|
||||
\emph{vier Eckpunkten} jedes Markers wird per Mehrsicht-Triangulation eine volle
|
||||
6-DOF-Markerpose (Position \emph{und} Flächennormale) bestimmt; aus diesen
|
||||
Beobachtungen werden über das Vorwärtskinematik-Modell die Gelenkwinkel
|
||||
geschätzt. Es werden vier austauschbare Schätzverfahren beschrieben und an
|
||||
simulierten Szenen mit bekannter Grundwahrheit validiert (mittlerer
|
||||
Winkelfehler $0{,}25^\circ$, Positionsfehler $0{,}10$\,mm).
|
||||
\end{abstract}
|
||||
|
||||
\tableofcontents
|
||||
\bigskip
|
||||
|
||||
% =====================================================================
|
||||
\section{Einführung und Problemstellung}
|
||||
% =====================================================================
|
||||
Gegeben sind $C$ kalibrierte Kameras, die denselben Roboter gleichzeitig
|
||||
fotografieren. Der Roboter trägt eine kinematische Kette starrer Glieder
|
||||
(\emph{Links}); auf den Gliedern und auf der fixen Bodenplatte sitzen ArUco-Marker
|
||||
bekannter Größe. Das \emph{Board} ist die Wurzel der Kette und definiert das
|
||||
Weltkoordinatensystem $W$. Gesucht ist der Gelenkvektor
|
||||
\[
|
||||
\vq = (x, y, z, a, b, c, e) \in \R^7,
|
||||
\]
|
||||
mit linearen Achsen $x,e$ (in mm) und rotatorischen Achsen $y,z,a,b,c$ (in Grad).
|
||||
|
||||
Die zentrale Idee ist, dass ein ArUco-Marker \emph{kein Punkt} ist, sondern ein
|
||||
ebenes Quadrat bekannter Kantenlänge mit vier detektierten Eckpunkten. Werden
|
||||
diese vier Ecken über mehrere Kameras trianguliert, liefert jeder einzelne
|
||||
Marker neben seiner Position auch eine \emph{gemessene} Orientierung. Dadurch
|
||||
genügt bereits ein einziger sichtbarer Marker, um den Winkel des Gliedes zu
|
||||
bestimmen, auf dem er sitzt.
|
||||
|
||||
% =====================================================================
|
||||
\section{Notation und Koordinatensysteme}
|
||||
% =====================================================================
|
||||
Punkte im Weltsystem $W$ werden mit $\vX\in\R^3$ bezeichnet (Einheit mm).
|
||||
Eine Kamera $c$ besitzt die Intrinsikmatrix $K_c\in\R^{3\times3}$, die
|
||||
Verzeichnungskoeffizienten $D_c$ sowie die Extrinsik $(R_c, \vt_c)$ in
|
||||
\emph{world-to-camera}-Konvention,
|
||||
\[
|
||||
\vX^{\mathrm{cam}} = R_c\,\vX + \vt_c , \qquad R_c\in SO(3).
|
||||
\]
|
||||
Das Kamerazentrum im Weltsystem ist $\mathbf{C}_c = -R_c^{\top}\vt_c$. Die
|
||||
normierte (intrinsikfreie) Projektion eines Weltpunkts lautet
|
||||
\begin{equation}
|
||||
\tilde{\pi}(\vX;R_c,\vt_c)=
|
||||
\frac{1}{[\vX^{\mathrm{cam}}]_3}\begin{pmatrix}[\vX^{\mathrm{cam}}]_1\\[2pt][\vX^{\mathrm{cam}}]_2\end{pmatrix}\in\R^2 .
|
||||
\label{eq:proj}
|
||||
\end{equation}
|
||||
Eine beobachtete Pixelkoordinate $\vu$ wird mittels $\tilde{\vu}=\mathrm{undistort}(\vu;K_c,D_c)$
|
||||
in dieselbe normierte Ebene überführt. Rotationen werden bei der Optimierung über
|
||||
den Rodrigues-Vektor $\bm{\omega}\in\R^3$ mit $R=\exp([\bm{\omega}]_\times)$
|
||||
parametrisiert.
|
||||
|
||||
% =====================================================================
|
||||
\section{Pipeline-Architektur}
|
||||
% =====================================================================
|
||||
Die Verarbeitung erfolgt in vier Stufen (Skript \texttt{run/run\_pipeline.bat}):
|
||||
|
||||
\begin{center}
|
||||
\begin{tabular}{@{}lll@{}}
|
||||
\toprule
|
||||
Stufe & Skript & Ausgabe \\
|
||||
\midrule
|
||||
1 & \texttt{1\_detect\_aruco\_observations.py} & \texttt{*\_aruco\_detection.json} (Ecken) \\
|
||||
2 & \texttt{2\_estimate\_camera\_from\_observations.py} & \texttt{*\_camera\_pose.json} \\
|
||||
3 & \texttt{3\_multiview\_bundle\_adjustment\_v4.py} & \texttt{aruco\_positions\_initial.json} \\
|
||||
3b & \texttt{3b\_corner\_marker\_poses.py} & \texttt{aruco\_marker\_poses.json} \\
|
||||
4 & \texttt{pose\_estimation.py} & \texttt{robot\_state.json} \\
|
||||
\bottomrule
|
||||
\end{tabular}
|
||||
\end{center}
|
||||
|
||||
Stufe 3 (reine Center-Triangulation) wird für Visualisierung und Vergleich
|
||||
beibehalten; die eigentliche Posenschätzung nutzt die in Stufe 3b erzeugten
|
||||
Eckposen.
|
||||
|
||||
% =====================================================================
|
||||
\section{Stufe 1: ArUco-Detektion}
|
||||
% =====================================================================
|
||||
Pro Bild liefert der OpenCV-ArUco-Detektor für jeden erkannten Marker $m$ die
|
||||
vier Eckpunkte $\vu_{m,0},\dots,\vu_{m,3}$ im Uhrzeigersinn sowie deren
|
||||
Mittelpunkt. Zusätzlich werden Qualitätsmaße (Fläche, Schärfe als
|
||||
Laplace-Varianz, Kontrast, Randabstand, Kantenverhältnis) berechnet und eine
|
||||
heuristische Konfidenz abgeleitet. Die Eckpunkte $\vu_{m,k}$ sind die
|
||||
\emph{primären} Messungen; der Mittelpunkt ist nur ihr Mittelwert.
|
||||
|
||||
% =====================================================================
|
||||
\section{Stufe 2: Kamera-Pose-Schätzung}
|
||||
% =====================================================================
|
||||
Die Board-Marker haben bekannte Weltpositionen $\vX_i$ (aus \texttt{robot.json}).
|
||||
Aus ihren beobachteten Mittelpunkten wird die Pose jeder Kamera geschätzt.
|
||||
Ein robuster Anfangswert ergibt sich aus \texttt{solvePnP} (IPPE-Square) je
|
||||
Marker, gefolgt von einem starren Ausgleich der per-Marker-Kamerazentren.
|
||||
Verfeinert wird mit Levenberg--Marquardt auf den normierten
|
||||
Reprojektionsresiduen:
|
||||
\begin{equation}
|
||||
\min_{\bm{\theta}}\ \sum_{i} \norm{\,\tilde{\vu}_i - \tilde{\pi}\!\big(\vX_i;\,R(\bm{\omega}),\vt\big)}^2,
|
||||
\qquad \bm{\theta}=(\bm{\omega},\vt)\in\R^6 .
|
||||
\label{eq:campose}
|
||||
\end{equation}
|
||||
Aus der Jacobi-Matrix am Optimum wird eine $6\times6$-Kovarianz und daraus die
|
||||
Unsicherheit von Kamerazentrum und Orientierung propagiert. Bemerkung: Da die
|
||||
Board-Marker nahezu koplanar sind ($z\approx0$), ist die Tiefe schlecht
|
||||
konditioniert --- eine korrekte Intrinsik ist deshalb essenziell (siehe
|
||||
Abschnitt~\ref{sec:limits}).
|
||||
|
||||
% =====================================================================
|
||||
\section{Stufe 3b: Eck-Triangulation und Normalenschätzung}
|
||||
% =====================================================================
|
||||
\subsection{Mehrsicht-Triangulation (DLT)}
|
||||
Sei $V_{m,k}\subseteq\{1,\dots,C\}$ die Menge der Kameras, die Ecke $k$ von
|
||||
Marker $m$ sehen. Mit der normierten Projektionsmatrix $P_c=[\,R_c \mid \vt_c\,]$
|
||||
(Zeilen $\vp_{c,1}^\top,\vp_{c,2}^\top,\vp_{c,3}^\top$) und der entzerrten
|
||||
Beobachtung $\tilde{\vu}=(x,y)$ liefert jede Kamera zwei lineare Gleichungen in
|
||||
den homogenen Punktkoordinaten $\vX_h$:
|
||||
\begin{equation}
|
||||
x\,\vp_{c,3}^\top \vX_h - \vp_{c,1}^\top \vX_h = 0,\qquad
|
||||
y\,\vp_{c,3}^\top \vX_h - \vp_{c,2}^\top \vX_h = 0 .
|
||||
\end{equation}
|
||||
Das Stapeln über alle $c\in V_{m,k}$ ergibt $A\,\vX_h=\mathbf 0$; die Lösung ist
|
||||
der rechte Singulärvektor zum kleinsten Singulärwert von $A$, gefolgt von der
|
||||
Dehomogenisierung $\vX = \vX_h^{1:3}/\vX_h^{4}$. Dies wird für alle vier Ecken
|
||||
ausgeführt und liefert $\vY_0,\dots,\vY_3$.
|
||||
|
||||
\subsection{Position und Normale}
|
||||
Die Markerposition ist der Schwerpunkt $\bar{\vY}=\tfrac14\sum_k\vY_k$. Die
|
||||
Normale ist der Normalenvektor der Ausgleichsebene durch die vier Ecken: mit
|
||||
$M=[\vY_0-\bar{\vY};\dots;\vY_3-\bar{\vY}]\in\R^{4\times3}$ und der
|
||||
Singulärwertzerlegung $M=U\Sigma V^\top$ ist
|
||||
\begin{equation}
|
||||
\vn = \pm\,\vv_3 \quad(\text{kleinster Singulärvektor}),
|
||||
\end{equation}
|
||||
wobei das Vorzeichen über die ArUco-Eckreihenfolge so gewählt wird, dass $\vn$
|
||||
zur Kamera weist (Konvention wie in der Grundwahrheit):
|
||||
$\vn\leftarrow-\vn$, falls $\vn\cdot\big((\vY_1-\vY_0)\times(\vY_2-\vY_0)\big)>0$.
|
||||
Die Validierung (\texttt{benchmark/stage0\_corner\_normals.py}) ergibt einen
|
||||
Median-Normalenfehler von $1{,}2^\circ$ ($p_{90}=3{,}0^\circ$); die
|
||||
Aufschlüsselung je Glied (Tabelle~\ref{tab:normals}) zeigt, dass selbst die
|
||||
kleinen, weit entfernten Fingermarker eine Orientierung von rund $1^\circ$
|
||||
liefern --- die Grundlage dafür, dass ein einzelner Marker seinen Gelenkwinkel
|
||||
bestimmen kann.
|
||||
|
||||
\begin{table}[h]
|
||||
\centering
|
||||
\begin{tabular}{@{}lccc@{}}
|
||||
\toprule
|
||||
Glied & \# Marker & Normalenfehler Mittel [$^\circ$] & Max [$^\circ$] \\
|
||||
\midrule
|
||||
Arm1 & 3 & 0{,}64 & 0{,}94 \\
|
||||
Arm2 & 4 & 0{,}78 & 1{,}04 \\
|
||||
Ellbow & 4 & 0{,}96 & 1{,}99 \\
|
||||
FingerA & 2 & 0{,}96 & 1{,}14 \\
|
||||
FingerB & 2 & 1{,}43 & 1{,}44 \\
|
||||
Board & 41 & 1{,}79 & 7{,}07 \\
|
||||
\bottomrule
|
||||
\end{tabular}
|
||||
\caption{Genauigkeit der aus Ecken abgeleiteten Normale je Glied (Scene8).}
|
||||
\label{tab:normals}
|
||||
\end{table}
|
||||
|
||||
% =====================================================================
|
||||
\section{Vorwärtskinematik}
|
||||
% =====================================================================
|
||||
Die Links werden topologisch (Wurzel zuerst) sortiert. Für Link $l$ mit Eltern-%
|
||||
link $p$ gilt
|
||||
\begin{equation}
|
||||
T_l \;=\; T_p \cdot \underbrace{\mathrm{Tr}(\mathbf{m}_l)\,R_{xyz}(\bm{\rho}_l)}_{\text{Mount}}
|
||||
\cdot \underbrace{\mathrm{Tr}(\mathbf{o}_l)\,R_{xyz}(\bm{\sigma}_l)}_{\text{Gelenkursprung}}
|
||||
\cdot Q_l(q_l),
|
||||
\end{equation}
|
||||
mit der Gelenkbewegung
|
||||
\begin{equation}
|
||||
Q_l(q_l)=
|
||||
\begin{cases}
|
||||
R(\hat{\mathbf{s}}_l,\,q_l) & \text{revolut (Achse }\hat{\mathbf{s}}_l,\text{ Winkel }q_l),\\
|
||||
\mathrm{Tr}(\hat{\mathbf{s}}_l\, q_l) & \text{linear (Verschiebung }q_l),\\
|
||||
I & \text{fest.}
|
||||
\end{cases}
|
||||
\end{equation}
|
||||
Ein Marker mit lokaler Position $\vp_m^{\text{lok}}$ und lokaler Normale
|
||||
$\vn_m^{\text{lok}}$ erscheint im Weltsystem als
|
||||
\begin{equation}
|
||||
\vp_m(\vq)=T_{l(m)}(\vq)\begin{psmallmatrix}\vp_m^{\text{lok}}\\1\end{psmallmatrix},
|
||||
\qquad
|
||||
\vn_m(\vq)=R\big(T_{l(m)}(\vq)\big)\,\vn_m^{\text{lok}} .
|
||||
\label{eq:fkmarker}
|
||||
\end{equation}
|
||||
Die Kette dieses Roboters ist in Tabelle~\ref{tab:chain} zusammengefasst.
|
||||
Bemerkenswert: \emph{Base}, \emph{Hand} und \emph{Palm} tragen keine eigenen
|
||||
Marker, und beide Finger teilen sich die Variable $e$ (mit entgegengesetzten
|
||||
Achsen --- ein Greifer).
|
||||
|
||||
\begin{table}[h]
|
||||
\centering
|
||||
\begin{tabular}{@{}llllc@{}}
|
||||
\toprule
|
||||
Link & Eltern & Gelenktyp & Variable & Achse \\
|
||||
\midrule
|
||||
Board & --- & (Wurzel) & --- & --- \\
|
||||
Base & Board & linear & $x$ & $[1,0,0]$ \\
|
||||
Arm1 & Base & revolut & $y$ & $[-1,0,0]$ \\
|
||||
Ellbow & Arm1 & revolut & $z$ & $[-1,0,0]$ \\
|
||||
Arm2 & Ellbow & revolut & $a$ & $[0,-1,0]$ \\
|
||||
Hand & Arm2 & revolut & $b$ & $[1,0,0]$ \\
|
||||
Palm & Hand & revolut & $c$ & $[0,-1,0]$ \\
|
||||
FingerA & Palm & linear & $e$ & $[1,0,0]$ \\
|
||||
FingerB & Palm & linear & $e$ & $[-1,0,0]$ \\
|
||||
\bottomrule
|
||||
\end{tabular}
|
||||
\caption{Kinematische Kette: sieben Freiheitsgrade, davon zwei linear ($x,e$).}
|
||||
\label{tab:chain}
|
||||
\end{table}
|
||||
|
||||
% =====================================================================
|
||||
\section{Pose-Estimation: Vier Schätzverfahren}
|
||||
% =====================================================================
|
||||
Die Beobachtungen sind die Eckposen $\{(\vY_m, \vn^{o}_m)\}_{m\in\mathcal M}$
|
||||
aus Stufe 3b. Alle Verfahren parametrisieren über die \emph{Gelenkvariablen}
|
||||
$\vq$ (nicht über Links), wodurch geteilte Variablen ($e$) und markerlose
|
||||
Glieder ($x,b,c$) generisch behandelt werden. Das gemeinsame
|
||||
Residuum eines Markers kombiniert Position und Normale,
|
||||
\begin{equation}
|
||||
\vr_m(\vq)=\Big[\;\vp_m(\vq)-\vY_m\,;\;\; w_n\big(\vn_m(\vq)-\vn^{o}_m\big)\Big]\in\R^6,
|
||||
\label{eq:res}
|
||||
\end{equation}
|
||||
mit der Normalengewichtung $w_n$ (Parameter \texttt{normal\_weight}).
|
||||
|
||||
\subsection{Blockstruktur der Kette}
|
||||
Für die sequenziellen Verfahren wird die Kette in Blöcke zerlegt: Von der Wurzel
|
||||
fortschreitend werden die Variablen markerloser Gelenke gesammelt und mit der
|
||||
Variable des nächsten markertragenden Gelenks zu einem Block zusammengefasst,
|
||||
der aus den Markern dieses Gliedes (zuzüglich Geschwistern mit geteilter
|
||||
Variable) geschätzt wird. Für diesen Roboter ergeben sich
|
||||
\[
|
||||
B_1=\{x,y\}\ \text{(Arm1)},\quad B_2=\{z\}\ \text{(Ellbow)},\quad
|
||||
B_3=\{a\}\ \text{(Arm2)},\quad B_4=\{b,c,e\}\ \text{(Finger)} .
|
||||
\]
|
||||
|
||||
\subsection{Verfahren A --- \texttt{sequential\_vector}}
|
||||
Analytische Winkelbestimmung pro rotatorischem Gelenk mit $\ge2$ sichtbaren
|
||||
Markern auf demselben Glied. Mit der Weltachse $\va$ des Gelenks (aus der FK bei
|
||||
$q=0$) bildet man für Markerpaare $(i,j)$ den Modellvektor
|
||||
$\vd^{\text{mod}}_{ij}=\vp_j(q{=}0)-\vp_i(q{=}0)$ (im Weltsystem, da der lokale
|
||||
Linkframe durch die Elterngelenke bereits rotiert ist) und den Beobachtungs-%
|
||||
vektor $\vd^{\text{obs}}_{ij}=\vY_j-\vY_i$. Nach Projektion senkrecht zur Achse,
|
||||
$\vd^{\perp}=\vd-(\vd\cdot\va)\va$, ist der vorzeichenbehaftete Drehwinkel
|
||||
\begin{equation}
|
||||
\varphi_{ij}=\operatorname{atan2}\!\Big(\va\cdot(\vd^{\text{mod},\perp}_{ij}\times \vd^{\text{obs},\perp}_{ij}),\;
|
||||
\vd^{\text{mod},\perp}_{ij}\cdot \vd^{\text{obs},\perp}_{ij}\Big).
|
||||
\end{equation}
|
||||
Der Gelenkwinkel ist das mit $w_{ij}=\norm{\vd^{\text{mod},\perp}_{ij}}\,\norm{\vd^{\text{obs},\perp}_{ij}}$
|
||||
gewichtete \emph{zirkuläre} Mittel
|
||||
$q^*=\operatorname{atan2}(\sum w\sin\varphi,\ \sum w\cos\varphi)$.
|
||||
Lineare, markerlose oder Einzelmarker-Gelenke werden über den Block-FK-Löser
|
||||
(Verfahren B) ergänzt. Sehr schnell, keine lokalen Minima, benötigt aber
|
||||
$\ge2$ Marker pro Gelenk.
|
||||
|
||||
\subsection{Verfahren B --- \texttt{sequential\_fk}}
|
||||
Blockweiser nichtlinearer Ausgleich entlang der Kette. Für Block $B$ mit
|
||||
Variablen $\vq_B$ und Markern $\mathcal M_B$ werden die vorhergehenden Blöcke
|
||||
$\vq_{<B}$ fixiert und
|
||||
\begin{equation}
|
||||
\vq_B^*=\arg\min_{\vq_B}\ \sum_{m\in\mathcal M_B}\rho_\delta\!\big(\norm{\vr_m(\vq_{<B},\vq_B)}\big)
|
||||
\end{equation}
|
||||
mit Huber-Verlust $\rho_\delta$ gelöst (Levenberg--Marquardt). Da die führende
|
||||
Variable großwinklig sein kann, wird ein \emph{Multi-Start} über
|
||||
$\{0,60,\dots,300\}^\circ$ durchgeführt und die Lösung mit den kleinsten Kosten
|
||||
gewählt. Funktioniert bereits mit einem einzelnen Marker je Glied, weil
|
||||
Gleichung~\eqref{eq:res} auch die Normale einbezieht.
|
||||
|
||||
\subsection{Verfahren C --- \texttt{global\_ba}}
|
||||
Globales Bündelausgleichsproblem über \emph{alle} Gelenkvariablen gemeinsam:
|
||||
\begin{equation}
|
||||
\vq^*=\arg\min_{\vq\in\R^7}\ \sum_{m\in\mathcal M}\rho_\delta\!\big(\norm{\vr_m(\vq)}\big).
|
||||
\label{eq:globalba}
|
||||
\end{equation}
|
||||
Da alle Marker simultan einfließen, bestimmen die Fingermarker rückwärts auch
|
||||
die markerlosen Gelenke $b$ (Hand) und $c$ (Palm) --- ein verdecktes Mittelglied
|
||||
wird durch nachgelagerte Marker überbrückt. Als Startwert dient die Lösung von
|
||||
Verfahren B, was lokale Minima bei großen Winkeln vermeidet. $\rho_\delta$
|
||||
(Huber, Skala \texttt{huber\_delta\_mm}) dämpft Ausreißer-Marker.
|
||||
|
||||
\subsection{Verfahren D --- \texttt{hybrid} (Standard)}
|
||||
Die empfohlene Kombination: sequenzieller Initialwert (B) gefolgt von globalem
|
||||
Refinement (C). Sie vereint die Robustheit des blockweisen Vorgehens gegen
|
||||
lokale Minima mit der Genauigkeit und der Lückenüberbrückung des globalen
|
||||
Ausgleichs und ist im Benchmark durchgehend am stabilsten.
|
||||
|
||||
% =====================================================================
|
||||
\section{Beobachtbarkeit und Konfidenz}
|
||||
% =====================================================================
|
||||
Aus dem Verhältnis sichtbarer Marker zu Variablen je Block,
|
||||
$\kappa_B=\lvert\mathcal M_B^{\text{obs}}\rvert/\lvert\vq_B\rvert$, wird pro
|
||||
Gelenk eine Konfidenz abgeleitet:
|
||||
\[
|
||||
\text{\texttt{high}}\ (\kappa\ge2),\quad
|
||||
\text{\texttt{medium}}\ (\kappa\ge1),\quad
|
||||
\text{\texttt{low}}\ (\kappa<1,\ \text{unterbestimmt}),\quad
|
||||
\text{\texttt{none}}\ (0\ \text{Marker}).
|
||||
\]
|
||||
Sie steht in \texttt{robot\_state.json} und ist sicherheitsrelevant: Gelenke mit
|
||||
\texttt{low}/\texttt{none} sollten nicht ungeprüft angefahren werden. In solchen
|
||||
Fällen kann durch Drehen der Achse $a$ (Arm2) die Hand aus weiteren Perspektiven
|
||||
sichtbar gemacht werden (Multi-Pose), womit die Beobachtbarkeit steigt.
|
||||
|
||||
% =====================================================================
|
||||
\section{Konfiguration über robot.json}
|
||||
% =====================================================================
|
||||
\texttt{robot.json} beschreibt zugleich das kinematische Modell, die
|
||||
Markerpositionen, die Rendering-Parameter und die Algorithmuseinstellungen.
|
||||
Die wichtigsten Abschnitte:
|
||||
|
||||
\begin{itemize}[leftmargin=1.4em,itemsep=2pt]
|
||||
\item \texttt{units} --- Längen-/Winkeleinheiten (mm / Grad).
|
||||
\item \texttt{vision\_config} --- ArUco-Wörterbuch und \texttt{MarkerSize}.
|
||||
\item \texttt{links} --- die kinematische Kette: pro Link \texttt{parent},
|
||||
\texttt{mountPosition/Rotation}, \texttt{jointToParent} (\texttt{type},
|
||||
\texttt{variable}, \texttt{axis}, \texttt{origin}) und \texttt{markers}
|
||||
(\texttt{id}, \texttt{position}, \texttt{normal}, \texttt{size}).
|
||||
\item \texttt{renderingInfo} --- Kamera/Render-Defaults (\texttt{width},
|
||||
\texttt{height}, \texttt{dofFStop}, \dots) für den Blender-Generator.
|
||||
\item \texttt{pose\_estimation} --- Algorithmussteuerung (siehe unten).
|
||||
\item \texttt{robot\_test\_poses}, \texttt{test\_camera\_positions/targets} ---
|
||||
Vorgaben für den Szenengenerator.
|
||||
\end{itemize}
|
||||
|
||||
Der Block \texttt{pose\_estimation} steuert Stufe 4:
|
||||
\begin{lstlisting}[language=]
|
||||
"pose_estimation": {
|
||||
"method": "hybrid", // sequential_vector | sequential_fk | global_ba | hybrid
|
||||
"marker_observation": "corner_pose", // oder "center_point"
|
||||
"use_normals": true,
|
||||
"normal_weight": 100.0, // Gewicht w_n der Normalenresiduen
|
||||
"robust_loss": "huber", // robuste Verlustfunktion
|
||||
"huber_delta_mm": 8.0, // Skala delta des Huber-Verlusts
|
||||
"max_iterations": 200,
|
||||
"min_cameras_per_marker": 2, // Mindestkameras zur Triangulation
|
||||
"per_link_method": {} // optional: Verfahren je Glied
|
||||
}
|
||||
\end{lstlisting}
|
||||
|
||||
\noindent\textbf{Wichtige Optionen.}
|
||||
\texttt{method} wählt das Verfahren (A--D). \texttt{normal\_weight} ist der
|
||||
zentrale Stabilitäts-Parameter: höhere Werte stabilisieren sicht-arme Posen
|
||||
deutlich, da die gemessene Normale stärker einbezogen wird; zu hohe Werte
|
||||
($\gtrsim\!300$) lassen die Orientierung die Position überstimmen. Empirisch ist
|
||||
$w_n=100$ ein robuster Kompromiss. \texttt{huber\_delta\_mm} bestimmt, ab welchem
|
||||
Residuum (in mm) ein Marker als Ausreißer gedämpft wird.
|
||||
\texttt{marker\_observation} schaltet zwischen den Eckposen (\texttt{corner\_pose},
|
||||
empfohlen) und reinen Mittelpunkten (\texttt{center\_point}) um.
|
||||
|
||||
% =====================================================================
|
||||
\section{Aufruf und Verwendung}
|
||||
% =====================================================================
|
||||
\textbf{Gesamte Pipeline} (Detektion bis Gelenkwinkel) für einen Bildordner:
|
||||
\begin{lstlisting}[language=bash]
|
||||
cd run
|
||||
.\run_pipeline.bat ..\data\simulation\Scene8
|
||||
# erzeugt data/evaluations/Scene8/robot_state.json
|
||||
\end{lstlisting}
|
||||
|
||||
\textbf{Einzelne Stufen} (z.\,B. zum Experimentieren mit dem Verfahren):
|
||||
\begin{lstlisting}[language=bash]
|
||||
python pipeline/3b_corner_marker_poses.py --evalDir data/evaluations/Scene8 --robot data/robot/robot.json
|
||||
python pipeline/pose_estimation.py data/evaluations/Scene8/aruco_marker_poses.json \
|
||||
-robot data/robot/robot.json --method hybrid -out data/evaluations/Scene8/robot_state.json
|
||||
\end{lstlisting}
|
||||
|
||||
\textbf{Validierung gegen Grundwahrheit} und \textbf{Benchmark}:
|
||||
\begin{lstlisting}[language=bash]
|
||||
python benchmark/eval_pose.py data/evaluations/Scene8/robot_state.json data/simulation/Scene8/pose.json
|
||||
python benchmark/run_benchmark.py --scenes 4 5 6 7 8 9 9a 9b 11 12
|
||||
python benchmark/stage0_corner_normals.py --evalDir data/evaluations/Scene5 \
|
||||
--gt data/simulation/Scene5/render_a.json
|
||||
\end{lstlisting}
|
||||
|
||||
\textbf{Szenengenerator} (Blender, erzeugt Bilder + npz + Grundwahrheit):
|
||||
\begin{lstlisting}[language=bash]
|
||||
cd run
|
||||
.\run_sceneGenerator.bat --poses 8 # nur Pose 8 rendern
|
||||
\end{lstlisting}
|
||||
|
||||
\textbf{Visualisierung.} \texttt{run/robot\_viewer.html} im Browser öffnen,
|
||||
\texttt{robot.json} und \texttt{aruco\_marker\_poses.json} laden. Der
|
||||
Observed-Normals-Pfeil zeigt die gemessene Normale, eingefärbt nach
|
||||
Winkelabweichung zur Modellnormale (grün $<2^\circ$ / gelb $2$--$5^\circ$ / rot $>5^\circ$).
|
||||
|
||||
% =====================================================================
|
||||
\section{Validierung und Ergebnisse}
|
||||
% =====================================================================
|
||||
\subsection{Methodik}
|
||||
Die Validierung nutzt synthetische Szenen aus einem Blender-Generator
|
||||
(\texttt{run/run\_sceneGenerator.bat}). Dieser rendert pro Pose $C=7$ Ansichten
|
||||
und schreibt neben Bildern (\texttt{render\_*.png}) und Intrinsiken
|
||||
(\texttt{render\_*.npz}) zwei Grundwahrheits-Dateien: \texttt{render\_*.json} mit
|
||||
den wahren Marker-Weltposen und \texttt{pose.json} mit den wahren Gelenkwinkeln.
|
||||
Drei Werkzeuge vergleichen die Rekonstruktion gegen diese Wahrheit:
|
||||
\texttt{stage0\_corner\_normals.py} (Normalenfehler der Triangulation),
|
||||
\texttt{9\_evaluateMarker.py} (Positions-/Normalenfehler je Marker und Glied)
|
||||
sowie \texttt{eval\_pose.py} (Gelenkwinkelfehler). Der Treiber
|
||||
\texttt{run\_benchmark.py} aggregiert über Szenen und Verfahren.
|
||||
|
||||
\subsection{Ergebnisse}
|
||||
Über zehn simulierte Posen mit bekannter Grundwahrheit (eine fehlerhaft
|
||||
gerenderte Pose ausgeschlossen) ergeben sich die folgenden mittleren
|
||||
Gelenk-Winkelfehler. Die Standardabweichung über die Szenen ist das Maß für die
|
||||
\emph{Stabilität}.
|
||||
|
||||
\begin{center}
|
||||
\begin{tabular}{@{}lccccc@{}}
|
||||
\toprule
|
||||
Verfahren & Mittel [$^\circ$] & Std [$^\circ$] & Schlechtest. [$^\circ$] & Mittel [mm] & Schlechtest. [mm]\\
|
||||
\midrule
|
||||
\texttt{sequential\_vector} & 0{,}315 & 0{,}128 & 1{,}717 & 0{,}144 & 0{,}712\\
|
||||
\texttt{sequential\_fk} & 0{,}434 & 0{,}171 & 1{,}838 & 0{,}158 & 0{,}851\\
|
||||
\texttt{global\_ba} & 0{,}253 & 0{,}134 & 1{,}568 & 0{,}103 & 0{,}390\\
|
||||
\textbf{\texttt{hybrid}} & \textbf{0{,}253} & \textbf{0{,}134} & \textbf{1{,}568} & \textbf{0{,}103} & \textbf{0{,}390}\\
|
||||
\bottomrule
|
||||
\end{tabular}
|
||||
\end{center}
|
||||
|
||||
Alle Verfahren liegen unter $0{,}5^\circ$ Mittelwert; \texttt{hybrid}/\texttt{global\_ba}
|
||||
sind am genauesten und am stabilsten und rekonstruieren auch die markerlosen
|
||||
Gelenke $b,c$ rein aus den Fingermarkern.
|
||||
|
||||
% =====================================================================
|
||||
\section{Grenzen und Ausblick}
|
||||
\label{sec:limits}
|
||||
% =====================================================================
|
||||
\begin{itemize}[leftmargin=1.4em,itemsep=2pt]
|
||||
\item \textbf{Intrinsik.} Da die Board-Marker koplanar sind, ist die Kamerantiefe
|
||||
schlecht konditioniert; ein fehlerhaftes $f_y$ (z.\,B. ein früherer
|
||||
Brennweitenfehler von $\sim$15\,\% bei 16:9) verfälscht alle Höhen. Korrekte
|
||||
npz-Intrinsiken sind Voraussetzung.
|
||||
\item \textbf{Sicht-arme Posen.} Sind zu wenige Marker eines Gliedes sichtbar,
|
||||
ist der zugehörige Winkel schlecht bestimmt (\texttt{confidence=low}). Abhilfe:
|
||||
Multi-Pose durch Drehen der Achse $a$.
|
||||
\item \textbf{Adaptive Normalengewichtung.} Ein festes $w_n$ ist ein Kompromiss;
|
||||
eine pro-Marker nach Sichtqualität skalierte Gewichtung könnte Best- und
|
||||
Worst-Case gleichzeitig optimieren.
|
||||
\item \textbf{Reale Kameras.} Stufe 1 nutzt derzeit dieselbe Intrinsik-npz für
|
||||
alle Bilder (Simulation: identische Kameras); reale Aufbauten benötigen je
|
||||
Kamera eine eigene Kalibrierung.
|
||||
\end{itemize}
|
||||
|
||||
\end{document}
|
||||
Reference in New Issue
Block a user