Skip to content
Snippets Groups Projects
Commit 576a9ac3 authored by Jan Wille's avatar Jan Wille
Browse files

kalibreirung: Node implentierung

parent 23af1218
No related branches found
No related tags found
No related merge requests found
...@@ -269,7 +269,138 @@ ...@@ -269,7 +269,138 @@
Mit dem verwendeten Datensatz ergibt sich ein Reprojektions-Fehler von $0,049$, was genau genug für diesen Anwendungsfall ist. 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} \section{Extrinsische Kalibrierung} \label{sec: extrensic}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment