% !TeX program = lualatex

\documentclass{spie}

\usepackage{cite}
\usepackage{times} % Times New Roman
\usepackage{indentfirst} % Indent first paragraphs
\usepackage[colorinlistoftodos,color=pink]{todonotes} % For fancy \todo{something}
\usepackage{xkeyval}
\presetkeys{todonotes}{inline}{}
\newcommand{\fourier}{\mathcal{F}}
\usepackage{ifluatex}

\ifluatex%                           % if we use luatex
%\ExecuteOptions{dvips}
\usepackage{graphicx}                % allow us to embed graphics files
\DeclareGraphicsExtensions{.eps}     % for pure latex we expect eps files
\else%                                 % else we use pure latex
\pdfoutput=1\relax                   % create PDFs from pdfLaTeX
\pdfcompresslevel=9                  % PDF Compression
\pdfoptionpdfminorversion=7          % create PDF 1.7
\ExecuteOptions{pdftex}
\usepackage{graphicx}                % allow us to embed graphics files
\DeclareGraphicsExtensions{.pdf,.png,.jpg,.jpeg} % for pdflatex we expect .pdf, .png, or .jpg files
\fi%

%% my changes
\usepackage{pgfplots}
\pgfplotsset{compat=newest}
\newcommand{\tabhead}[1]{\textbf{#1}} % for tables
\usepackage{pbox} % for breaking lines in tables
\usepackage{booktabs} % To thicken table lines
\usepackage{microtype}
\usepackage{enumitem} % for compact enum
\usepackage{amsmath}
\usepackage{amssymb}
\usepackage{microtype}
\usepackage{subcaption}

%% SUBMISSION ONLY
%\usepackage[pagebackref=false,breaklinks=true,letterpaper=true,colorlinks=true,citecolor=black, linkcolor=black, bookmarks=false]{hyperref}

%% EDIT ONLY
\usepackage[pagebackref=false,breaklinks=true,letterpaper=true,colorlinks,bookmarks=false]{hyperref}
%\usepackage[pagewise]{lineno}
%\linenumbers

%% shorthands
\newcommand{\etal}{\textit{et al}. }
\newcommand{\ie}{\textit{i}.\textit{e}. }
\newcommand{\eg}{\textit{e}.\textit{g}. }
\newcommand{\bigO}{\mathcal{O}}
\newcommand{\pseudosection}[1]{\todo[color=green!40]{#1}}

%Section capitalization fix
\renewcommand{\sectionautorefname}{Section}
\renewcommand{\subsectionautorefname}{Subsection}
\renewcommand{\subsubsectionautorefname}{Subsubsection}

\title{Calibrating Depth Sensors with a Genetic Algorithm}

\author{Jonas Haeling\supit{1,2}, Marc Necker\supit{1}, Andreas Schilling\supit{2}
  \skiplinehalf
  \normalsize 
  \supit{1}Daimler AG R\&D, Sindelfingen, Germany; \\
  \supit{2}University of T{\"u}bingen, T{\"u}bingen, Germany;
}

\begin{document}
\maketitle
\begin{abstract}
In this report, we deal with the optimization of the transformation estimate between the coordinate systems of depth sensors, \ie sensors that produce 3D measurements. For that, we present a novel method using a genetic algorithm to refine the six degrees of freedom (6 DoF) transformation via three rotational and three translational offsets. First, we demonstrate the necessity for an accurate depth sensor calibration using a depth error model of stereo cameras. The fusion of stereo disparity assumes a Gaussian disparity error distribution, which we examine with different stereo matching algorithms on the widely-used KITTI visual odometry dataset. Our analysis shows that the existing calibration is not adequate for accurate disparity fusion. As a consequence, we employ our genetic algorithm on this particular dataset, which results in a greatly improved calibration between the mounted stereo camera and the Lidar. Thus, stereo disparity estimates show improved results in quantitative evaluations.
\end{abstract}

%%%%%%%%% BODY TEXT
\section{Introduction}
An accurate calibration between depth sensors is important to ensure that data between sensors can be merged or evaluated jointly. The evaluation of a cheap sensor is an additional important task, \eg comparing a stereo camera against an accurate but prohibitively expensive sensor like a Lidar or a structured light setup. Even though our proposed technique is general enough to handle any comparisons between depth maps, we are particularly interested in Lidar-to-stereo calibration in this report. This calibration is needed in vehicle setups like the popular KITTI visual odometry dataset \cite{Geiger2012} in which the Lidar delivers accurate but sparse measurements. The stereo camera produces dense depth maps with additional image intensity or color information. The stereo depth error increases quadratically with the depth itself (see \autoref{subsec:importance_stereo}), depending on the intrinsics of the stereo camera and the particular stereo algorithm. To correctly evaluate the achieved accuracy of the dense stereo matching against an accurate Lidar, an exact transformation estimate, i.e. a calibration, between the two coordinate systems is required, which is the main focus of this report.

Genetic algorithms are optimization techniques, which are bio-inspired by evolution. The term genetic algorithm has a diffuse meaning \cite{Whitley1994genetic}, but the general idea is to use an iterative process with evolutionary elements like natural selection, crossover and mutation to find solutions to hard problems, \eg the traveling salesman problem. The solution may not be optimal and might only be a local optimum in a niche, but a direct solution for some problems is not always feasible.

A generation of individuals constitutes the current population in a genetic algorithm. An individual could simply be an encoded solution to the problem, \eg a set of real numbers or indices. The genetic algorithm iteratively produces new generations using the immediate previous one to optimize solutions. To mimic natural selection, one has to model the fitness of an individual, which corresponds to a measure of how good the solution is. The fittest individuals have a higher chance to reproduce. For the reproduction, there is a crossover phase in which new offspring for the next generation are created by the recombination of genes. The parents are sampled from the population with weights according to the fitness. Random mutations occur during reproduction, which adds variance to the gene pool, thus potentially better or worse fitness values. That concludes one iteration. This process leads to an optimization of the species to the specific environment over time, \ie an optimized solution to the problem.

Our genetic algorithm optimizes the six degrees of freedom between the coordinate systems of both depth sensors. We assume an existing approximate estimate for the transformation and refine it with a rotational and translational offset.  An individual of a population has six offsets, which can be altered. The genetic algorithm iteratively recombines and mutates new offspring, which is then evaluated over several frames to compute an individual fitness using the histogram of the resulting error distribution. We demonstrate our algorithm on a real dataset and show greatly improved results concerning the disparity error distributions. This new ``recalibration'' may also improve quantitative results for stereo reconstruction approaches, especially those that rely on disparity fusions.

A similar algorithm to our proposed solution is the widely researched iterative closest point (ICP) algorithm to register two 3D point clouds iteratively \cite{Besl1992method}. Here, we match the points in image space like some ICP variants over multiple frames and explore and refine the existing solution via a genetic algorithm. Even though we recalibrate a Lidar to a stereo camera, it would still be possible to calibrate two separate stereo cameras. This will result in higher run times due to the increased point cloud density.

\autoref{sec:motivation} describes the motivation behind calibrating depth sensors using a stereo camera depth error model, including an examination of the disparity error on a real dataset. \autoref{sec:gen_algo} explains the basic sequence of our genetic algorithm and the GPU implementation.  We test our algorithm and show the corresponding results in \autoref{sec:results}. 
Finally, we give a summary and an outlook for further research in \autoref{sec:conclusion}.

\section{Motivation}
\label{sec:motivation}
\subsection{Stereo Disparity Fusion}
\label{subsec:importance_stereo}
Kalman filters have been widely used in stereo vision for various aspects including the fusion of disparities between frames \cite{ matthies1989kalman, lee1990kalman, franke20056d, sibley2006iterated, Wedel2011stereo, morales2013kalman}. For most Kalman filters used in the stereo vision context, one key assumption is Gaussian stereo matching noise, \ie Gaussian disparity errors. If this assumption is not given, \eg the distribution is actually skewed or multimodal, then formally correct fusion algorithms might help measurements to be more precise, but the accuracy of the measurement does not improve because of an inherent bias. Popular stereo matching benchmarks like the ``Middlebury Stereo Evaluation'' \cite{Scharstein2002} or ``Stereo Evaluation 2015'' from the KITTI Vision Benchmark Suite \cite{Menze2015} do not report the error distribution directly, unfortunately. Assuming that a certain stereo matching algorithm exhibits Gaussian noise as disparity errors, an inexact transformation to ground truth depth sensor measurements would yield an artificial error overestimation in a quantitative error evaluation. Thus, refining this calibration would improve quantitative results without changing the actual stereo depth estimation process.

Geyer \etal \cite{Geyer2006recursive} proposed to model the depth error $e_z$ as follows:
\begin{equation}
e_z = \frac{Bf}{d} - \frac{Bf}{d + e_d} = \frac{z^2 e_d}{B f+z e_d}
\label{eq:z_error}
\end{equation}
$B$ is the stereo baseline, $f$ the focal length, $d$ the disparity and $e_d$ the disparity or matching error. Thus, to decrease the depth error $e_z$ we have to either increase $B$ or $f$, decrease $e_d$ or simply move closer to the object for a smaller $z$. Gallup \cite{Gallup2011} varied the baseline and focal length by choosing the input cameras for their plane sweep algorithm or downsampling the images to achieve a desired fixed depth accuracy. Geyer \etal \cite{Geyer2006recursive} increased the stereo baseline and fused measurements from a moving airborne vehicle observing the ground using overlapping images of a mono-camera to reduce the error variance of range measurements. 

Alternatively, we can fuse 3D points temporally between frames with a static stereo baseline, which enables us to decrease the stereo matching error $e_d$. Reformulating \autoref{eq:z_error}, we can approximate the needed disparity error $e_d$ to achieve a certain desired and fixed depth error $e_z$:

\begin{equation}
e_{d} = \frac{e_z B f}{z (z - e_z)}
\label{eq:d_error}
\end{equation}

$e_z$ is not distributed as a Gaussian because the disparities are transformed non-linearly from image space to 3D camera space (cf. Sibley \etal \cite{Sibley2007}). Therefore we can only approximately handle $e_z$ as Gaussian. 

Assuming Gaussian noise for the disparity error (with $e_d$ being the standard deviation), the fusion of two uncorrelated observations of the same depth will result in a lower variance of the error. If the error is perfectly correlated, \eg when running a deterministic stereo matching with unchanged parameters again on the same exact images, then the fusion will not decrease the error. 

Geyer \etal \cite{Geyer2006recursive} found in simulated experiments that the error of stereo measurements was correlated up to 60\%, which suggests that we underestimate the error with disparity fusion in practice. For the time-being, we will not address this issue. Assuming uncorrelated errors in the stereo matching, the fused disparity estimate $e_{d'}$ is computed from the normalized product of two 1D Gaussian probability density functions with standard deviations $e_{d_1}$ and $e_{d_2}$ (see Barfoot \cite{Barfoot2017state}):

\begin{equation}
\frac{1}{e_{d'}^2} = \frac{1}{e_{d_1}^2} +\frac{1}{e_{d_2}^2}
\label{eq:gauss_product}
\end{equation}

This is essentially the summation of both information terms to a new information term. To generalize, the fusion of $n$ uncorrelated observations with the same disparity variance results in a new error $e_{d'}$, which allows us to compute the needed number of fusions $n$ for the disparity error with a fixed depth error:

\begin{align}
&\frac{1}{e_{d'}^2} = n \cdot \frac{1}{e_d^2}\nonumber\\
&\Rightarrow n = \frac{e_d^2}{e_{d'}^2} = \frac{e_d^2}{\left(\frac{e_{z'} B f}{z (z - e_{z'})}\right)^2} = \frac{e_d^2 z^4 + e_d^2 e_{z'}^2 z^2 - 2 e_d^2 e_{z'} z^3}{e^2_{z'}B^2f^2} = \bigO(z^4)
\label{eq:n_fusion}
\end{align}

We can see that the number of frames $n$ is bound by $\bigO(z^4)$, which means to achieve the desired and fixed depth error $e_{z'}$ from measurements of a particular depth, we need quartically more uncorrelated measurements of the same depth (assuming $e_{d'} < e_d$). 

One concrete fusion example could be a house facade seen from far away by a sidewards looking camera in a vehicle, so that we always measure the same depth. \autoref{tab:fusion_examples} gives examples for the needed independent observations to get a desired accuracy. The intrinsic parameters for this were chosen similar to the actual parameters of the KITTI visual odometry dataset \cite{Geiger2012}. Without fusion, the desired accuracy is achieved by
points below a distance of ca. 8.7 m. We also compare the fused vs. unfused depth error in $e_z$, which is only an approximation since the resulting distribution is skewed for far depth values (cf. Sibley \etal \cite{sibley2006iterated}).

\begin{table*}
	\centering
	\caption{Examples for the needed uncorrelated observations (n) at a certain fixed depth to achieve a desired accuracy $e_{z'} = 0.10$ m with $e_d=0.5$ px, $B=0.54$ m and $f=700$ px.}
	\label{tab:fusion_examples}
	\vspace*{1pt}
	\begin{tabular}{r r | c c}
		\toprule
		\tabhead{Depth} & \tabhead{n} & \tabhead{Fused $e_z$} & \tabhead{Unfused $e_z$}\\ 
		\midrule
		8.7 m & 1 & 0.10 m& 0.10 m\\
		10 m& 1.7 & 0.10 m& 0.13 m\\	
		20 m& 27.7 & 0.10 m& 0.53 m\\
		30 m& 140.8 & 0.10 m& 1.19 m\\	
		40 m& 445.7 & 0.10 m& 2.12 m\\
		50 m& 1089.2 & 0.10 m& 3.32 m\\
		\bottomrule
	\end{tabular}
\end{table*}

To summarize, temporal disparity fusion is able to improve reconstruction results if the Gaussian noise assumption for disparity errors is valid.

\subsection{Stereo Matching Error as Gaussian Noise Assumption in Literature}

In the literature, Sibley \etal \cite{Sibley2007} used the assumption that the disparity error is distributed as a Gaussian and provided evidence using corner features and a synthetic checkerboard scene. Similarly, Matthies and Grandjean \cite{Matthies1994} found the disparity distribution to be Gaussian-like when computing the disparity by minimizing the sum of squared difference (SSD) in a search window. Sebe and Lew \cite{Sebe2000} found that in their case that a better fit is a Cauchy function when using the sum of absolute differences or SSD with real world ground truth data. Wedel and Cremers \cite{Wedel2011stereo} on the other hand propose to approximate the disparity distribution with a Laplacian distribution.

Nevertheless, this noise attribute is highly dependent on the actual stereo matching algorithm itself and the transformation to the ground truth, \ie the calibration between the depth sensors. On the one hand, the fusion may still be unbiased when the true distribution is unimodal and symmetrical, like the Cauchy or Laplacian distribution. On the other hand, the variance estimate will be off and hence the weighting of the fusion, which may introduce errors or limit the accuracy increase. In practice, the true variance is hard or impossible to estimate anyway due to the aforementioned unknown error correlations.

\subsection{Verification of Distribution}

To verify whether the stereo matching error distribution is indeed Gaussian, we test how the disparity error of actual stereo matching results is distributed. We tested the disparity distribution of the publicly available and widely used stereo matcher ELAS (``\textbf{E}fficient \textbf{La}rge-scale \textbf{S}tereo Matching'' \cite{Geiger2010}) on the KITTI odometry training dataset. This dataset is widely used to evaluate 3D reconstructions from multi-view stereo quantitatively (see \cite{Geiger2011,Sengupta2013,Alcantarilla2013, Usenko2015,Pillai2016,Tanner2016,Cigla2017gaussian,Barsan2018}) as it provides synchronized laser-scanned data as ground-truth and ground-truth poses. It is a de facto benchmark for stereo scene reconstructions from a moving vehicle. For disparity error distributions, we compute the disparity on the rectified color camera images via stereo matching, then project the 3D laser points onto the rectified left color camera image and compare the laser projections to the stereo matching disparities. We only evaluate pixels for which both stereo and laser measurements are present. We do this for every frame and every sequence except sequence 03, as there is no raw calibration data available and the measurements were made on a different day than all the others. Note that we can omit the ground-truth poses for this evaluation as the transformation between stereo camera and Lidar should remain constant. The error is computed via $d_{Stereo} - d_{Laser}$ and in \autoref{fig:elas_disparity_distribution_original} all disparity error histograms of ELAS are plotted. The sequences 00 to 02 were recorded on a different day than 04 to 10, which is why the calibration also differs, as well as the resulting shape of the distribution. For sequences 04 to 10 we clearly see a local maximum at ca. +0.6 px and for sequences 00 to 02 closer to ca. +1.4 px. However, testing ELAS on the KITTI 2015 stereo dataset \cite{Menze2015}, we see very different results, as shown in \autoref{fig:disparity_distribution_kitti2015}. Unimodal distributions with only a small bias are visible. Sources of this bias may be camera calibration errors, biased stereo matching, relative small ground truth transformation errors or Lidar uncertainty.

\begin{figure}[!bp]
	\centering
	\begin{minipage}[t]{0.49\textwidth}
		\begin{centering}	
			\input{pictures/original_disparity_distribution_kitti_elas.tex}
			\caption{The disparity error distribution of ELAS compared to velodyne laser measurements on all KITTI odometry training datasets (except 03) with the original velodyne transformation. The different line styles indicate different calibrations.}
			\label{fig:elas_disparity_distribution_original}
		\end{centering}	
	\end{minipage}
	\hfill
	\begin{minipage}[t]{0.49\textwidth}
		\begin{centering}	
			\input{pictures/stereo_2015.tex}
			\caption{Disparity error distribution of ELAS, SGBM and MeshStereo of 200 ground truth frames of the multi-view sequences of the SceneFlow 2015 KITTI training dataset \cite{Menze2015}. A normal distribution with $\mu=0$ and $\sigma = 0.65$ is drawn for comparison.}
			\label{fig:disparity_distribution_kitti2015}		
		\end{centering}	
	\end{minipage}
\end{figure}

\begin{figure}[!h]
	\centering
	\begin{tabular}[t]{c}
		\vspace{8pt}
		\begin{subfigure}[c]{1\textwidth}	
			\includegraphics[scale=0.875]{pictures/ComparisonKittiVO_a.png}
			\caption{Left color camera RGB image.}
		\end{subfigure}\\
		\vspace{8pt}
		\begin{subfigure}[c]{1\textwidth}	
			\includegraphics[scale=0.875]{pictures/ComparisonKittiVO_b.png}
			\caption{Left color camera ELAS depth image.}
		\end{subfigure}\\
		\vspace{8pt}
		\begin{subfigure}[c]{1\textwidth}	
			\includegraphics[scale=0.875]{pictures/ComparisonKittiVO_c.png}
			\caption{Disparity comparison of stereo to Lidar measurements with original calibration.}
		\end{subfigure}\\
		\vspace{8pt}
		\begin{subfigure}[c]{1\textwidth}	
			\includegraphics[scale=0.875]{pictures/ComparisonKittiVO_d.png}
			\caption{Disparity comparison after applying our new calibration.}
		\end{subfigure}\\
	\end{tabular}    
	\caption{Comparison of disparities between ELAS and the ground-truth on frame 08-430 (left) and frame 01-1100 (right). Red corresponds to positive disparity errors and blue to negative ones. Green represents good matches (< 0.5 px) between the laser and stereo measurement.}
	\label{fig:elas_kitti_comparison}
\end{figure}


\autoref{fig:elas_kitti_comparison} shows one sample frame in sequence 08 and 01, consisting of RGB image, depth image and disparity comparisons. The disparity error images were enhanced to make the visual comparison easier. Note that the vertical field of view of the velodyne laser is inherently tilted down, resulting in a cutoff of higher stereo matching points. We see a strong disparity bias, represented by red pixels on the ground, denoting too high disparity estimates of the stereo. 

We test OpenCV's StereoSGBM algorithm (SGBM) \cite{Opencv2018} as well, which is an altered implementation of semi-global matching (SGM) \cite{Hirschmuller2008stereo}. Additionally, we employ the approach ``MeshStereo'' by Zhang \etal \cite{Zhang2015}. We use three different stereo algorithm to show that they produce similar distributions, \ie it is not the fault of a single stereo matcher. After testing ELAS on synthetic images and the KITTI 2015 stereo dataset, we did not expect such a bias with the ground surface on the odometry dataset. We suspected that the velodyne to camera transformation was not sufficiently calibrated, which was confirmed in early tests with small transformation changes (also cf. Guindet \etal \cite{Guindel2017automatic}). Some of those changes made the bimodal distribution more unimodal and moved the center of mass of the error probability distribution towards zero. 

Fusing measurements with the original bimodal error distributions would still increase the precision, but not the accuracy. We would still be off the mark due to the bias of the distributions, \ie not being centered around zero error. A better calibration remedies this problem, so a recalibration of the Lidar to camera transformation is required for improved quantitative results.

\section{Genetic Algorithm}
\label{sec:gen_algo}

\begin{figure}
	\begin{centering}
		\includegraphics{pictures/EvoPipeline/EvoPipeline.pdf}
%		\vspace*{0pt}
		\caption{Schematic sequence of the genetic algorithm.}
		\label{fig:evoPipeline}
	\end{centering}
\end{figure}


Here, we are interested only in the refinement of a depth sensor to depth sensor transformation, \ie we already have a rough estimate and want to hone in on an improved solution via evolution. We do this by adding three translational offsets and three Euler angles as rotational offsets to the existing calibration.

To find suitable offsets, we employ a standard genetic algorithm, which explores and optimizes the 6 DoF solution space. Per generation, we have a population of $n=100$ individuals. An overview of the general sequence of the algorithm is shown in \autoref{fig:evoPipeline}. We heavily mutate the first generation before the first iteration. We compute the fitness of the population and rank the individuals according to the fitness score. Here, we actually use a fitness cost instead of score, so we minimize the cost instead of maximizing the score. For the fitness cost, we calculate the ratio of pixels with a resulting absolute disparity error of below 0.5 px to the total amount of valid pixels. This corresponds to the green pixels seen in \autoref{fig:elas_kitti_comparison} and empirically resulted in satisfactory results fast. It is also one of the metrics used in the Middlebury stereo evaluation \cite{Scharstein2002}. We also add one additional term to the fitness cost, which is almost identical but uses a threshold of 0.2 px to hone in on excellent stereo matches. These fitness function terms reward unbiased error distributions with low variance. We also strictly punish outlier distributions, which do not have enough projected Lidar points. After the ranking, we use elitism with $k=3$ to transfer the three best individuals to the next generation directly. For the rest, we use rank selection to select parents and produce the offspring by randomly choosing the parameter value from one parent, which corresponds to a crossover rate of 1. The offspring then finally mutates, in which a big and a tiny  mutation can occur. A big mutation consists of choosing a random value from an interval which spans the given search space ("wiggle room"), \eg 1$^\circ$ for the angles and 5 cm for the translational components. A tiny mutation is a very small variation around the current value, given a downscaled random value from the aforementioned interval. These mutations ensure that the solution space is explored but also that local minima are found faster. We use a mutation rate of 0.1 for both kinds of mutation. Then we begin computing the fitness again, which marks a completed generation iteration.

\subsection{GPU Implementation of the Fitness Evaluation}
The fitness evaluation of a whole generation is the bottleneck of the evolutionary algorithm. All Lidar points over multiple frames have to be projected with the new transformation and compared to the corresponding depth image for each individual. On a single-core CPU implementation, this has to be done sequentially: For each individual, for each frame and for each velodyne point in the frame. Almost all of the input data used in the evolution remains static, except the new calibration $T^{cam}_{velo}$. For these reasons, a simple GPU implementation of the fitness evaluation was developed with CUDA \cite{Nickolls2008scalable}.

Almost all the input data remain static, so we only have to upload all precomputed depth images, Lidar points and the projection matrix once to the GPU. Since the Lidar points have an irregular size each frame, we take the maximum Lidar point count of one frame as the array length. We pad the rest with points that will not be projected onto the camera. The GPU kernel takes as input an array of $n$ calibrations and outputs $n$ average fitnesses over all frames. We launch a kernel for each individual and each frame, which means there is potential to break down the work load even further by partitioning the Lidar point cloud per frame. We allocate $n$ floats for the fitness of each individual beforehand. For that, we employ the CUDA function atomicAdd() on the specific individual fitness index to add the computed fitness per frame, which prevents race conditions during the write operation. After the kernels have finished, we compute the individual mean of the fitness values. Then these results are downloaded to the CPU, which logs the results and starts the next generation iteration.

\section{Results on KITTI Visual Odometry Dataset}
\label{sec:results}

\subsection{Main Results}

We are looking for a new calibration between the laser and the cameras. This calibration does not need to be optimal, but should show improved error distribution results. The transformation chain for transforming a homogeneous 4D velodyne point $X = [x, y, z, 1]^T$ into a camera image point $p=(u,v,1)^T$ in the rectified left color camera coordinate system (camera 2) is the following (see Geiger \etal \cite{Geiger2013}):

\begin{equation}
p = P^{(2)}_{rect}  R^{(0)}_{rect} T^{cam}_{velo} X
\end{equation}
with 
\begin{equation}
T^{cam}_{velo} = 
\begin{pmatrix}
R^{cam}_{velo} & t^{cam}_{velo} \\
0 & 1
\end{pmatrix}	
\end{equation}

$P^{(2)}_{rect}$ is the 3x4 projection matrix for the left rectified color camera (``camera 2'') and $R^{(0)}_{rect}$ the rotation from the left gray camera (``camera 0'') to the rectified camera coordinate system of all rectified cameras. Note that $P^{(2)}_{rect}$ incorporates a translational component from camera 0. $T^{cam}_{velo}$ denotes the 4x4 transformation from the velodyne laser coordinate system to camera 0, which incorporates a 3x3 rotation matrix $R^{cam}_{velo}$ and a 3x1 translation vector $t^{cam}_{velo}$. The intrinsic stereo camera calibration was done separately from the velodyne to camera calibration (see \cite{Geiger2012}), which is why we only try to optimize $T^{cam}_{velo}$. Since we assume that the original calibration is already close to the truth, we try to find an offset to a new calibration ${T'}^{cam}_{velo}$ consisting of $R'^{cam}_{velo}$ and $t'^{cam}_{velo}$. Thus, we are looking for three Euler angle offsets and three translation offsets to improve the calibration. The Euler angles are global rotations around X, Y and Z in this order (pitch-yaw-roll), which corresponds to the following multiplication sequence of local 3D rotation matrices:

$R_XR_YR_Z = R$

One observation is that the sequences 00 to 02 were recorded on the same day and the original calibration did not change. Similarly, sequences 04 to 10 were also recorded on the same day. This means that we only have to recalibrate twice, finding one calibration for 00-02 and one for 04-10. The raw calibration data for the sequence 03 is not available.

As input to our genetic algorithm, we use every 50th KITTI frame, corresponding to a frame every five seconds to offer variation in the scenes and still maintain acceptable run times. We employed a wiggle room of $\pm$2.5$^\circ$ and $\pm$ 7.5 cm, which delivered good results quickly.

The evolution process for sequence 00 is shown in \autoref{fig:evoProcess00} and for sequence 08 in \autoref{fig:evoProcess08}. The horizontal axis depicts the generations on a logarithmic scale. One can see that after ca. 100 generations the fitness improvements are largely marginal. Improvements still occur, but they get less probable and significant. The resulting offsets after 5000 generations represent our final recalibration for the sequences 00-02 and 04-10. One observation is that the computed calibration offset for the translation is up multiple centimeters, which means that either we may be overfitting and evolving to an unlikely offset or that the original calibration is less accurate than expected. 

\begin{figure}[!h]
	\centering
	\begin{tabular}[t]{r}
		\begin{subfigure}[c]{0.82\textwidth}	
		\input{pictures/evoProcess00_fitness.tex}
		\caption{Fitness cost of best individual and average value of the generation.}
		\end{subfigure}\\
		\begin{subfigure}[c]{0.82\textwidth}	
			\input{pictures/evoProcess00_translational.tex}
			\caption{Translational offset of best individual.}
		\end{subfigure}	\\
		\begin{subfigure}[c]{0.82\textwidth}	
			\input{pictures/evoProcess00_rotational.tex}
			\caption{Rotational offset of best individual.}
		\end{subfigure}\\
	\end{tabular}    
	\caption{Evolution results over 5000 generations for sequence 00. Top to bottom: fitness scores, translational and rotational offset.}
	\label{fig:evoProcess00}
\end{figure}


\begin{figure}[!h]
	\centering
	\begin{tabular}[t]{r}
		\begin{subfigure}[c]{0.82\textwidth}	
			\input{pictures/evoProcess08_fitness.tex}
			\caption{Fitness cost of best individual and average value of the generation.}
		\end{subfigure}\\
		\begin{subfigure}[c]{0.82\textwidth}	
			\input{pictures/evoProcess08_translational.tex}
			\caption{Translational offset of best individual.}
		\end{subfigure}	\\
		\begin{subfigure}[c]{0.82\textwidth}	
			\input{pictures/evoProcess08_rotational.tex}
			\caption{Rotational offset of best individual.}
		\end{subfigure}\\
	\end{tabular}    
	\caption{Evolution results over 5000 generations for sequence 08. Top to bottom: fitness scores, translational and rotational offset. The average fitness values appear fuzzier in the end due the logarithmic scale.}
	\label{fig:evoProcess08}
\end{figure}

In \autoref{fig:distro_original_and_recal}, the two final calibration offsets for sequences 00-02 and 04-10 are used to compute the new disparity error distribution for ELAS, SGBM and MeshStereo. The new resulting distributions are clearly more unimodal, sharper and show less bias than the original distributions. Again, we did not alter the stereo measurements, merely the relative transformation between the ground truth and the stereo for the evaluation. Although we only use ELAS for the evolution of these offsets, we still improve the results of SGBM and MeshStereo. This is evidence that we are indeed not overfitting, but that the offsets are plausible since they improve the error distributions of other stereo algorithms in a very similar way. In \autoref{tab:gen_algo_final_results} we showcase our final optimized offsets for reference. 

\begin{table}
	\centering
	\caption{Final calibration offset parameters for the KITTI VO dataset.}
	\label{tab:gen_algo_final_results}
	\vspace*{1pt}
	\begin{tabular}{c c c c c c c}
		\toprule
		\tabhead{Calibration} & \tabhead{X} & \tabhead{Y} & \tabhead{Z} & \tabhead{Rx} & \tabhead{Ry} & \tabhead{Rz} \\ 
		\midrule
		00 - 02 & -0.0150154 & -0.030895 & 0.0644574 & 0.00277647 & -0.00124684 & -0.00100826\\
		04 - 10 & -0.00486741 & 0.00384346 & 0.0912176 & 0.00283365 & -0.00150622 & -0.00124315\\
		\bottomrule
	\end{tabular}
\vspace{1.5em}
\end{table}
\autoref{fig:elas_kitti_comparison} shows two KITTI VO frames without and with the new calibration. The original calibration shows a clear bias on the ground surface, while more vertical structures like the hedge and vehicles show less. Our recalibration result removes this bias and keeps the rest of the scene intact or improves it. The aforementioned ground bias is largely eliminated, which is also apparent from the new distributions shown in \autoref{fig:distro_original_and_recal}. 

\begin{figure}
	\begin{subfigure}{.5\textwidth}	
		\input{pictures/original_disparity_distribution_kitti_elas.tex}
		\caption{ELAS Original}
		\label{fig:distro_elas_orig}
	\end{subfigure}
	\begin{subfigure}{.5\textwidth}
		\input{pictures/recalibrated_disparity_distribution_kitti_elas.tex}
		\caption{ELAS Recalibration}				
		\label{fig:distro_elas_recal}
	\end{subfigure}
	\begin{subfigure}{.5\textwidth}	
		\input{pictures/original_disparity_distribution_kitti_sgbm.tex} 
		\caption{SGBM Original}
		\label{fig:distro_sgbm_orig}
	\end{subfigure}
	\begin{subfigure}{.5\textwidth}
		\input{pictures/recalibrated_disparity_distribution_kitti_sgbm.tex}
		\caption{SGBM Recalibration}				
		\label{fig:distro_sgbm_recal}
	\end{subfigure}
	\begin{subfigure}{.5\textwidth}
		\input{pictures/original_disparity_distribution_kitti_meshstereo.tex}
		\caption{MeshStereo Original}
		\label{fig:distro_meshstereo_orig}
	\end{subfigure}
	\begin{subfigure}{.5\textwidth}
		\input{pictures/recalibrated_disparity_distribution_kitti_meshstereo.tex}
		\caption{MeshStereo Recalibration}
		\label{fig:distro_meshstereo_recal}
	\end{subfigure}
	\caption{Left column: Original disparity error distributions of different stereo matching algorithms over the KITTI visual odometry training sequences. Right column: Applying the resulting offsets of the genetic algorithm.}
	\label{fig:distro_original_and_recal}
\end{figure}

\autoref{tab:run_time} compares the CPU single core implementation without SIMD instructions against the implementation with our CUDA fitness evaluation with a frame skip of 50. We use an Intel i7-6700T 2.8 GHz CPU with a nVidia GTX 1080 GPU. Our GPU implementation accomplishes a speedup of 529 x with ca. 11 Hz, which makes it feasible for online recalibration using a fixed generation limit.

\begin{table}
	\centering
	\caption{Average run time per generation of both algorithm implementations on sequence 08.}
	\label{tab:run_time}
	\vspace*{1pt}
	\begin{tabular}{c c c}
		\toprule
		\tabhead{CPU} & \tabhead{GPU} & \tabhead{GPU speedup}\\ 
		\midrule
		48.677 s & 0.092 s & ca. 529 x\\
		\bottomrule
	\end{tabular}
\end{table}

\subsection{Comparison of Variations and Alternative Approaches}
For our final fitness cost function we chose to punish bad pixels instead of minimizing the mean absolute error (MAE) or the root mean square error (RMSE). To compare these variants, we computed the mean MAE and mean RMSE over all considered frames of sequence 00 and tested it on the whole sequence. In \autoref{fig:disparity_error_variations} we show the disparity error distribution of the calibration trained with our bad pixel approach, an MAE approach and an an RMSE approach. The bad pixels approach delivers a less biased error distribution with less variance. Even with perfect calibration, the error distribution would still possess non-empty tails due to stereo matching outliers, which would dramatically influence the RMSE value and also MAE values. Our function represents a robust alternative.

Additionally, we tested training on every 25th frame or 100th frame instead of every 50th frame, which is also depicted in \autoref{fig:disparity_error_variations}. This did not significantly improve the error distribution, which means that skipping even more frames than 50 might speed up the calibration process even further without introducing significant errors.

Furthermore, the optimization was evaluated with a standard downhill simplex method and a standard gradient descent method. We computed the cost of a parameter set with our GPU implementation of the fitness cost function. The downhill simplex version in our implementation was not suitable, because it quickly got stuck in local minima. The gradient descent approach behaves similarly, but depends strongly on the choice of the step size. A gradual decrease of the step size for example proved to produce comparable results to our evolution algorithm. Still, our evolution method was faster in delivering good results due to the sheer amout of parallelization. Also, our fitness cost function compares the ratio of bad pixels over the chosen frames. This means that it is a non-differentiable function due to the discrete pixel steps, which might prove to be problematic for the convergence of gradient descent methods.

\begin{figure}
	\centering
	\input{pictures/VeloToCamVariations.tex}
	\caption{Comparison of the disparity error distributions on sequence 00 of a calibration trained in the same sequence with different frame skips (50, 25, 100) and other fitness cost functions (mean MAE, mean RMSE) than ours. Note that the frame skip graphs are overlapping.}
	\label{fig:disparity_error_variations}
\end{figure}

\section{Conclusion and Future Work}
\label{sec:conclusion}
We presented a novel recalibration method for existing depth sensors calibrations using a genetic algorithm. We showed the potential of disparity accuracy improvements with fusion if the disparity error distribution is indeed Gaussian. To demonstrate the effectiveness of our algorithm, we showed that the Lidar to stereo camera transformation in the KITTI visual odometry dataset is not sufficiently calibrated, \ie the disparity error distribution is not Gaussian. The resulting new calibration shows significant improvements in the evaluation, which was even true for other stereo matching algorithms than the input.

For future work, it might be interesting to see if an even faster implementation or better fitness function would make stereo to stereo calibration feasible for the automatic online calibration of depth sensors even during transit. It might even be possible to refine stereo camera pose movements between frames, \ie as a pose movement optimizer in a visual odometry framework. Furthermore, a comparison to an analogical ICP variant may yield more insight on the true performance of the proposed method.

\bibliographystyle{spiebib}
\bibliography{../../Literature}

\end{document}
