From 576a9ac30b231ed5059db6bc2d51b872d7d17f96 Mon Sep 17 00:00:00 2001
From: Jan Wille <jan.wille@stud.hs-hannover.de>
Date: Tue, 9 Aug 2022 21:45:23 +0200
Subject: [PATCH] kalibreirung: Node implentierung
---
chap/kalibrierung.tex | 133 +++++++++++++++++++++++++++++++++++++++++-
1 file changed, 132 insertions(+), 1 deletion(-)
diff --git a/chap/kalibrierung.tex b/chap/kalibrierung.tex
index 238ad7f..47a1497 100644
--- a/chap/kalibrierung.tex
+++ b/chap/kalibrierung.tex
@@ -269,7 +269,138 @@
Mit dem verwendeten Datensatz ergibt sich ein Reprojektions-Fehler von $0,049$, was genau genug für diesen Anwendungsfall ist.
- \subsection{Anwenden der Kalibrierung in einem \gls{ROS Nodelet}}
+
+ \subsection{Anwenden der Kalibrierung in einer ROS Node}
+
+ Um die Kalibrierungsergebnisse auf jedes Bild, dass vom Kamera Treiber veröffentlicht wird, anzuwenden, wird eine weitere \gls{ROS Node}
+ erstellt. Diese entzerrt jedes erhaltene Bild und veröffentlicht die korrigierte Version als eigens \gls{Topic}. Das korrigierte Bild wird
+ sowohl in Farbe als auch in Schwarz-Weiß veröffentlicht. Die Beziehung der \glspl{Topic} ist in \autoref{fig: topics graph undistoreter node}
+ Grafisch dargestellt.
+
+ \begin{figure}
+ \includegraphics[scale=.9]{svg/Topics_undistorter.pdf}
+ \caption{Beziehungen der entzerrer Node zu bestehenden Nodes}
+ \label{fig: topics graph undistoreter node}
+ \end{figure}
+
+
+ \subsubsection{Initialisieren der Node}
+
+ Beim Start der \gls{ROS Node} wird die \lstinline{main()} Funktion aufgerufen, welche die notwendigen \gls{ROS} Funktionen zur
+ Initialisierung aufruft, das benötigt \gls{Topic} abonniert, ein \gls{Callback} anhängt und die eigenen \glspl{Topic} veröffentlicht.
+ \todo{Code hierzu?}
+
+ Außerdem werden die Kalibrierdaten aus einer Konfigurationsdatei im YAML-Format eingelesen und in variablen übernommen. Die
+ Verzerrungsparameter werden als Vektor eingelesen und die Kameramatrix wird in eine \gls{OpenCV} Matrix umgewandelt. Außerdem wird die
+ Bildgröße benötigt und aus der Konfigurationsdatei gelesen. \autoref{code: intrinsic einlesen YAML} zeigt den Ablauf. Es ist sinnvoll,
+ dies bereits in der \lstinline{main()} Funktion durchzuführen, um die \gls{Callback} zu entlasten und dort Rechenzeit einzusparen.
+
+ \begin{lstlisting}[
+ float,
+ style=example,
+ caption=Einlesen der Kalibrierungsergebnisse aus einer YAML-Datei,
+ label=code: intrinsic einlesen YAML,
+ language=C++
+ ]
+ // open YAML-file and get config
+ std::string configFilePath = "./tools/calibration/calibration.yaml";
+ YAML::Node full_config = YAML::LoadFile(configFilePath);
+ YAML::Node camera_config = full_config["cameras"]["default"];
+
+ // read distorion coeffitiones and convert to OpenCV vector
+ auto distortion_YAML = camera_config["intrinsic"]["distortion"] .as<std::vector<double>>();
+ cv::Mat distortion ( distortion_YAML );
+
+ // read camera matrix and convert to OpenCV matrix
+ auto cameraMatrix_YAML = camera_config["intrinsic"]["matrix"] .as<std::vector<std::vector<double>>>();
+ cv::Mat cameraMatrix = toMat( cameraMatrix_YAML );
+
+ // read image size
+ cv::Size imageSize(
+ full_config["images"]["size"]["width"].as<int>(),
+ full_config["images"]["size"]["height"].as<int>()
+ );
+ \end{lstlisting}
+
+ Mit diesen Werten können nun \emph{Mappings} erzeugt werden, welche die geometrische Beziehung zwischen einem Pixel im Originalbild und
+ einem Pixel im entzerrten Bild abspeichern. Es werden zwei \emph{Mappings} für die X und die Y-Koordinate erzeugt, welche in globalen
+ Variablen abgelegt werden. Das ist notwendig damit die Informationen der \gls{Callback} zur Verfügung stehen.
+
+ Zuvor ist es aber noch sinnvoll, eine umskalierte, optimierte Kameramatrix zu erzeugen. \gls{OpenCV} stellt hierzu die Funktion
+ \lstinline{getOptimalNewCameraMatrix()} zur Verfügung. Diese erstellt die neue Matrix abhängig von einem freien Skalierungsparameter
+ $\alpha$. Für $\alpha=0$ ist die zurückgegebene Matrix so gewählt, dass das entzerrte Bild möglichst wenig unbekannte Pixel enthält. Das
+ bedeutet aber, dass einige Pixel des Originalbildes außerhalb des neuen Bildbereiches liegen und vernachlässigt werden. Mit $\alpha=1$
+ enthält das entzerrte Bild alle Pixel des Originalbildes, allerdings bleiben einige Pixel schwarz. Da die Funktion zusätzlichen eine
+ \gls{ROI} liefert, welches den Bildausschnitt ohne schwarze Pixel beschreibt, wird hier $\alpha=1$ verwendet. Die veröffentlichten Bilder
+ werden zwar auf die \gls{ROI} reduziert, aber die vorhanden Informationen werdenden grundsätzlich erhalten und bei Bedarf kann das
+ Programm einfach angepasst werden, um die vollständigen Bilder zu veröffentlichen.
+
+ \begin{lstlisting}[
+ float,
+ style=example,
+ caption=Bestimmen der Pixel-Mappings zu Entzerrung,
+ language=C++
+ ]
+ // get scaled camera matrix
+ auto scaledCameraMatrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distortion, imageSize, 1, imageSize, &ROI);
+
+ // calculate undistortion mappings
+ cv::initUndistortRectifyMap(cameraMatrix, distortion, cv::Mat(), scaledCameraMatrix, imageSize, CV_16SC2, rectifyMapX, rectifyMapY);
+ \end{lstlisting}
+
+
+ \subsubsection{Callback-Funktion zur Handhabung der Einzelbilder}
+
+ Die \gls{Callback} \lstinline{callback_undistort_image()} wurde während der Initialisierung an das \gls{Topic} \lstinline{/img/raw}
+ angehängt und wird nun für jedes dort veröffentlichte Bild aufgerufen. Der \autoref{code: undistort callback} zeigt eine vereinfachte
+ Version der Implementierung, ohne Umwandlung in ein Schwarzweißbild und ohne Laufzeitmessung.
+
+ Da das Bild als \gls{ROS} eigener Datentyp übergeben wird, muss es zuerst in ein mit \gls{OpenCV} kompatibles Format umgewandelt
+ werden. Die dazu notwendigen Funktionen sind im \gls{ROS}-Paket \lstinline{cv_bridge} zur Verfügung gestellt. Dessen Funktion
+ \lstinline{toCvCopy()} kopiert die Daten des Originalbildes in eine OpenCV Matrix, welche weiter verwendet werden kann.
+
+ Das Bild kann nun mit der OpenCV Funktion \lstinline{remap()} entzerrt werden. Diese benutzt die zuvor bestimmten \emph{Mappings}, um
+ jeden Pixel des Originalbildes an die korrekte Position im entzerrten Bild zu übertragen. Dabei wird linear interpoliert.
+
+ Das Erhalten Bild wird auf die \gls{ROI} reduziert und unter dem \gls{Topic} \lstinline{\img\color} veröffentlicht. Außerdem wird ein
+ Schwarz-Weiß Version erzeugt und diese als \lstinline{\img\gray} veröffentlicht.
+
+ \begin{lstlisting}[
+ float,
+ style=example,
+ caption=Vereinfachte Version der \gls{Callback} zur Durchführung der Entzerrung,
+ label=code: undistort callback,
+ language=C++
+ ]
+ void callback_undistort_image(sensor_msgs::Image original) {
+ cv::Mat undistoredImage;
+
+ // convert from ROS msg-type to opencv matrix
+ cv_bridge::CvImagePtr imagePtr = cv_bridge::toCvCopy(original);
+
+ // apply the calculated maps to undistort the image
+ cv::remap(imagePtr->image, undistoredImage, rectifyMapX, rectifyMapY, cv::INTER_LINEAR);
+
+ // crop relevant section from image
+ undistoredImage = undistoredImage(ROI);
+
+ // publish images
+ cv_bridge::CvImage colorImage(std_msgs::Header(), "rgb8", undistoredImage);
+ pub_colorImage->publish(colorImage.toImageMsg());
+ }
+ \end{lstlisting}
+
+
+ \subsubsection{Performance Betrachtung}
+
+ \begin{figure}
+ \includegraphics[width=.6\textwidth, trim={0 0 12px 31px}, clip]{img/jtop_camera.png}
+ \caption{CPU Auslastung des JetBots mit laufender Kamera und entzerrer \gls{ROS Node}}
+ \label{fig: jtop cam+undist}
+ \end{figure}
+
+ Runtime: $\approx 6\,\ms$
+
\section{Extrinsische Kalibrierung} \label{sec: extrensic}
--
GitLab