%\documentclass[9pt, compress]{beamer}
\documentclass[10pt,compress]{beamer}

\usepackage[utf8]{inputenc}
\usepackage[spanish]{babel}

\usepackage{graphicx}
\usepackage{psfrag}
\usepackage{amssymb,amsmath,amsthm}
\usepackage[font=footnotesize]{subfig}
\usepackage{beamerthemesplit}

\definecolor{myblue}{RGB}{0,0,128}

\newcommand{\worktitle}{Fusion of Monocular Visual-Inertial Measurements for Three Dimensional Pose Estimation}
\newcommand{\w}{\omega}
\newcommand{\boldomega}{\boldsymbol{\omega}}
\newcommand{\boldtheta}{\boldsymbol{\theta}}

\hypersetup{%
pdftitle = {\worktitle},
pdfsubject = {},
pdfkeywords = {Sensor fusion, Visual odometry, Inertial sensors, Pose estimation, UAV, Kalman filter},
pdfauthor = {Gonzalo Perez-Paina},
pdfcreator = {\LaTeX},
colorlinks = {false},
}

\mode<presentation> {
% Tema seleccionado (Antibes, Copenhagen, Darmstadt, Dresden, Frankfurt, Luebeck, Madrid, shadow, Warsaw, sidebar, PaloAlto)
   \usetheme{Boadilla}
%   \usetheme{Warsaw}
%   \usetheme{Frankfurt}
%   \usetheme{Malmoe}
%   \usetheme{Luebeck}
%   \usetheme{CambridgeUS}
%   \usetheme{Copenhagen}
% Color del tema, (albatross, seahorse, beetle, crane, dolphin, dove, fly, lily, orchid, rose, seagull, whale)
%\usecolortheme{rose}              
   \usefonttheme{serif}
%   \usefonttheme{professionalfonts}
   \setbeamercovered{invisible}          % transparent-invisible
}

\setbeamertemplate{navigation symbols}{}  %remove navigation symbols
%\setbeamercovered{transparent}
%\setbeamertemplate{itemize item}[rectangle]

\usefonttheme{professionalfonts}
\let\Tiny=\tiny

\title[MESAS 2016]{\worktitle}
%\subtitle{} % Opcional
\author[Miroslav Kulich]{Gonzalo Perez-Paina, Claudio Paz, Miroslav Kulich, Martin Saska, Gast\'{o}n Aragu\'{a}s}
\date[]{
  \vspace{1.50cm}
  \includegraphics[height=0.3\textheight]{figs/mesas2016.eps}
}

\setbeamercovered{invisible}
\addtobeamertemplate{block begin}{\setlength\abovedisplayskip{0pt}}

\begin{document}
%\frame{\titlepage}
%{
%\begin{frame}[plain]
%  \titlepage
%\end{frame}
%}

\begin{frame}[plain,noframenumbering]
  \begin{center}
%    {\color{myblue}{\Large \worktitle}}
    {\color{myblue}{\Large Fusion of Monocular Visual-Inertial Measurements}}\\\smallskip
    {\color{myblue}{\Large for Three Dimensional Pose estimation}}

    \vspace{0.50cm}
    Gonzalo Perez-Paina, Claudio Paz, \textbf{Miroslav Kulich}\\Martin Saska, Gast\'{o}n Aragu\'{a}s

    \vspace{0.50cm}
    \includegraphics[height=5\baselineskip]{figs/mesas2016.eps}

    \vspace{0.50cm}
    {\footnotesize Center for IT Research, National Technological University, Córdoba Branch, Argentina}\\\smallskip
    {\footnotesize Czech Institute of Informatics, Robotics, and Cybernetics, Czech Technical University in Prague, Czech Republic}\\\smallskip
    {\footnotesize Department of Cybernetics, Faculty of Electrical Engineering, Czech Technical University in Prague, Prague, Czech Republic}
  \end{center}
\end{frame}

\begin{frame}[plain]
	\frametitle{Outline}
  \vspace{0cm}
  \tableofcontents[hidesubsections]
\end{frame}

\section{Introduction and Objectives}
\subsection{Flying robots}
\begin{frame}[t]
  \frametitle{\secname}
  \framesubtitle{\subsecname}

  \begin{columns}
    \begin{column}{0.3\linewidth}
      \centering
      \includegraphics[height=0.30\textheight]{figs/fixed-wing.eps}\\
      Fixed-wing airplanes
    \end{column}
    \hspace{6mm}
    \begin{column}{0.3\linewidth}
      \centering
      \includegraphics[height=0.30\textheight]{figs/helicopter.eps}\\
      Helicopters
    \end{column}
    \begin{column}{0.3\linewidth}
      \centering
      \includegraphics[height=0.30\textheight]{figs/quad.eps}\\
      Quadrotors
    \end{column}
  \end{columns}

  \vspace{0.50cm}
  \begin{block}{Quadrotors are in the focus of interest because \dots }
    \begin{itemize}
      \item are inexpensive
      \item are easy to build and to maintain
      \item are easy to control 
      \item can keep position
      \item \alert{are appropriate for autonomous flight}
    \end{itemize}
  \end{block}
\end{frame}

\subsection{Autonomous flight}
\begin{frame}
  \frametitle{\secname}
  \framesubtitle{\subsecname}

  \begin{block}{Low level control}
    \begin{itemize}
      \item maintain attitude, stabilize
      \item disturbance compensation
    \end{itemize}
  \end{block}

  \vspace{0.50cm}
  \begin{block}{High level control}
    \begin{itemize}
      \item drift compensation
      \item obstacle avoidance
      \item localization and mapping
      \item navigation to a point
      \item \dots
    \end{itemize}
  \end{block}
\end{frame}

\subsection{Common sensors}
\begin{frame}
  \frametitle{\secname}
  \framesubtitle{\subsecname}

  {\centering
    \psfrag{se1}[Bc][Bc]{\small Inertial sensors}
    \psfrag{se2}[Bc][Bc]{\small Camera}
    \psfrag{se3}[Bc][Bc]{\small Sonar}
    \psfrag{se4}[Bc][Bc]{\small Magnetic compass}
    \psfrag{se5}[Bc][Bc]{\small GPS}
    \includegraphics<1-2>[width=1.0\textwidth]{figs/sensors1.eps}
    \includegraphics<3->[width=1.0\textwidth]{figs/sensors2.eps}
  }
  \begin{alertblock}{Problems}
    \begin{itemize}
      \item sensors are noisy
      \item measurements can be partial
      \item potentially missing
      \item \uncover<2->{\alert{sensor fusion is required}}
    \end{itemize}
  \end{alertblock}

  \begin{block}<3->{Challenge}
    Improve the pose estimation for indoor and outdoor environments
  \end{block}
\end{frame}

\section{System Overview}
\subsection{}
\begin{frame}
  \frametitle{\secname}
%  \framesubtitle{\subsecname}
  \begin{itemize}
    \item  Estimation of the position and orientation of a UAV using inertial sensors, a monocular camera, and an altimeter.
    \item  The visual algorithm uses the so called spectral features, and is based on the plane-induced homography when observing a flat floor.
  \end{itemize}

  \pause\medskip
  \centering\footnotesize
  \psfrag{i}[][]{\shortstack{Image\\frames}}
  \psfrag{v}[][]{\shortstack{Visual\\algorithms}}
  \psfrag{a}[][]{Accelerometer}
  \psfrag{g}[][]{Gyroscope}
  \psfrag{s}[][]{Altimeter}
  \psfrag{1}[][]{\shortstack{Previous\\estimation}}
  \psfrag{2}[][]{\shortstack{Current\\estimation}}
  \psfrag{p}[][]{\shortstack{Kalman filter\\prediction}}
  \psfrag{u1}[][]{\shortstack{Kalman filter\\update 1}}
  \psfrag{u2}[][]{\shortstack{Kalman filter\\update 2}}
  \includegraphics[height=0.70\textheight]{figs/Fig1.eps}
\end{frame}

\section{Sensor Fusion}
\subsection{State vector and inertial models}
\begin{frame}
  \frametitle{\secname}
  \framesubtitle{\subsecname}

  \begin{itemize}
    \item  State vector and error
      \[
        \mathbf{x} = \left[
          \begin{array}{c}
            \mathbf{p} \\ \mathbf{v} \\ \mathbf{q}
          \end{array}
          \right], \quad \delta\mathbf{x} = \left[
          \begin{array}{c}
            \delta\mathbf{p} \\ \delta\mathbf{v} \\ \delta\boldtheta
          \end{array}
          \right]
      \]
    \item  Inertial sensors (input signal)
      \[
        \mathbf{u} = \left[
        \begin{array}{c}
          \mathbf{a}_m \\ \boldomega_m
        \end{array}
        \right], \quad \left\{
        \begin{array}{l}
          \mathbf{a}_m = \mathbf{a} + \mathbf{w}_a\\
          \boldomega_m = \boldomega + \mathbf{w}_\w
        \end{array}
        \right.
      \]
    \item  Process model, $\dot{\mathbf{x}} = f(\mathbf{x}, \mathbf{u})$
      \begin{align*}
        \dot{\mathbf{p}} &= \mathbf{v}\\
        \dot{\mathbf{v}} &= \mathbf{R}(\mathbf{q})\mathbf{a} + \mathbf{g}^n\\
        \dot{\mathbf{q}} &= \frac{1}{2}\mathbf{q} \otimes \left[
          \begin{array}{c}
            0 \\ \boldomega
          \end{array}
          \right]
        \end{align*}
    \end{itemize}
\end{frame}

\subsection{Measurement models}
\begin{frame}
  \frametitle{\secname}
  \framesubtitle{\subsecname}

  \begin{itemize}
    \item  Camera measurement model, ${}^c \mathbf{z} = h_c(\mathbf{x})$
      \begin{align*}
        {}^c \hat{\mathbf{z}}_k^- = \left[
          \begin{array}{c}
            {}^c \hat{\mathbf{p}}_k^- \\
            {}^c \hat{\mathbf{q}}_k^-
          \end{array} 
       \right]
      \end{align*}
      (relative position and orientation between consecutive frames)\bigskip
    \item  Altimeter measurement model, ${}^a z = h_a(\mathbf{x}) = p_z$\medskip
      \begin{center}
        \psfrag{h}[][]{$p_z$}
        \psfrag{s}[][]{$s$}
        \psfrag{np}[][]{$\mathbf{n}_f$}
        \psfrag{nq}[][]{$\mathbf{n}_q$}
        \psfrag{p0}[][]{$\mathbf{p}_f$}
        \psfrag{p1}[][]{$\mathbf{p}$}
        \psfrag{p2}[][]{$\mathbf{p}_1$}
        \includegraphics[width=0.40\textwidth]{figs/Fig3.eps}
      \end{center}
  \end{itemize}
\end{frame}


\section{Visual Algorithm}
\subsection{Homography-based pose estimation}
\begin{frame}
  \frametitle{\secname}
  \framesubtitle{\subsecname}

  \begin{block}{Considerations}
    \begin{itemize}
      \item two images of a planar scene are related by a homography
      \item a downward-looking camera observes the floor (assumed flat)
      \item the homography encodes the spatial transformation of the camera
    \end{itemize}
  \end{block}

  \pause\medskip
  {\color{myblue} Corresponding points}

  \begin{center}
    \includegraphics[width=0.80\linewidth]{figs/homog_based_patches.eps}
  \end{center}

  \begin{align*}
    \{ \mathbf{x}_{A_i} \leftrightarrow \mathbf{x}_{A_i} + \Delta \mathbf{d}_i  =  \mathbf{x}_{B_i} \}
  \end{align*}
\end{frame}

\subsection{Spectral Features}
\begin{frame}
  \frametitle{\secname}
  \framesubtitle{\subsecname}

  \centering
  \includegraphics[width=0.60\linewidth]{figs/patch_disp.eps}
  
  \medskip
  \begin{align}
    \{ \mathbf{x}_{A_i} \leftrightarrow \mathbf{x}_{A_i} + \Delta \mathbf{d}_i  =  \mathbf{x}_{B_i} \}
  \end{align}
\end{frame}


\section{Results}
\subsection{Synthetic Dataset}
\begin{frame}
  \frametitle{\secname}
  \framesubtitle{\subsecname}

    Based on a dynamic simulation of a quadcopter, which allows to obtain the ground truth position and orientation, together with the ideal inertial sensor measurements.

  \begin{columns}
    \begin{column}{0.40\textwidth}
      \centering
      \psfrag{px}[][]{\tiny $p_x$}
      \psfrag{py}[][]{\tiny $p_y$}
      \psfrag{pz}[][]{\tiny $p_z$}
      \includegraphics[height=9\baselineskip]{figs/Fig4a.eps}
    \end{column}
    \begin{column}{0.60\textwidth}
      \psfrag{x}[][]{\tiny $a_x$}
      \psfrag{y}[][]{\tiny $a_y$}
      \psfrag{z}[][]{\tiny $a_z$}
      \psfrag{t}[tc][bc]{\tiny $t[s]$}
      \psfrag{p}[][]{\tiny $\w_x$}
      \psfrag{q}[][]{\tiny $\w_y$}
      \psfrag{r}[][]{\tiny $\w_z$}
      \includegraphics[height=9\baselineskip]{figs/Fig4b.eps}
    \end{column}
  \end{columns}

  \pause\bigskip
  \begin{columns}
    \begin{column}{0.33\textwidth}
      \centering
      \includegraphics[height=5\baselineskip]{figs/Fig5a.eps}
    \end{column}
    \begin{column}{0.33\textwidth}
      \centering
      \includegraphics[height=5\baselineskip]{figs/Fig5b.eps}
    \end{column}
    \begin{column}{0.33\textwidth}
      \centering
      \includegraphics[height=5\baselineskip]{figs/Fig5c.eps}
    \end{column}
  \end{columns}
\end{frame}

\subsection{Estimated position, velocity and orientation}
\begin{frame}
  \frametitle{\secname}
  \framesubtitle{\subsecname}

  \centering
  \psfrag{t}[][]{\tiny $t[s]$}
  \psfrag{x}[][]{\tiny $p_x$}
  \psfrag{y}[][]{\tiny $p_y$}
  \psfrag{z}[][]{\tiny $p_z$}
  \psfrag{vx}[][]{\tiny $v_x$}
  \psfrag{vy}[][]{\tiny $v_y$}
  \psfrag{vz}[][]{\tiny $v_z$}
  \psfrag{a}[][]{\tiny $\phi$}
  \psfrag{b}[][]{\tiny $\theta$}
  \psfrag{c}[][]{\tiny $\psi$}
  \psfrag{aaaaaaaaaaaaaaa}[Br][cr]{\scalebox{0.7}{\tiny Integration}}
  \psfrag{bbbbbbbbbbbbbbb}[Br][cr]{\scalebox{0.7}{\tiny Ground truth}}
  \psfrag{ddddddddddddddd}[Br][cr]{\scalebox{0.7}{\tiny Estimation}}
  \includegraphics[height=15\baselineskip]{figs/Fig6.eps}
\end{frame}

\subsection{Estimated orientation (unit quaternion)}
\begin{frame}
  \frametitle{\secname}
  \framesubtitle{\subsecname}

  \centering
  \psfrag{t}[][]{\tiny $t[s]$}
  \psfrag{qw}[][]{\tiny $q_w$}
  \psfrag{qx}[][]{\tiny $q_x$}
  \psfrag{qy}[][]{\tiny $q_y$}
  \psfrag{qz}[][]{\tiny $q_z$}
  \psfrag{aaaaaaaaaaaaaaa}[Br][cr]{\scalebox{0.7}{\tiny Integration}}
  \psfrag{bbbbbbbbbbbbbbb}[Br][cr]{\scalebox{0.7}{\tiny Ground truth}}
  \psfrag{ddddddddddddddd}[Br][cr]{\scalebox{0.7}{\tiny Estimation}}
  \includegraphics[height=15\baselineskip]{figs/Fig7.eps}
\end{frame}

\subsection{Estimation error}
\begin{frame}
  \frametitle{\secname}
  \framesubtitle{\subsecname}

  \centering
  \psfrag{t}[][]{\tiny $t[s]$}
  \psfrag{x}[][]{\tiny $\delta p_x$}
  \psfrag{y}[][]{\tiny $\delta p_y$}
  \psfrag{z}[][]{\tiny $\delta p_z$}
  \psfrag{vx}[][]{\tiny $\delta v_x$}
  \psfrag{vy}[][]{\tiny $\delta v_y$}
  \psfrag{vz}[][]{\tiny $\delta v_z$}
  \psfrag{a}[][]{\tiny $\delta\phi$}
  \psfrag{b}[][]{\tiny $\delta\theta$}
  \psfrag{c}[][]{\tiny $\delta\psi$}
  \includegraphics[height=15\baselineskip]{figs/Fig8.eps}
\end{frame}

\subsection{IMU measurements with constant bias}
\begin{frame}
  \frametitle{\secname}
  \framesubtitle{\subsecname}

  \centering
  \psfrag{x}[][]{\tiny $a_x$}
  \psfrag{y}[][]{\tiny $a_y$}
  \psfrag{z}[][]{\tiny $a_z$}
  \psfrag{t}[tc][bc]{\tiny $t[s]$}
  \psfrag{p}[][]{\tiny $\w_x$}
  \psfrag{q}[][]{\tiny $\w_y$}
  \psfrag{r}[][]{\tiny $\w_z$}
  \psfrag{n}[][]{\tiny noisy}
  \psfrag{g}[][]{\tiny noisy}
  \includegraphics[height=15\baselineskip]{figs/Fig9.eps}
\end{frame}

\subsection{Estimation error}
\begin{frame}
  \frametitle{\secname}
  \framesubtitle{\subsecname}

  \centering
  \psfrag{t}[][]{\tiny $t[s]$}
  \psfrag{x}[][]{\tiny $\delta p_x$}
  \psfrag{y}[][]{\tiny $\delta p_y$}
  \psfrag{z}[][]{\tiny $\delta p_z$}
  \psfrag{vx}[][]{\tiny $\delta v_x$}
  \psfrag{vy}[][]{\tiny $\delta v_y$}
  \psfrag{vz}[][]{\tiny $\delta v_z$}
  \psfrag{a}[][]{\tiny $\delta\phi$}
  \psfrag{b}[][]{\tiny $\delta\theta$}
  \psfrag{c}[][]{\tiny $\delta\psi$}
  \psfrag{d}[cl][cl]{\scalebox{0.7}{\tiny No bias}}
  \psfrag{e}[cl][cl]{\scalebox{0.7}{\tiny 25\% of bias}}
  \psfrag{f}[cl][cl]{\scalebox{0.7}{\tiny 50\% of bias}}
  \includegraphics[height=15\baselineskip]{figs/Fig10.eps}
\end{frame}

\section{Conclusion and Future Work}
\begin{frame}
  \frametitle{\secname}

  \begin{itemize}
    \item  The three dimensional pose estimation has a precision that seems promising to be applied in UAV on-board algorithms for control purposes.
    \item  A not precise bias correction, as high as 50\% of the real bias, does not affect pose estimation considerably, specially for short time flights.
    \item  Test the proposed fusion schema with a real UAV, in order to evaluate how the estimation is affected by real sensor noise and unmodeled sensor bias.
    \item  Formulate a new filter model incorporating the sensor biases in the state vector in order to be estimated.
    \item  Compare the proposed visual algorithm with other similar approaches, particularly those that does not use corner-like features.
  \end{itemize}
\end{frame}

\begin{frame}
  \begin{center}
    {\color{myblue}{\Large Fusion of Monocular Visual-Inertial Measurements}}\\\smallskip
    {\color{myblue}{\Large for Three Dimensional Pose estimation}}

    \vspace{0.50cm}
    Gonzalo Perez-Paina, Claudio Paz, Miroslav Kulich\\Martin Saska, Gast\'{o}n Aragu\'{a}s
  \end{center}

  \vspace{0.50cm}
  \begin{columns}
    \begin{column}{0.50\textwidth}
      \noindent\textbf{Gonzalo Perez-Paina}\\
      \noindent\texttt{\small gperez@frc.utn.edu.ar}\\\vspace{0.30cm}
      \noindent\textbf{Miroslav Kulich}\\
      \noindent\texttt{\small kulich@ciirc.cvut.cz}
    \end{column}
    \begin{column}{0.30\textwidth}
    \end{column}
  \end{columns}

  \vspace{0.50cm}
  \begin{center}
    {\large Thanks for your attention.}
  \end{center}

  \begin{columns}
    \begin{column}{0.70\textwidth}
    \end{column}
    \begin{column}{0.30\textwidth}
      \includegraphics[height=3\baselineskip]{figs/mesas2016.eps}
    \end{column}
  \end{columns}
\end{frame}

\end{document}

%\begin{columns}
%  \begin{column}{0.48\textwidth}
%  \end{column}
%  \begin{column}{0.48\textwidth}
%  \end{column}
%\end{columns}

