Motion Planning Strategies for \\Autonomously Mapping 3D Structures
Manikandasriram~Srinivasan~Ramanagopal
~\IEEEmembership{Student~Member
~IEEE
....lastname \thanks{...
This paper presents a system capable of autonomously mapping the visible part of a bounded three-dimensional structure using a mobile ground robot equipped with a depth sensor. We describe motion planning strategies to determine appropriate successive viewpoints and attempt to fill holes automatically in a point cloud produced by the sensing and perception layer. We develop a local motion planner using potential fields to maintain a desired distance from the structure. The emphasis is on accurately reconstructing a 3D model of a structure of moderate size rather than mapping large open environments, with applications for example in architecture, construction and inspection. The proposed algorithms do not require any initialization in the form of a mesh model or a bounding box. We compare via simulations the performance of our policies to the classic frontier based exploration algorithm. We illustrate the efficacy of our approach for different structure sizes, levels of localization accuracy and range of the depth sensor. Note to Practitioners--- The objective of this work is to automate the process of building a 3D model of a structure of interest that is as complete as possible, using a mobile depth sensor, in the absence of any prior information about this structure. Given that several robust solutions for the Visual Simultaneous Localization and Mapping problem (vSLAM) are readily available, the key challenge that we address here is to develop motion planning policies to control the trajectory of the sensor. We present a system that is capable of autonomously determining the boundaries of the structure, before attempting to fill the holes in the constructed model. The performance of our system is illustrated through simulations performed with a depth sensor carried by a mobile manipulator.
main.tex
%% bare_jrnl.tex
%% V1.3
%% 2007/01/11
%% by Michael Shell
%% see http://www.michaelshell.org/
%% for current contact information.
%%
%% This is a skeleton file demonstrating the use of IEEEtran.cls
%% (requires IEEEtran.cls version 1.7 or later) with an IEEE journal paper.
%%
%% Support sites:
%% http://www.michaelshell.org/tex/ieeetran/
%% http://www.ctan.org/tex-archive/macros/latex/contrib/IEEEtran/
%% and
%% http://www.ieee.org/



% *** Authors should verify (and, if needed, correct) their LaTeX system  ***
% *** with the testflow diagnostic prior to trusting their LaTeX platform ***
% *** with production work. IEEE's font choices can trigger bugs that do  ***
% *** not appear when using other class files.                            ***
% The testflow support page is at:
% http://www.michaelshell.org/tex/testflow/


%%*************************************************************************
%% Legal Notice:
%% This code is offered as-is without any warranty either expressed or
%% implied; without even the implied warranty of MERCHANTABILITY or
%% FITNESS FOR A PARTICULAR PURPOSE! 
%% User assumes all risk.
%% In no event shall IEEE or any contributor to this code be liable for
%% any damages or losses, including, but not limited to, incidental,
%% consequential, or any other damages, resulting from the use or misuse
%% of any information contained here.
%%
%% All comments are the opinions of their respective authors and are not
%% necessarily endorsed by the IEEE.
%%
%% This work is distributed under the LaTeX Project Public License (LPPL)
%% ( http://www.latex-project.org/ ) version 1.3, and may be freely used,
%% distributed and modified. A copy of the LPPL, version 1.3, is included
%% in the base LaTeX documentation of all distributions of LaTeX released
%% 2003/12/01 or later.
%% Retain all contribution notices and credits.
%% ** Modified files should be clearly indicated as such, including  **
%% ** renaming them and changing author support contact information. **
%%
%% File list of work: IEEEtran.cls, IEEEtran_HOWTO.pdf, bare_adv.tex,
%%                    bare_conf.tex, bare_jrnl.tex, bare_jrnl_compsoc.tex
%%*************************************************************************

% Note that the a4paper option is mainly intended so that authors in
% countries using A4 can easily print to A4 and see how their papers will
% look in print - the typesetting of the document will not typically be
% affected with changes in paper size (but the bottom and side margins will).
% Use the testflow package mentioned above to verify correct handling of
% both paper sizes by the user's LaTeX system.
%
% Also note that the "draftcls" or "draftclsnofoot", not "draft", option
% should be used if it is desired that the figures are to be displayed in
% draft mode.
%
\documentclass[journal]{IEEEtran}
%
% If IEEEtran.cls has not been installed into the LaTeX system files,
% manually specify the path to it like:
% \documentclass[journal]{../sty/IEEEtran}





% Some very useful LaTeX packages include:
% (uncomment the ones you want to load)


% *** MISC UTILITY PACKAGES ***
%
%\usepackage{ifpdf}
% Heiko Oberdiek's ifpdf.sty is very useful if you need conditional
% compilation based on whether the output is pdf or dvi.
% usage:
% \ifpdf
%   % pdf code
% \else
%   % dvi code
% \fi
% The latest version of ifpdf.sty can be obtained from:
% http://www.ctan.org/tex-archive/macros/latex/contrib/oberdiek/
% Also, note that IEEEtran.cls V1.7 and later provides a builtin
% \ifCLASSINFOpdf conditional that works the same way.
% When switching from latex to pdflatex and vice-versa, the compiler may
% have to be run twice to clear warning/error messages.



\usepackage{flushend}  % to balance the last page


% *** CITATION PACKAGES ***
%
\usepackage{cite}
% cite.sty was written by Donald Arseneau
% V1.6 and later of IEEEtran pre-defines the format of the cite.sty package
% \cite{} output to follow that of IEEE. Loading the cite package will
% result in citation numbers being automatically sorted and properly
% "compressed/ranged". e.g., [1], [9], [2], [7], [5], [6] without using
% cite.sty will become [1], [2], [5]--[7], [9] using cite.sty. cite.sty's
% \cite will automatically add leading space, if needed. Use cite.sty's
% noadjust option (cite.sty V3.8 and later) if you want to turn this off.
% cite.sty is already installed on most LaTeX systems. Be sure and use
% version 4.0 (2003-05-27) and later if using hyperref.sty. cite.sty does
% not currently provide for hyperlinked citations.
% The latest version can be obtained at:
% http://www.ctan.org/tex-archive/macros/latex/contrib/cite/
% The documentation is contained in the cite.sty file itself.






% *** GRAPHICS RELATED PACKAGES ***
%
\ifCLASSINFOpdf
  \usepackage[pdftex]{graphicx}
  % declare the path(s) where your graphic files are
  % \graphicspath{{../pdf/}{../jpeg/}}
  % and their extensions so you won't have to specify these with
  % every instance of \includegraphics
  \DeclareGraphicsExtensions{.pdf,.jpeg,.png}
\else
  % or other class option (dvipsone, dvipdf, if not using dvips). graphicx
  % will default to the driver specified in the system graphics.cfg if no
  % driver is specified.
  \usepackage[dvips]{graphicx}
  % declare the path(s) where your graphic files are
  % \graphicspath{{../eps/}}
  % and their extensions so you won't have to specify these with
  % every instance of \includegraphics
  \DeclareGraphicsExtensions{.eps}
\fi
% graphicx was written by David Carlisle and Sebastian Rahtz. It is
% required if you want graphics, photos, etc. graphicx.sty is already
% installed on most LaTeX systems. The latest version and documentation can
% be obtained at: 
% http://www.ctan.org/tex-archive/macros/latex/required/graphics/
% Another good source of documentation is "Using Imported Graphics in
% LaTeX2e" by Keith Reckdahl which can be found as epslatex.ps or
% epslatex.pdf at: http://www.ctan.org/tex-archive/info/
%
% latex, and pdflatex in dvi mode, support graphics in encapsulated
% postscript (.eps) format. pdflatex in pdf mode supports graphics
% in .pdf, .jpeg, .png and .mps (metapost) formats. Users should ensure
% that all non-photo figures use a vector format (.eps, .pdf, .mps) and
% not a bitmapped formats (.jpeg, .png). IEEE frowns on bitmapped formats
% which can result in "jaggedy"/blurry rendering of lines and letters as
% well as large increases in file sizes.
%
% You can find documentation about the pdfTeX application at:
% http://www.tug.org/applications/pdftex





% *** MATH PACKAGES ***
%
\usepackage[cmex10]{amsmath}
% A popular package from the American Mathematical Society that provides
% many useful and powerful commands for dealing with mathematics. If using
% it, be sure to load this package with the cmex10 option to ensure that
% only type 1 fonts will utilized at all point sizes. Without this option,
% it is possible that some math symbols, particularly those within
% footnotes, will be rendered in bitmap form which will result in a
% document that can not be IEEE Xplore compliant!
%
% Also, note that the amsmath package sets \interdisplaylinepenalty to 10000
% thus preventing page breaks from occurring within multiline equations. Use:
%\interdisplaylinepenalty=2500
% after loading amsmath to restore such page breaks as IEEEtran.cls normally
% does. amsmath.sty is already installed on most LaTeX systems. The latest
% version and documentation can be obtained at:
% http://www.ctan.org/tex-archive/macros/latex/required/amslatex/math/





% *** SPECIALIZED LIST PACKAGES ***
%
% \usepackage{algorithmic}
% algorithmic.sty was written by Peter Williams and Rogerio Brito.
% This package provides an algorithmic environment fo describing algorithms.
% You can use the algorithmic environment in-text or within a figure
% environment to provide for a floating algorithm. Do NOT use the algorithm
% floating environment provided by algorithm.sty (by the same authors) or
% algorithm2e.sty (by Christophe Fiorio) as IEEE does not use dedicated
% algorithm float types and packages that provide these will not provide
% correct IEEE style captions. The latest version and documentation of
% algorithmic.sty can be obtained at:
% http://www.ctan.org/tex-archive/macros/latex/contrib/algorithms/
% There is also a support site at:
% http://algorithms.berlios.de/index.html
% Also of interest may be the (relatively newer and more customizable)
% algorithmicx.sty package by Szasz Janos:
% http://www.ctan.org/tex-archive/macros/latex/contrib/algorithmicx/
\usepackage{algpseudocode}
%\usepackage{algorithmicx}
\usepackage[]{algorithm}



% *** ALIGNMENT PACKAGES ***
%
%\usepackage{array}
% Frank Mittelbach's and David Carlisle's array.sty patches and improves
% the standard LaTeX2e array and tabular environments to provide better
% appearance and additional user controls. As the default LaTeX2e table
% generation code is lacking to the point of almost being broken with
% respect to the quality of the end results, all users are strongly
% advised to use an enhanced (at the very least that provided by array.sty)
% set of table tools. array.sty is already installed on most systems. The
% latest version and documentation can be obtained at:
% http://www.ctan.org/tex-archive/macros/latex/required/tools/


%\usepackage{mdwmath}
%\usepackage{mdwtab}
% Also highly recommended is Mark Wooding's extremely powerful MDW tools,
% especially mdwmath.sty and mdwtab.sty which are used to format equations
% and tables, respectively. The MDWtools set is already installed on most
% LaTeX systems. The lastest version and documentation is available at:
% http://www.ctan.org/tex-archive/macros/latex/contrib/mdwtools/


% IEEEtran contains the IEEEeqnarray family of commands that can be used to
% generate multiline equations as well as matrices, tables, etc., of high
% quality.


%\usepackage{eqparbox}
% Also of notable interest is Scott Pakin's eqparbox package for creating
% (automatically sized) equal width boxes - aka "natural width parboxes".
% Available at:
% http://www.ctan.org/tex-archive/macros/latex/contrib/eqparbox/





% *** SUBFIGURE PACKAGES ***
% \usepackage[tight,footnotesize]{subfigure}
% subfigure.sty was written by Steven Douglas Cochran. This package makes it
% easy to put subfigures in your figures. e.g., "Figure 1a and 1b". For IEEE
% work, it is a good idea to load it with the tight package option to reduce
% the amount of white space around the subfigures. subfigure.sty is already
% installed on most LaTeX systems. The latest version and documentation can
% be obtained at:
% http://www.ctan.org/tex-archive/obsolete/macros/latex/contrib/subfigure/
% subfigure.sty has been superceeded by subfig.sty.



% \usepackage[caption=false]{caption}
% \usepackage[font=footnotesize,caption=false]{subfig}
% subfig.sty, also written by Steven Douglas Cochran, is the modern
% replacement for subfigure.sty. However, subfig.sty requires and
% automatically loads Axel Sommerfeldt's caption.sty which will override
% IEEEtran.cls handling of captions and this will result in nonIEEE style
% figure/table captions. To prevent this problem, be sure and preload
% caption.sty with its "caption=false" package option. This is will preserve
% IEEEtran.cls handing of captions. Version 1.3 (2005/06/28) and later 
% (recommended due to many improvements over 1.2) of subfig.sty supports
% the caption=false option directly:
\usepackage[caption=false,font=footnotesize]{subfig}
%
% The latest version and documentation can be obtained at:
% http://www.ctan.org/tex-archive/macros/latex/contrib/subfig/
% The latest version and documentation of caption.sty can be obtained at:
% http://www.ctan.org/tex-archive/macros/latex/contrib/caption/




% *** FLOAT PACKAGES ***
%
%\usepackage{fixltx2e}
% fixltx2e, the successor to the earlier fix2col.sty, was written by
% Frank Mittelbach and David Carlisle. This package corrects a few problems
% in the LaTeX2e kernel, the most notable of which is that in current
% LaTeX2e releases, the ordering of single and double column floats is not
% guaranteed to be preserved. Thus, an unpatched LaTeX2e can allow a
% single column figure to be placed prior to an earlier double column
% figure. The latest version and documentation can be found at:
% http://www.ctan.org/tex-archive/macros/latex/base/



%\usepackage{stfloats}
% stfloats.sty was written by Sigitas Tolusis. This package gives LaTeX2e
% the ability to do double column floats at the bottom of the page as well
% as the top. (e.g., "\begin{figure*}[!b]" is not normally possible in
% LaTeX2e). It also provides a command:
%\fnbelowfloat
% to enable the placement of footnotes below bottom floats (the standard
% LaTeX2e kernel puts them above bottom floats). This is an invasive package
% which rewrites many portions of the LaTeX2e float routines. It may not work
% with other packages that modify the LaTeX2e float routines. The latest
% version and documentation can be obtained at:
% http://www.ctan.org/tex-archive/macros/latex/contrib/sttools/
% Documentation is contained in the stfloats.sty comments as well as in the
% presfull.pdf file. Do not use the stfloats baselinefloat ability as IEEE
% does not allow \baselineskip to stretch. Authors submitting work to the
% IEEE should note that IEEE rarely uses double column equations and
% that authors should try to avoid such use. Do not be tempted to use the
% cuted.sty or midfloat.sty packages (also by Sigitas Tolusis) as IEEE does
% not format its papers in such ways.


%\ifCLASSOPTIONcaptionsoff
%  \usepackage[nomarkers]{endfloat}
% \let\MYoriglatexcaption\caption
% \renewcommand{\caption}[2][\relax]{\MYoriglatexcaption[#2]{#2}}
%\fi
% endfloat.sty was written by James Darrell McCauley and Jeff Goldberg.
% This package may be useful when used in conjunction with IEEEtran.cls'
% captionsoff option. Some IEEE journals/societies require that submissions
% have lists of figures/tables at the end of the paper and that
% figures/tables without any captions are placed on a page by themselves at
% the end of the document. If needed, the draftcls IEEEtran class option or
% \CLASSINPUTbaselinestretch interface can be used to increase the line
% spacing as well. Be sure and use the nomarkers option of endfloat to
% prevent endfloat from "marking" where the figures would have been placed
% in the text. The two hack lines of code above are a slight modification of
% that suggested by in the endfloat docs (section 8.3.1) to ensure that
% the full captions always appear in the list of figures/tables - even if
% the user used the short optional argument of \caption[]{}.
% IEEE papers do not typically make use of \caption[]'s optional argument,
% so this should not be an issue. A similar trick can be used to disable
% captions of packages such as subfig.sty that lack options to turn off
% the subcaptions:
% For subfig.sty:
% \let\MYorigsubfloat\subfloat
% \renewcommand{\subfloat}[2][\relax]{\MYorigsubfloat[]{#2}}
% For subfigure.sty:
% \let\MYorigsubfigure\subfigure
% \renewcommand{\subfigure}[2][\relax]{\MYorigsubfigure[]{#2}}
% However, the above trick will not work if both optional arguments of
% the \subfloat/subfig command are used. Furthermore, there needs to be a
% description of each subfigure *somewhere* and endfloat does not add
% subfigure captions to its list of figures. Thus, the best approach is to
% avoid the use of subfigure captions (many IEEE journals avoid them anyway)
% and instead reference/explain all the subfigures within the main caption.
% The latest version of endfloat.sty and its documentation can obtained at:
% http://www.ctan.org/tex-archive/macros/latex/contrib/endfloat/
%
% The IEEEtran \ifCLASSOPTIONcaptionsoff conditional can also be used
% later in the document, say, to conditionally put the References on a 
% page by themselves.





% *** PDF, URL AND HYPERLINK PACKAGES ***
%
%\usepackage{url}
% url.sty was written by Donald Arseneau. It provides better support for
% handling and breaking URLs. url.sty is already installed on most LaTeX
% systems. The latest version can be obtained at:
% http://www.ctan.org/tex-archive/macros/latex/contrib/misc/
% Read the url.sty source comments for usage information. Basically,
% \url{my_url_here}.





% *** Do not adjust lengths that control margins, column widths, etc. ***
% *** Do not use packages that alter fonts (such as pslatex).         ***
% There should be no need to do such things with IEEEtran.cls V1.6 and later.
% (Unless specifically asked to do so by the journal or conference you plan
% to submit to, of course. )


% correct bad hyphenation here
\hyphenation{op-tical net-works semi-conduc-tor auto-nomously vSLAM	}

\newtheorem{thm}{Theorem}
\newtheorem{assum}{Assumption}
\newtheorem{init}{Initial Condition}
\newtheorem{defi}{Definition}

%%%%%%%%%%%%%%
% comments macros
\usepackage[normalem]{ulem}
\usepackage{color}
\usepackage[textwidth=1.3cm,textsize=footnotesize]{todonotes} 
\setlength{\marginparwidth}{1cm}
\newcommand{\jln}[1]{\textcolor{red}{#1}}
\newcommand{\jlnComment}[1]{
\todo[color={red}]{#1}
}
\newcommand{\mani}[1]{\textcolor{blue}{#1}}
\newcommand{\maniComment}[1]{
\todo[color={cyan}]{#1}
}
\usepackage{soul}
%%%%%%%%%%%%%%

\usepackage{multirow}

% typography for vectors, frames, etc.
\usepackage{amsfonts}
\newcommand{\fr}[1]{\mathsf{#1}}
\newcommand{\vect}[1]{\mathbf{#1}}


\begin{document}
%
% paper title
% can use linebreaks \\ within to get better formatting as desired
\title{Motion Planning Strategies for \\Autonomously Mapping 3D Structures}
%
%
% author names and IEEE memberships
% note positions of commas and nonbreaking spaces ( ~ ) LaTeX will not break
% a structure at a ~ so this keeps an author's name from being broken across
% two lines.
% use \thanks{} to gain access to the first footnote area
% a separate \thanks must be used for each paragraph as LaTeX2e's \thanks
% was not built to handle multiple paragraphs
%

\author{Manikandasriram~Srinivasan~Ramanagopal,~\IEEEmembership{Student~Member,~IEEE,}
and Jerome~Le~Ny,~\IEEEmembership{Member,~IEEE}% <-this % stops a space
%\thanks{Manuscript received April 19, 2005; revised January 11, 2007.}
% \thanks{\mani{This paper has supplementary downloadable material available at
% http://ieeexplore.ieee.org, provided by the authors. This includes an
% MP4 format video clip, which shows simulations of our policies implemented on
% a mobile manipulator carrying a depth sensor. This
% material is 48.2 MB in size.}}
\thanks{Part of this work was performed while the first author was visiting 
Polytechnique Montreal, under a Globalink Fellowship from MITACS.
This work was also supported by NSERC (Grant 435905-13) and the 
Canada Foundation for Innovation (Grant 32848).
}% <-this % stops a space
\thanks{M. S. R. is with the Department of Electrical Engineering, IIT Madras, 
Chennai, India. 
J. Le Ny is with the Department of Electrical Engineering, 
Polytechnique Montreal, and GERAD, Montreal, QC H3T 1J4, Canada.
e-mails: {srmanikandasriram@gmail.com, jerome.le-ny@polymtl.ca}.}% <-this % stops a space
}

% note the % following the last \IEEEmembership and also \thanks - 
% these prevent an unwanted space from occurring between the last author name
% and the end of the author line. i.e., if you had this:
% 
% \author{....lastname \thanks{...} \thanks{...} }
%                     ^------------^------------^----Do not want these spaces!
%
% a space would be appended to the last name and could cause every name on that
% line to be shifted left slightly. This is one of those "LaTeX things". For
% instance, "\textbf{A} \textbf{B}" will typeset as "A B" not "AB". To get
% "AB" then you have to do: "\textbf{A}\textbf{B}"
% \thanks is no different in this regard, so shield the last } of each \thanks
% that ends a line with a % and do not let a space in before the next \thanks.
% Spaces after \IEEEmembership other than the last one are OK (and needed) as
% you are supposed to have spaces between the names. For what it is worth,
% this is a minor point as most people would not even notice if the said evil
% space somehow managed to creep in.



% The paper headers
% \markboth{Submitted to the
% IEEE Transactions on Automation Science and Engineering}%,~Vol.~X, No.~X, January~XXXX}%
% {Ramanagopal \MakeLowercase{\textit{et al.}}: 
% Motion Planning Strategies for \\Autonomously Mapping 3D Structures}
% The only time the second header will appear is for the odd numbered pages
% after the title page when using the twoside option.
% 
% *** Note that you probably will NOT want to include the author's ***
% *** name in the headers of peer review papers.                   ***
% You can use \ifCLASSOPTIONpeerreview for conditional compilation here if
% you desire.




% If you want to put a publisher's ID mark on the page you can do it like
% this:
%\IEEEpubid{0000--0000/00\$00.00~\copyright~2007 IEEE}
% Remember, if you use this you must call \IEEEpubidadjcol in the second
% column for its text to clear the IEEEpubid mark.



% use for special paper notices
%\IEEEspecialpapernotice{(Invited Paper)}




% make the title area
\maketitle


% IEEEtran.cls defaults to using nonbold math in the Abstract.
% This preserves the distinction between vectors and scalars. However,
% if the journal you are submitting to favors bold math in the abstract,
% then you can use LaTeX's standard command \boldmath at the very start
% of the abstract to achieve this. Many IEEE journals frown on math
% in the abstract anyway.

\begin{abstract}
This paper presents a system capable of autonomously mapping 
the visible part of a bounded three-dimensional structure using 
a mobile ground robot equipped with a depth sensor.
We describe motion planning strategies to determine appropriate successive 
viewpoints and attempt to fill holes automatically in a point cloud 
produced by the sensing and perception layer. We develop a local motion planner
using potential fields to maintain a desired distance from the structure. The emphasis is on accurately 
reconstructing a 3D model of a structure of moderate size rather than mapping 
large open environments, with applications for example in architecture, 
construction and inspection. The proposed algorithms do not require any 
initialization in the form of a mesh model or a bounding box. 
We compare via simulations the performance of our policies 
to the classic frontier based exploration algorithm. We illustrate
the efficacy of our approach for different structure sizes, levels of 
localization accuracy and range of the depth sensor.

\emph{Note to Practitioners---}
The objective of this work is to automate the process of building a 3D model 
of a structure of interest that is as complete as possible, using a mobile 
depth sensor, in the absence of any prior information about this structure.
Given that several robust solutions for the Visual Simultaneous Localization 
and Mapping problem (vSLAM) are readily available, the key challenge that 
we address here is to develop motion planning policies to control the 
trajectory of the sensor. We present a system that is capable of autonomously
determining the boundaries of the structure, before attempting to fill
the holes in the constructed model. The performance of our system is illustrated through 
simulations performed with a depth sensor carried by a mobile manipulator.

\end{abstract}

%\\

\begin{IEEEkeywords}
Motion Planning, Active Sensing, Active SLAM, Autonomous Mapping, Autonomous Inspection
\end{IEEEkeywords}

% For peer review papers, you can put extra information on the cover
% page as needed:
% \ifCLASSOPTIONpeerreview
% \begin{center} \bfseries EDICS Category: 3-BBND \end{center}
% \fi
%
% For peerreview papers, this IEEEtran command inserts a page break and
% creates the second title. It will be ignored for other modes.
\IEEEpeerreviewmaketitle

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\input{texFiles/intro.tex}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\input{texFiles/statement.tex}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%\section{Method}
We divide our mapping process into two phases.
The first phase is the Perimeter Exploration (PE) phase, during which 
the robot moves clockwise around the structure to determine its boundaries.
The robot continuously moves towards previously unseen regions 
of the structure, with the exploration directed towards finding 
the limits of the structure rather than closely following its geometry. 
The PE phase ends when our algorithm detects that the robot has returned 
to the neighborhood of its starting point $\fr O$
% \jlnComment{I suggest changing the notation $\mathcal O$ used here
% and in III.D. Why not $\mathsf O$ as for the other points and you fig. 8? 
% Because in IV, $\mathcal O_i$ denotes a set.}
and the vSLAM module detects 
a global loop closure. After completing the PE phase, the system determines 
the locations of potential missing parts in the constructed 3D model. 
We can then start the second phase, which we call the Cavity Exploration (CE)
phase, during which the system explores these missing parts in the model.
The following subsections explain each step of our process in detail.

\input{texFiles/perimeterExplo.tex}
\input{texFiles/cavityExplo.tex}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\input{texFiles/simulations.tex}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


\section{Conclusion and Future Work}
This paper presents novel motion planning policies that guide
a mobile ground robot carrying a depth sensor to autonomously
explore the visible portion of a bounded three-dimensional 
structure. The proposed policies do not assume any prior
information about the size or geometry of the structure. 
Coupled with state-of-art vSLAM systems, our
strategies are able to achieve high coverage in the 
reconstructed model, given the physical limitations of the
platform. We have illustrated the efficacy of our approach
via 3D simulations for different structure sizes, camera range 
and localization accuracy. In addition, a comparison of our policies
with the classic frontier based exploration algorithms clearly
shows the improvement in performance for a realistic structure such
as a house.

% This is one of the first attempts aimed at autonomously mapping
% a \emph{specific} structure of interest as accurately and as completely
% as possible. 
This work opens interesting questions such as combined vehicle and 6-DOF 
arm motion planning to map fine structural details. 
Extending our algorithms to aerial platforms and hybrid teams of robots, 
for example, would also allow for autonomous inspection of tall structures 
such as wind turbines or telecom towers.
%power transmission lines.
%Summarize the work and highlight the contribution. Illustrate the real
%world applications and the expected impact of this work.  Briefly 
%talk about future work.
%\begin{itemize}
%\item This preliminary \jln{[do not say preliminary, you are shooting yourself
%in the foot]} work shows the feasibility of the proposed approach - 
%breaking down the problem into vSLAM + Motion Planning for determining 
%bounding boxes. Opened up multiple questions. 
%\item Designing the local planner for removing Type I flaws. 
%%\item Using CV techniques to automatically post process the images to get a more accurate model. 
%% Objective of an online SLAM system is different from these CV techniques. 
%% Hence, CV techniques are expected to give better performance. 
%\item For a large structure, multiple cavities at different locations could be present. 
%Hence optimal order of filling cavities, improving the model while moving to the starting 
%viewpoint of a cavity. 
%\item Aerial platforms to explore tall buildings such as power transmission lines.
%%\item reducing the annulus of free space requirement
%\end{itemize}



% if have a single appendix:
%\appendix[Proof of the Zonklar Equations]
% or
%\appendix  % for no appendix heading
% do not use \section anymore after \appendix, only \section*
% is possibly needed

% use appendices with more than one appendix
% then use \section to start each appendix
% you must declare a \section before using any
% \subsection or using \label (\appendices by itself
% starts a section numbered zero.)
%


%appendices
%section{Proof of the First Zonklar Equation}
% appendix one text goes here.

% you can choose not to have a title for an appendix
% if you want by leaving the argument blank
%section{}
% appendix two text goes here.


% use section* for acknowledgement
%\section*{Acknowledgment}
%The authors would like to thank...


% Can use something like this to put references on a page
% by themselves when using endfloat and the captionsoff option.
\ifCLASSOPTIONcaptionsoff
  \newpage
\fi



% trigger a \newpage just before the given reference
% number - used to balance the columns on the last page
% adjust value as needed - may need to be readjusted if
% the document is modified later
%\IEEEtriggeratref{8}
% The "triggered" command can be changed if desired:
%\IEEEtriggercmd{\enlargethispage{-5in}}

% references section

% can use a bibliography generated by BibTeX as a .bbl file
% BibTeX documentation can be easily obtained at:
% http://www.ctan.org/tex-archive/biblio/bibtex/contrib/doc/
% The IEEEtran BibTeX style support page is at:
% http://www.michaelshell.org/tex/ieeetran/bibtex/
\bibliographystyle{IEEEtran}
% argument is your BibTeX string definitions and bibliography database(s)
\bibliography{IEEEabrv,biblio}
%
% <OR> manually copy in the resultant .bbl file
% set second argument of \begin to the number of references
% (used to reserve space for the reference number labels box)
%\begin{thebibliography}{1}
%
%\bibitem{IEEEhowto:kopka}
%H.~Kopka and P.~W. Daly, \emph{A Guide to \LaTeX}, 3rd~ed.\hskip 1em plus
%  0.5em minus 0.4em\relax Harlow, England: Addison-Wesley, 1999.
%
%\end{thebibliography}

% biography section
% 
% If you have an EPS/PDF photo (graphicx package needed) extra braces are
% needed around the contents of the optional argument to biography to prevent
% the LaTeX parser from getting confused when it sees the complicated
% \includegraphics command within an optional argument. (You could create
% your own custom macro containing the \includegraphics command to make things
% simpler here.)
%\begin{biography}[{\includegraphics[width=1in,height=1.25in,clip,keepaspectratio]{mshell}}]{Michael Shell}
% or if you just want to reserve a space for a photo:


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Commented by Jerome
% \begin{IEEEbiography}{Michael Shell}
% Biography text here.
% \end{IEEEbiography}

% % if you will not have a photo at all:
% \begin{IEEEbiographynophoto}{John Doe}
% Biography text here.
% \end{IEEEbiographynophoto}

% % insert where needed to balance the two columns on the last page with
% % biographies
% %\newpage

% \begin{IEEEbiographynophoto}{Jane Doe}
% Biography text here.
% \end{IEEEbiographynophoto}

% You can push biographies down or up by placing
% a \vfill before or after them. The appropriate
% use of \vfill depends on what kind of text is
% on the last page and whether or not the columns
% are being equalized.

%\vfill

% Can be used to pull up biographies so that the bottom of the last one
% is flush with the other column.
%\enlargethispage{-5in}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% that's all folks
\end{document}
cavityExplo.tex
% !TEX root =  ../main.tex

\section{Completing the Model}

\subsection{Determining flaws}

There are two possible types of flaws in the model obtained at the end of the PE phase. 
Type I flaws correspond to holes that are present in the already explored regions. 
As noted in Section II, these holes could be due to limitations of the sensor or local occlusions 
caused by small irregularities in the structure itself, %\maniComment{or complex structure?,
and should be filled using a platform with 
a more appropriate reachable space, hence we do not consider them further. 
%As noted in Section II, these flaws can be resolved by augmenting our system 
%with a more local planner. 
%\mani{[Why can't these local planners be used for detecting and filling cavities as well? These techniques assume a bounding box for the object. But the size of the cavity is unknown.]}
Type II flaws, called cavities in the following, correspond to regions that were missed 
during the PE phase, e.g., due to the situation depicted on Fig. \ref{fig:exploration},
and will be filled during the CE phase.
%At the end of the PE phase, 
%the past trajectory of the robot provides an 
%upper bound on the size of the structure. 
%The path followed by the robot so far forms a curve enclosing the structure
%\jln{in the $XY$ plane, which together with the planes $Z=0$ and $Z=H_{\max}$
%defines a cylinder containing the part of the structure to map}.
%Thus, any missing parts in the constructed model must 
%be present within this bounded finite volume. 
%lie within this cylinder.
%The assembled point cloud from the vSLAM module depicts only the occupied space.
Our exploration policies only need the location of the entrances of the cavities and in this subsection we 
% provide a definition of cavities and 
describe an algorithm to determine these locations 
% of the entrance to these cavities 
in the model.

% \jln{[The rest of this section is still too rough for me to be able to work on it. 
% Announce what is your point/goal and then provide details supporting it.
% Right now you juxtapose sentences without providing a sense of
% why the reader should care.]}

% ,The convex hull of the projection of the global point cloud 
% \mani{
% Let $\mathcal{O}_i$ denote the region that is not occluded by the structure and falls within the field of view of 
% the camera positioned at the $i^{th}$ viewpoint of the PE phase. 
% % Let $\mathcal{O}_i$ denote the portion of $\mathcal{V}_i$ that is 
% % not occluded by the structure and falls within the range of the camera.
% Note that the path followed by the camera during the PE phase forms a curve 
% enclosing the structure in the $\vect{x}_{\fr g} \vect{y}_{\fr g}$ plane.
% % Hence, $\mathcal{V} = \cup_{i=1}^{m} \mathcal{V}_i$
% which along with the half spaces $\{z^{\fr g}>0\}$ and $\{z^{\fr g}<H_{\max}\}$ defines a volume 
% $\mathcal{H}$ that contains the entire structure. Now, let $\mathcal{M}$ 
% denote the entire structure to be mapped. }
% % and $\mathcal{V} = \cup_{i=1}^{m} \mathcal{V}_i$, where
% % $\mathcal{V}_i$ is the observable region in the view frustum of the camera at 
% % the $i^{th}$ viewpoint in the PE phase, denote all the explored regions. 
% \mani{We can then define $\mathcal{C} = \mathcal{H} \setminus (\mathcal{M} \cup (\cup_{i=1}^{m}\mathcal{O}_i))$, which is the union of all cavities present
% in the structure, with each connected component in $\mathcal{C}$ a separate cavity.
% The set $\mathcal{M}$ is only used to provide a definition of a cavity but
% is not required by our exploration policies, which only need to determine
% the entrances of the cavities.}
% %by our algorithm. Our policies only require the location of an 
% %entrance to these cavities and our algorithm to determine these locations is 
% %presented below.}
% \jln{[You seem to need $\mathcal V$ only to define $\mathcal H$. Isn't $\mathcal H$
% simply the cylinder defined by the curve you mention in the $\vect{x}_{\fr g} \vect{y}_{\fr g}$
% plane and the two planes $\{z^{\fr g}=0\}$ and $\{z^{\fr g}=H_{\max}\}$?]}

Our algorithm to determine the entrance to the cavities uses
%Firstly, 
a voxel based 3D occupancy grid constructed from the global point cloud. 
%and the corresponding positions of the viewpoints from the vSLAM module.
% , see Fig. \ref{fig:octomap}.
%In this paper, 
We use the OctoMap \cite{hornung13auro} library to maintain this
occupancy grid in a
%which uses an 
hierarchical tree data structure. % to hierarchically partition the space. 
%In order to provide a precise definition for a cavity, we first need an alternative 
%representation of the model which can represent free spaces and unknown regions.
%\jln{[Really? Is the definition of cavity tied to octomap? Aren't voxels enough?}]
%An OctoMap \cite{hornung13auro} provides this representation through an efficient 3D 
%occupancy grid based on an octree data structure. % mapping approach 
%It hierarchically partition the space into 
%cubical volumes also called voxels. Each voxel contains a probabilistic occupancy 
%measurement and a voxel (or a node) is subdivided only if a child node has a different 
%occupancy than its other children. The unknown regions are implicitly specified by the 
%absence of the corresponding nodes. 
%The underlying rationale is that if a sensor present 
%at a position $S$ detects a point at position $P$, then all the voxels that are 
%intersected by $\vec{SP}$ must be free. For a sensor such as a Kinect, only the region 
%present within the view frustum of the camera can be detected due to the 
%finite range and FOV of the camera. Typically, these sensors could fail to 
%detect certain regions present within the view frustum of the camera due to 
%limitations in its sensing \jln{technology}. %methodologies. 
%Hence all the free spaces are not marked as free in the OctoMap \jln{[??]}.
%\jln{[I think the definition of unknown space vs. free space needs to
%be clarified still. Perhaps also add a pic of an octomap voxel and one
%subdivision level, with some free and occupied children.]}
%In general, OctoMaps that are constructed from vSLAM systems only 
%maintain the occupied voxel information \mani{[should add references]}. These 
%OctoMaps are constructed by inserting the fully assembled point cloud from 
%the SLAM module without corresponding sensor pose information. As a result, 
%the free space and unknown space information is completely lost, but this 
%information is vital for exploration algorithms. Therefore,
%Construction of the OctoMap requires a point cloud along with the corresponding sensor
%position. 
Internally, the OctoMap library performs ray casting operations, labelling the %updating the 
occupancy measurement of each voxel along the line segment from the camera 
position to each point in the point cloud as \emph{free} and the point itself as 
\emph{occupied}.
%Fig. \ref{fig:octomap_single} shows the OctoMap constructed from just the initial image.
%we incrementally 
%construct an OctoMap of the model (as shown in Fig. \ref{fig:octomap}) such 
%that the free space and unknown region information are correctly maintained.
For this, we require the vSLAM module to provide the sequence of point clouds
and associated camera positions used in assembling the current model.
All voxels in the occupancy grid that are not labeled free or occupied 
are called \emph{unknown}.
%As discussed in Section II, 
% \mani{This is the only additional requirement of our policies from the 
% vSLAM module.}\jlnComment{I suggest to remove this last sentence} 
%Fig. \ref{subfig:octomap_single} shows the OctoMap constructed from 
%just the initial image. Observe that the grey region corresponds to free space but this
%information is not integrated into the OctoMap \jln{[??]}. 
%Fig. \ref{subfig:octomap_whole} shows the OctoMap constructed at the end of PE phase. 
%We now define the following regions to aid the 
%discussion but do not explicitly compute them in our algorithm.
%
%\begin{defi}\label{defi: explored region}
%Voxels that lie within any of the view frustums, irrespective of their occupancy, form the \emph{explored region}.
%%are referred to as explored regions.
%\end{defi}
%
%\begin{defi}\label{defi: free space}
%Voxels that have been marked free during ray casting operations denote the \emph{free space}.
%\end{defi}
%
%\begin{defi}\label{defi: unknown region}
%Voxels that have not taken part in any of the ray casting operations denote the \emph{unknown region}.
%\end{defi}
%
%\begin{defi}\label{defi: unexplored regions}
%Voxels that do not lie within any of the view frustums are called the \emph{unexplored region}.
%\end{defi}
%
%\begin{defi}\label{defi: cavities}
%\emph{Cavities} are unexplored regions that can potentially contain portions of the structure. \jln{[Is that a formal definition?]}
%\end{defi}

%The path followed by the robot during the PE phase forms a curve enclosing the structure
%\jln{in the $x^{\fr g}y^{\fr g}$ plane, which together with the planes $z^{\fr g}=0$ and $z^{\fr g}=H_{\max}$
%defines a cylinder $\mathcal{H}$ that contains the entire structure}. Therefore, cavities
%must necessarily lie within this cylinder. Moreover, unexplored regions above and below 
%view frustums cannot hold portions of the structure. These properties are now used to identify a cavity, see Fig. \ref{subfig:octomap_whole}. 
%Thus, any missing parts in the constructed model must 
%be present within this bounded finite volume. 
%lie within this cylinder
% This corresponds to 
%a class of SLAM methods referred to as Pose-Graph SLAM \jln{[Ref?]}. 
%Thus, each point cloud is inserted from its corresponding pose. 
%Note that these are the optimized \jln{[??]} sensor poses as determined by 
%the SLAM module. Constructing an OctoMap is a computationally intensive operation. 
%Also, further corrections in the sensor poses cannot be dynamically integrated 
%into the OctoMap and the OctoMap will have to be reconstructed from scratch. 
%Hence, we construct the OctoMap only at the end of the PE phase using the 
%optimized sensor poses.
%\jln{[Note: once you have defined an acronym like vSLAM, you need to use it,
%not return to visual SLAM. I changed this above already.]}

% \begin{figure}[!t]
% \centering
% \subfloat[with initial point cloud alone]{
% 	\includegraphics[width=1.5in]{figures/view_frustum}
% 	\label{subfig:octomap_single}
% }
% \hfil
% \subfloat[after PE phase]{
% 	\includegraphics[width=1.5in]{figures/OctoMapPerspFree}
% 	\label{subfig:octomap_whole}
% }
% \caption{a) The black trapezoid shows the sectional view of the view frustum. b) The incrementally constructed OctoMap representation of the model at the end of PE phase. Green represents free space, blue represents the structure and yellow represents the cavity.  }
% \label{fig:octomap}
% \end{figure}

% \begin{figure}
% \begin{algorithmic}[1]
% \Procedure{determineCavities}{octomap,cloud}
% 	\For{\textbf{each} voxel \textbf{in} octomap}
% 		\If{getOccupancy(voxel,16) == FREE}
% 			\For{\textbf{each} ngbr \textbf{in} getNeighbors(voxel)}
%             	\If{getOccupancy(ngbr) == UNKNOWN \textbf{and} getOccupancy(voxel,13) == FREE}
%                 	\State candidates.append(voxel)
%                     \State \textbf{break}
%                 \EndIf\label{dCIf2}
% 			\EndFor\label{dCFor2}	        	
%         \EndIf\label{dCIf1}
% 	\EndFor\label{dCFor1}
%     \State normals $\gets$ PCLNormalEst(candidates)
%     \State chull $\gets$ getConvexHull(cloud)
%     \State cropped $\gets$ PCLCropHull(candidates, chull)
%   	\For{\textbf{each} pt \textbf{in} cropped}
%     	\State pt\_n $\gets$ getNormalOf(pt)
% 		\If{pt\_n.z() $<$ 0.1}
% %         	\If{octomap.castRay(pt, pt\_n) == HIT}
% 				\State frontiers.append(pt)
% %             \EndIf\label{dCIf5}
% 		\EndIf\label{dCIf4}
% 	\EndFor\label{dCFor3}
%     \State clusters $\gets$ PCLClusterExtraction(frontiers)
% \EndProcedure
% \end{algorithmic}
% \caption{Procedure for determining the locations of unexplored cavities in the structure.
% \jln{Unfortunately you do not really refer to this Fig. in the text, so I don't know
% when or how I should read it, plus it's full of undefined stuff.}
% }\label{alg:determineCavitites}
% \end{figure}

%, as mentioned in Section.... 
%Our policies only require the location of an entrance to these cavities.
%and these are determined using the constructed OctoMap.
%, following the pseudo-code provided in Fig. \ref{alg:determineCavitites}. 

% We refer to these voxels as frontier voxels whose definition is 
% %adapted from In order to extract 
% %these voxels from the OctoMap, we make use of the concept of Frontier Voxels 
% % and 
% % Frontiers and list the necessary conditions for a frontier to denote a cavity.
% %,see Fig. \ref{fig:octomap frontier}. 
% % These definitions are 
% adapted from \cite{Yamauchi:1997:FAA:523996.793157}.
% % and restated here to aid the discussion.
Using the constructed OctoMap, we compute a set of frontier voxels,
whose definition is adapted from \cite{Yamauchi:1997:FAA:523996.793157}.
\begin{defi}\label{defi: frontier voxel}
A \emph{frontier voxel} is a free voxel with at least one neighboring unknown voxel. 
\end{defi}

% \begin{defi}\label{defi: frontier}
% A \emph{frontier} is a surface formed by a collection of frontier voxels. 
% % that forms a border
% % between known free space and the unknown region is called a \emph{frontier}. 
% % \jln{[Can't we get rid of ``that forms a border between known free space and the unknown region'',
% % isn't that a restatement of the definition?]}
% \end{defi}

%\jln{[Are these the same definitions as in Yamauchi? Then cite again.]}

\begin{figure}[!t]
\centering
\subfloat[with frontier voxels]{
	\includegraphics[width=\linewidth]{figures/frontier_candidates}
	\label{subfig:frontier candidates}
}
\hfil
\subfloat[with cavity entrance voxels]{
	\includegraphics[width=\linewidth]{figures/frontier_voxels}
	\label{subfig:frontier}
}
\caption{ 
% The frontier voxels based on the original definition from 
% \cite{Yamauchi:1997:FAA:523996.793157} is shown in yellow and those based
% on our adapted definition is shown in red.
a) The constructed OctoMap with occupied voxels shown in blue and frontier voxels 
shown in yellow. These yellow voxels form the boundary of the explored region and most of them lie along the top and bottom faces of the view frustums. b) The cavity entrance voxels are shown in red.
%\mani{change to all frontier voxels}
}
\label{fig:octomap frontier}
\end{figure}

Recall that the camera is constrained to move in a horizontal plane and the
system continuously explored the structure in the clockwise sense during the
PE phase. Consequently, point clouds captured before and after a cavity 
result in vertical planes of frontier voxels that border it and that we
want to detect as entrance of the cavity for further exploration, see Fig. 
\ref{subfig:frontier}. Therefore, we define \emph{cavity entrance voxels}
as
%frontier voxels that denote the entrances to cavities should satisfy two additional
%conditions.
frontier voxels satisfying two additional conditions.
% frontier voxels are required to satisfy two 
% additional conditions. 
First, the normal to the frontier voxel, computed using
the nearby frontier voxels\cite{mitra2004estimating}, must approximately lie 
on the $\vect{x}_{\fr g} \vect{y}_{\fr g}$ plane.
% \jlnComment{Recall why. Removing the top of view frustums?} 
This serves to remove frontier voxels that lie along the top and bottom faces of
the view frustums, See Fig. \ref{subfig:frontier candidates}.
Second, Type I flaws can result in frontier voxels, which we want to exclude
from cavity entrance voxels. 
Therefore, we require that the distance to the closest occupied voxel 
should be greater than some threshold $d_0$, which can be chosen as 
% the typical scale length of the robot. 
a small fraction of the distance maintained from the structure, say $0.1\,D$.
%The complete set of frontier voxels $\mathcal{F}$ is 
% computed by searching through the 
% OctoMap, see Fig. \ref{subfig:frontier candidates}. These frontier voxels are 
%filtered based on the following two conditions \jln{[why?]}:
%\begin{enumerate}
%\item The distance to the closest occupied voxel is greater than $d_0$, where 
%$d_0$ is the typical scale length of the robot as cavities are unexplored regions.
%% \jln{[Change ``approximately more'']}
%% \item The frontier voxels must lie within the convex hull of the structure. 
%\item The normal to the frontier voxel, computed using the nearby frontier
%voxels, must approximately lie on the $\vect{x}_{\fr g} \vect{y}_{\fr g}$ plane
%as free voxels bordering the cavity form vertical planes.
%\end{enumerate}
%The total number of frontier voxels exponentially increases \jln{[??]} with the 
%size of the structure. Therefore, computing the exact distance of the frontier 
%voxel from the structure is time consuming. On the other hand, 
As the number of voxels in a typical structure is very large, we do not
perform this thresholding exactly but instead we use 
an estimate for the distance to the structure obtained from OctoMap. The 
hierarchical structure of OctoMap allows efficient multi-resolution queries, 
see Fig. \ref{fig:octomap_diff_res}, and thus we keep as cavity entrance
voxels only those that are marked free at a resolution of approximately $d_0$.
%therefore the frontier voxels must be marked free at a resolution of approximately $d_0$.
% are removed from $\mathcal{F}$. In our simulations,
%the resolution of the constructed OctoMap is $0.05m$ and $d_0$ is $1m$. Hence,
%a depth of $12$ corresponds to $d_0$.   \jln{[blah, blah... where is this going?]}
% First, \jln{frontier} voxels need to be sufficiently far away from the structure \jln{[what does this mean?]}. 
% On the other hand, those frontier voxels with nearby occupied voxels \jln{[What is nearby? 
% whose neighboring voxels are occupied?]} represent Type 1 flaws. 
% Therefore \jln{[??]}, we require an efficient method for estimating the distance from 
% a candidate voxel to the structure. OctoMaps can be efficiently queried at
%multiple resolutions and this provides an efficient means to obtain a 
%heuristic on the distance of the candidate voxel from the structure. Fig. 
%\ref{fig:octomap_diff_res} shows an OctoMap at two different resolutions.
% Note that the majority of Type I flaws disappear when the leaf size of the
% OctoMap is increased. This is because every node that has at least one
% occupied child node is also occupied. Thus, frontier voxels that are present 
% close to the structure would be marked occupied at a larger scale length. 
% On the contrary, those voxels that are sufficiently far away from the 
% structure would remain as free even at a larger scale length. An OctoMap 
% has a maximum depth of $16$ and the resolution doubles when the depth is reduced by $1$ level. Here, we take that if a voxel is marked free at a depth
% of $13$ then it is considered to be sufficiently far from the structure.
% \jln{[This is a bit weird. Since we mentioned the scale of a robot as
% typical scale we are mapping, perhaps we need to know if that corresponds
% to depth 13...]}

\begin{figure}[!t]
\centering
\subfloat[Leaf size: $0.05m$, depth: 16]{
	\includegraphics[width=1.5in]{figures/OctoMap16}
	\label{subfig:octomap_16}
}
\hfil
\subfloat[Leaf size: $0.4m$, depth: 13]{
	\includegraphics[width=1.5in]{figures/OctoMap13}
	\label{subfig:octomap_13}
}
\caption{
OctoMap queried at depth level $16$ and $13$ respectively. In this paper, the
% typical scale length of the robot $d_0$ is $1m$.
value of $D$ is $3\text{m}$ and the threshold $d_0$ is chosen as $0.4\text{m}$.
% The OctoMap
% does not inflate uniformly and therefore, some frontier voxels that are
% close to the structure may not be filtered out.
%Hence \jln{[??]}, holes that are present at a smaller scale \jln{[can?]} 
%get filled when viewed at a bigger scale length \jln{[and so?]}.
}
\label{fig:octomap_diff_res}
\end{figure}

% The PE phase ensures that all the cavities must lie within $\mathcal{H}$ \jln{[already said]}. 
% Using the same argument, an even stronger condition is obtained \jln{[useless sentence]}. Instead of
% using the past trajectory of the robot, which is an additional $D$ distant 
% from the structure, the convex hull of the projection of the point cloud on
% the $xy$ plane can be used to define $\mathcal{H}$. This reduces the
% number of candidate frontier voxels. \jln{[and so?]}

% The frontier voxels corresponding to unexplored regions that are above and
% below view frustums are removed from $\mathcal{F}$ by checking if the 
% $z$-component of the local normal is above a certain threshold. This local
% normal is also computed using PCA as discussed in Section III A. 
% Consequently, only the frontier voxels associated to the lateral faces of the view frustum
% remain. 

% Second, the normal to the frontier at the candidate voxel must approximately
% lie in the $XY$ plane %\jln{[was XY plane before]}. 
% This follows from Assumption \ref{assumption: fixed camera}. 
% Recall that cavities are formed due to the finite range of the 
% camera. Therefore, a cavity would be flanked by occupied voxels that fell
% within the range of the camera. Correspondingly, there would \jln{[why these would?]} 
% exist frontiers 
% adjoining these occupied voxels. Typically, when the point cloud is inserted 
% into the OctoMap, at least $4$ frontiers corresponding to the $4$ lateral 
% faces of the View Frustum \jln{are} formed. As more point clouds are added, some of 
% these frontiers disappear while others merge to form larger or smaller 
% frontiers. When the entire OctoMap is constructed, the remaining frontiers
% are candidates for denoting cavities
% \jln{[I'm lost in your structure. Shouldn't these kind of explanations be earlier]}.
% Following Assumption \ref{assumption: 
% fixed camera}, the camera is constrained to move in the $XY$ plane. Hence, 
% the frontiers corresponding to the top and bottom faces of the View Frustum
% remain. By imposing a constraint on the local normal, we select only those
% frontiers corresponding to the side faces of the View Frustum \jln{[why the capital
% letters? If you need it, add an acronym]}. 
% \jln{[Is this whole paragraph to explain the first sentence?]}

% Third, the frontier voxels must lie within the convex hull of the structure.
% When point clouds from two adjacent sensor positions are combined, the right
% frontier of the left View Frustum and the left frontier of the right View 
% Frustum merge forming a new $V$ shaped frontier. As the distance between
% sensor positions is kept small, these $V$ shaped frontiers are small and 
% generally lie outside the convex hull of the structure. 
% \jln{I don't get this paragrah, and plus I don't know what generally,
% small, etc. mean. How do I know it does not break the algo?}

% Fourth, a ray from the frontier voxel along its normal should intersect the 
% structure. At junctions where the height of the structure increases or 
% decreases significantly, frontiers with normal along the $X-Y$ plane are 
% formed. But the rays from these frontiers would 
% \mani{[Need to check if castRay condition is a necessary condition]}

% Thus, the frontiers formed by those frontier voxels satisfying the above 
% conditions denote a cavity.  
Finally, the cavity entrance voxels are clustered using an Euclidean clustering 
algorithm from PCL \cite{rusu20113d} and each cluster is referred to as a 
\emph{cavity entrance}. %, %Point Cloud Library (PCL) \cite{rusu20113d} %\jln{[?? + ref]}
%which groups voxels that are within a distance of $R$ from each other. 
Moreover, there could be some sparsely located cavity entrance voxels,
% that also satisfy the above conditions. These 
which are removed by setting a minimum size for the cavity entrances. % frontiers. 
%Fig. \ref{subfig:frontier} shows the computed cavity entrances in red % frontiers in red 
%along with the constructed OctoMap. 
% The centroid of each frontier denotes the entrance to
% a cavity and our CE policy explores each of these cavities sequentially.
%Having determined all the frontiers that denote cavities, we can
% now start the CE phase where each of these cavities are explored to 
% completion.

% \jln{[Again, this section feels way to long and wordy. Can't you come up
% with a more concise semi-formal definition of a cavity? You even had mentioned
% cavities in Section III.A. At the end, we are lost, what is a cavity exactly?
% A reader also needs visual hooks in the text that he can use to navigate.
% For example, use bullet points, Definition environments, etc.]}



\subsection{Cavity Exploration}

% \jln{[Might want to mention in this section (or at the beginning of the paper) 
% that we assume that we cannot enter hallways/tunnels that go through the structure 
% s.t. cavities are connected with each other inside the structure...]}

\begin{figure}[!t]
\centering
\subfloat[at the starting viewpoint]{
	\includegraphics[width=3.0in]{figures/CE_start}
	\label{subfig:CE_start}
}
\hfil
\subfloat[mapping confined spaces]{
	\includegraphics[width=3.0in]{figures/CE_exploring}
	\label{subfig:CE_progress}
}
\hfil
\subfloat[end of cavity exploration]{
	\includegraphics[width=3.0in]{figures/CE_end}
	\label{subfig:CE_end}
}
\caption{Cavity exploration. a) The robot is at the starting viewpoint for exploring the cavity. The starting and ending viewpoints for the first cavity entrance are shown in green and red respectively. All the cavity entrance voxels are also shown. b) The extracted model showing CE in progress. c) %The view frustum of the ending viewpoint is shown in red and 
The system detects the end of the cavity after coming close to the ending viewpoint. The blue line shows the trajectory followed by the robot. % and the black lines represent loop closures \jln{[??]} from the vSLAM module. 
}
\label{fig:cavity exploration}
\end{figure}

%Every cavity is denoted by \jln{[associated to ?]} at least one frontier. 
%Alternatively, the frontier
%can be considered as the entrance to the cavity \jln{[So remove the first sentence]}. 
%From the previous subsection we have determined
%Here, we explain our policies for exploring a single cavity, given the %location of a frontier.
%Hence, the exploration policy is similar to the PE phase.

%There are three parts to the CE phase. First, a starting viewpoint such that
%the cavity is visible from it. Second, a motion planning policy for exploring 
%the cavity. Third, a termination condition for detecting when the cavity
%exploration is complete. 

% \jln{[The CE phase terminology is not clear. Initially, you seem to define it for the exploration
% of all cavities, then CE phase refers to a single cavity.]}

Once the 
%frontier voxels representing the entrance of cavities 
cavity entrances
have been determined, we can start the CE phase.
We explore each detected cavity using an exploration
strategy analogous to the PE phase. For this, we require a starting and ending
viewpoint for each cavity entrance. We assume that there are no tunnels that
go through the structure and that a robot entering a cavity can exit via the same
location only. For a given cavity entrance, the starting viewpoint
is chosen from the set of camera poses returned by the vSLAM module during the PE phase
and such that the centroid of the cavity entrance lies within the view frustum.
% at least one of the frontier voxels lie within the view frustum. 
Additionally, the centroid should not be occluded by the structure from the
camera position.
% space along the line segment joining this centroid and the 
% camera position could either be free or unknown but should not be occupied.
% must be free\jln{[What if there is no such voxel?]}.
%as the frontier voxels is itself free.
From these camera poses, the one with the earliest timestamp $\tau_0$ is chosen as 
starting viewpoint and we let $\tau_1$ denote the largest timestamp. %, 
%see Fig. \ref{subfig:CE_start}.
% Finally, for cavities with wide entrances, viewing along the centroid of the cavity entrance might result in an empty point cloud for the limited range of the camera. Therefore, the starting viewpoint is chosen as that with the largest timestamp $\tau_1$ such that $\tau_1 < \tau_0$ and no \jln{cavity entrance} voxel lies within the view frustum 
%\jlnComment{I don't understand. So the starting Viewpoint is in fact not in
%the set of camera poses such that the centroid lies in the view frustum??}
The ending viewpoint is chosen from the set of viewpoints from the PE phase as the one 
with the smallest timestamp $\tau_2$ such that $\tau_2 > \tau_1$ and such that 
no cavity entrance voxel lies within the view frustum, see Fig. \ref{subfig:CE_start}.

The timestamps of the starting viewpoints of the cavity entrances are used to sort them in increasing order
% Once the starting and ending viewpoints are determined for each cavity entrance, the set of cavity
% entrances are sorted in increasing order of the timestamps of their starting viewpoints
and each
of the cavities is explored in sequence. A typical cavity has at least two cavity entrances bordering it and 
it is possible to have more cavity entrances if there is a row of pillars, for example. 
During the CE phase, if the centroid of a cavity entrance falls within the view frustum of the
current camera position and is not occluded by the structure, we remove that
cavity entrance from our list.

%The algorithm of the previous section identifies the entrances to cavities,
%but does not provide any information on their size of geometry.
%The frontier denotes the entrance to a cavity but there is 
%no information regarding its size or geometry. 
%Once the robot is positioned at the starting viewpoint, we use again 
%Algorithm \ref{alg:gpp_computeNBV} as in the PE phase to compute the successive viewpoints 
%and explore the cavity, but 
Exploring confined regions during the CE phase requires certain modifications to the
PE policy. Recall that the system skipped the cavities during the PE phase as the
robot came closer than a distance $D$ from the structure. Therefore during the CE phase, 
only the region directly ahead of the robot and within a distance $\delta<D$ is checked
for interference of the computed path with the structure. Moreover, our local path planner
returns paths that maintain a distance $\delta$ from the structure, see Fig.
\ref{fig:CE_dynamicD}.
%adapt the same procedure as in PE phase for computing the next viewpoint as shown in 
% but we modify the value of $D$ used in line \ref{algo: final goal} in order to let the robot map confined spaces. 
% \mani{For regions such as hallways and short narrow sections near the entrance of a cavity, 
% iteratively determine the value of 
%\jlnComment{what does this sentence mean? And $g$ is the 
%minimum of $N$ only if it is chosen far enough from the obstacles}
% $D$ such that $N(g)$ is a local minimum, where $N$ is defined in \eqref{eq: N definition}. 
% The robot is considered to be in a confined area if $N(g)>0$.
For this, using the notation of Sections \ref{section: next goal} 
and \ref{section: local path planner}, we modify $goal$ as $goal \gets \overline{p}^{\fr C} - \delta \, \vect{n} + step \, \vect{r}$ and transform to the global FoR to obtain the new point $g$. 
% \jln{[Clarify confined regions - choice of $\delta = D$ or not, etc., as discussed.]}
The distance $\delta$ is chosen by starting from a small value and increasing it until
we reach a local minimum of $N(g)$, where $N$ is defined in \eqref{eq: N definition}.
% Note that if the robot is not in a confined area, $N(g) = 0$ for $\delta=D$ and the computed 
% path lies along $\partial Q$ as in the PE phase. 
% }
%Moreover, using the arm to scan ahead could prevent the robot from having 
%a smooth trajectory in confined spaces. Hence, the robot is turned in place 
%such that the structure ahead falls within the FOV of the camera. 
% \jln{[Simplify this paragraph, no need to detect confined areas.]}

\begin{figure}[!t]

	\includegraphics[width=\linewidth]{figures/CE_dynamicD}
	\caption{Costmap at the beginning of the exploration of a cavity. %CE phase. 
    The robot is shown as a black rectangle and the green arrow 
    oriented along $\vect{r}$ has its base at the computed next goal $g$ such that $N(g)$ 
    is a local minimum.
    %, is shown as a green arrow 
    The value of $\delta$ determined online is $2.0\text{m}$ while
    the value of $D$ used in PE phase is $3.0\text{m}$.
	}
\label{fig:CE_dynamicD}
\end{figure}

% Moreover, an obstacle is detected ahead of the robot inside a cavity, as in Section \ref{section: obstacle ahead},
% the robot is turned in place until there are no occupied cells in front of the it, see Fig. \ref{subfig:CE_progress} \jln{[sentence
% ambiguous. Also, why not turn the camera first, could then move the robot backwards]}.
% the procedure to compute
% the next viewpoint is the same as for the PE phase except 
% the size of the parameter $D$ which determines how far the robot is from
% the structure \jln{[You can use Algorithm 1 for Fig. 4 and refer to it that 
% way. See Algorithm package]}. 
% During the PE phase, the objective was to determine the bounds 
% of the structure and hence, the parameter $D$ is kept comparable to the 
% range of the camera. Thus, a larger section of the structure is used in the
% computation of the next viewpoint. On the other hand, the objective in the CE 
% phase is to explore the cavity thoroughly. Hence, a small value for the parameter 
% $D$ is used, typically the characteristic length of the robot. Note that this 
% would also reduce the maximum height that can be mapped for these portions of 
% the structure based on the vertical angle of view of the camera. Another 
% difference is in the use of the costmaps. The range of the look-ahead is 
% fixed at $D$. When the structure is detected ahead, the robot turns in place 
% until there are no occupied cells ahead of the robot in the local costmap.
% This allows the robot to map tight corners. \jln{[Can't he turn the camera instead?]}
% \mani{[need to consider different types of junctions inside a cavity]}
% \maniComment{Have to discuss this}
% \mani{[Handling hall ways and narrow cavities?]}

The system detects that it has %\underline{CE phase for the current cavity} by 
finished exploring the current cavity by
monitoring the loop closures obtained by the vSLAM module. When the robot
exits the current cavity, the point clouds captured by the camera correspond
to parts of the structure that are already present in the model,
see Fig. \ref{subfig:CE_end}. Consequently, these result in loop closures 
in the vSLAM module. We declare the cavity explored once the system receives 
a loop closure with a viewpoint at timestamp $\tau$ such that $\tau_2 < \tau < \tau_F$ where $\tau_F$ corresponds to the last viewpoint of the PE phase.
Alternatively, the number of changes in the occupancy measurements of the OctoMap
could be used to detect the end of the cavity, since the point clouds captured after
exiting the cavity do not add new information to the OctoMap. 
% But this requires
% % the OctoMap to be reconstructed whenever the vSLAM module makes a correction in the
% % odometry and this is highly time consuming.
% re-building the OctoMap in real-time, which is a computationally intensive task. 
But this solution tends to be less robust because the localization errors and 
sensor noise can induce a large number of changes even when the camera 
is viewing a region that is already present in the model.
%, even though the camera is viewing a region that is already present in the model.
% The system detects the completion of CE phase for the current cavity by monitoring 
% the number of changes in the occupancy measurements of the OctoMap. When the robot
% exits the current cavity, the point clouds captured by the camera correspond
% to parts of the structure that are already present in the model, %OctoMap,
% %re-visits locations that were already seen during the PE phase and therefore the captured point clouds yield no new information,
% see Fig. \ref{subfig:CE_end}. Consequently, when these point clouds are inserted into
% the OctoMap, the number of changes is relatively small, depending on the measurement noise 
% and localization error of the robot.
% %is below a threshold. These few changes are due to the noise in measurements and error in localization of the robot. 
% We declare the cavity explored once this number of changes falls below a threshold set to be 
% the average number of changes detected between successive goals %\jln{[by each point cloud?]} 
% during the PE phase.
% \maniComment{Change to loop closures after the end of cavity}
% \mani{[If the value of $D$ for PE and CE vary greatly, then SLAM module may 
% not obtain loop closures.]} 
% \mani{[Could possibly use the property that at the end of the exploration, the robot trajectory would be parallel to segment of 1st pass. But loop closures might skew the path. Or that no new information is seen? Fine structural details might be seen when viewed at closer range.]}
%\jln{[Not clear. Also, remove all these would.]}

intro.tex
% !TEX root =  ../main.tex

%\jln{[Overall the text is too wordy. Do not try to write a novel, more
%a technical paper. Use figures if needed, bullet points, a few equations here and there 
%if that simplifies, refer to the lines in your algorithms, etc. For example,
%if I need to explain what cavity means from this text, I can't, the explanation
%takes almost 3 pages. Cut to the chase, your typical reader will just want to skim 
%through the paper and quickly understand the main ideas in each section, help 
%him do that.]}

\section{Introduction}
% The very first letter is a 2 line initial drop letter followed
% by the rest of the first word in caps.
% 
% form to use if the first word consists of a single letter:
% \IEEEPARstart{A}{demo} file is ....
% 
% form to use if you need the single drop letter followed by
% normal text (unknown if ever used by IEEE):
% \IEEEPARstart{A}{}demo file is ....
% 
% Some journals put the first two words in caps:
% \IEEEPARstart{T}{his demo} file is ....
% 
% Here we have the typical use of a "T" for an initial drop letter
% and "HIS" in caps to complete the first word.
%
% \IEEEPARstart{A}{ctive} sensing can be stated as a problem of controlling strategies applied to the data acquisition process  which will depend on the current state of the data interpretation and the goal or the task of the process. -  Ruzena Bajcsy, \emph{Active Perception}, 1988 \cite{Bajcsy:IEEE88:activePerception}.
% \jln{[We need to rewrite the intro.]}

% \jln{[Our algorithm does not require any initialization in the form of a mesh or hull model or of a bounding box.]}

% \IEEEPARstart{A}{ctive} sensing refers to the problem of controlling 
% the data acquisition process, typically in a way that should be adapted 
% online as more data is collected and interpreted 
% \cite{Bajcsy:IEEE88:activePerception}. 
% \jln{[Start rather with your examples below (inspection, etc.). You can mention active sensing later when you place the paper in the context of a broader technical subject.]}

\IEEEPARstart{A}{ccurate} 3D computer models of large structures have
a wide range of practical applications, from inspecting an aging structure 
to providing virtual tours of cultural heritage sites 
%\st{, and the pre-requisite is a 3D model of the structure} 
\cite{Jacobi201580,el2004detailed}. 
%\jln{[Can we remove or merge the 2nd sentence, redundant]}.
In order to autonomously build such a 3D model in real-time, we need to address 
two problems. First, we need a robust mapping system that can build 
the 3D model on the fly when given a sequence of images or depth maps as 
input. 
This is a widely researched problem called Visual Simultaneous Localization 
and Mapping (vSLAM), for which several open source 
packages offer increasingly accurate and efficient solutions 
\cite{labbe14online,Endres:TRO2014:RGBDSLAM}.
The second problem relates to active sensing \cite{Bajcsy:IEEE88:activePerception},
as we need motion planning strategies that can guide a mobile sensor to explore 
the structure of interest. For mapping, monitoring or inspection applications, 
certain classical strategies such as frontier-based exploration algorithms \cite{Yamauchi:1997:FAA:523996.793157}, which guide the robot to previously 
unexplored regions irrespective of whether it is part of the structure 
of interest or not, are not necessarily well adapted.

% You must have at least 2 lines in the paragraph with the drop letter
% (should never be an issue)

% needed in second column of first page if using \IEEEpubid
%\IEEEpubidadjcol

The goal of this work is to guide a mobile ground robot equipped with 
a depth sensor, in order to autonomously determine the boundaries of an initially unknown 
structure, build a 3D model of the structure and attempt to fill holes in the model
so that the reconstruction is as accurate and complete as possible.
%\subsection{Related Work}
%Recently, there has been a lot of work in 3D reconstruction 
%of unknown objects by moving a depth sensor around the object.  % relatively
Some recent work considers the problem of reconstructing a 3D model
of arbitrary objects by moving a depth sensor relative to the object \cite{kriegel2013efficient,krainin2011autonomous}.
% By either moving the depth sensor around the area to be scanned 
% \cite{DBLP:conf/iros/Vasquez-GomezSM14} 
% or by positioning the object relative to a fixed depth sensor using 
% a robotic arm \cite{krainin2011autonomous}, these papers focus on 
% maximizing the expected information gain for successive %different relative 
% positions of the depth sensor relative to the object. 
Typically, these systems iteratively build a complete 3D model of 
the object by heuristically choosing the next best viewpoint 
according to some performance measure.
%, which is optimal in some sense, 
% 
%A major limitation of these systems is that they 
However, much of this work is %focused on 
restricted to building models of relatively 
small objects that 
%can completely fall inside the field-of-view of the depth sensor,
are bounded by the size of the robot workspace.
In contrast, our focus is on
%as opposed to the 
3D reconstruction of larger but still
bounded structures such as buildings, which can be several orders 
of magnitude larger than a mobile robot. %the field-of-view of the sensor.
%Moreover, in contrast to our work, these systems assume some prior 
%knowledge about the approximate size and position of the object 
%\jln{[seems to contradict the word ``unknown'' you use above]}.
%
The related problem of automated inspection deals with 
large structures such as tall buildings \cite{Lin:civEng2015:UAVconstructionMonitoring}
and ship hulls.
%This is also true for most of the work on automated inspection 
Bircher et al. \cite{bircher2015structural} assume that
a prior 3D mesh of the structure to inspect is available and
compute a short path connecting viewpoints that together are guaranteed
to cover all triangles in the mesh.
As they point out, the inspection problem starting from a prior
model is related to coverage path planning, see, e.g., \cite{Acar:TRO06:coverage,Lim14CrackInspection}.
In \cite{englot2012sampling}, Englot et al. begin by assuming
a safe bounding box of the hull and construct a coarse mesh
of the hull by tracing along the walls of this box in a
fixed trajectory without taking feedback from the actual 
geometry of the structure. Moreover,
this coarse mesh is manually processed offline to yield an 
accurate 3D mesh which is then used to inspect the finer 
structural details. Sheng et al. \cite{Sheng08CrawlerInspection} use 
a prior CAD model of an aircraft to plan a path for a robotic crawler
such that it inspects all the rivets on the surface of the aircraft.
%\jln{[Really? Skimming through that paper,
%I don't see that, seems similar setup as Bircher et al.]}
%\mani{[Refer section 7.1 and 7.2 and Fig. 15. But they state that automating 
%the process of obtaining this coarse 3D mesh is an open problem.]}
In this paper however, we do not assume any prior information 
in terms of a 3D mesh, CAD model or a bounding box around the
structure, and focus on reactive path-planning to build the
model online.
%, bircher2015structural}, which often assumes 
%that a prior 3D model of the structure to be inspected is available. Englot et al. 
%
%A key application of mapping structures is for the purpose of inspection. 
%Most of the work in this area assumes that a prior 3D model of the structure 
%to be inspected is readily available.
%A set of waypoints can then be generated from a given mesh of the structure to be inspected. These waypoints guarantee the complete coverage of the inspection process and thus the problem is reduced to a Traveling Salesman Problem, where the shortest trajectory that visits all the waypoints is computed \cite{bircher2015structural,englot2012sampling}.
% These assumptions also remove the need for collision free path planning \jln{[not clear why to me]}.
%\jln{[You might be giving too much explanations about these other papers above, lost space.]}
%

\begin{figure*}[!t]
\centering
\subfloat[]{
	\includegraphics[width=3.2in]{figures/House_Original}
	\label{subfig:gazebo_model}
}
\hfil
\subfloat[]{
	\includegraphics[width=3.2in]{figures/House_Model}
	\label{subfig:reconstructed_model}
}
\caption{Comparison of the a) Simulated Model in Gazebo \cite{gazebo} that needs to be mapped and b) Reconstructed 3D model by a mobile ground robot using our policies. Only the bottom portion is mapped due to the limited reachable space of the sensor.}%using a mobile ground robot.}
\label{fig:comparison}
\end{figure*}



In computer vision and photogrammetry, Structure from Motion (SfM) 
techniques aim at building a 3D model of a scene from a large number of images 
\cite{Frahm:2010:BRC:1888089.1888117,wu2011visualsfm,Furukawa:PAMI10:stereopsis}.
%A closely related line of work in Computer Vision and Photogrammetry relates to Structure from Motion (SfM) techniques. Given a 
%large dataset of images, these techniques build a 3D model of the scene \cite{Frahm:2010:BRC:1888089.1888117,wu2011visualsfm,Furukawa:PAMI10:stereopsis}.
But most of this work focuses on batch post-processing and in any case assumes 
%that the provided dataset guarantees the complete coverage of the scene. 
a given dataset.
On the other hand, our work focuses on actively exploring the environment
to build a complete model in real-time, with our controller taking at any time
the  current model as an input.
Naturally, eventual completeness of the model can be limited by the physical 
characteristics of the robot, and specifically the reachable space of the sensor,
see Fig. \ref{fig:comparison}.
We emphasize that 
%we discuss solely the problem of controlling the trajectory of the sensor, 
we do not discuss in details the task of actually building the model 
from the collected depth maps, which can be executed by one of the 
available vSLAM systems, such as the Real-Time Appearance Based Mapping 
package (RTAB-Map) \cite{labbe14online} that we use in our simulations. % and experiments.
%policies build the model in real-time and actively 
%explores to build a complete model. 
This package can in fact be replaced with little change to our algorithms 
by any vSLAM system based on pose-graph optimization \cite{kuemmerle11icra}. 
State-of-the-art SfM systems can also be used to post-process 
the sequence of images or depth maps captured using our policies in order 
to obtain a more accurate model.



Finally, another line of work in informative path planning relates to 
autonomous exploration and coverage of relatively large environments, using 
variants of frontier based exploration algorithms for example
\cite{shade2011choosing, shen2012autonomous, heng2015efficient, Atanasov:ICRA2015:activeSLAM}. 
While these papers focus on path planning to quickly build models of potentially large and complex spaces, 
%reconstruction of indoor environments, 
they do not address the problem of autonomously delimiting and mapping 
as completely as possible a specific bounded structure of interest.
%directed towards modeling a specific structure of interest.


%\subsection{Contribution}
%The policies presented in this paper guide a mobile ground robot equipped with 
%a depth sensor, in order to autonomously determine the boundaries of an initially unknown 
%structure, build a 3D model of the structure and attempt to fill holes in the model
%so that the reconstruction is as accurate and complete as possible.
%until the structure is accurately and completely reconstructed.
%a complete and accurate 3D model is obtained.
%order to accurately reconstruct a complete 3D model.
%Naturally, completeness of the model can be limited by the physical characteristics
%of the robot, and specifically the reachable space of the sensor.
%
%We emphasize again that our focus is solely on the control 
%of the trajectory of the sensor, while the task of actually building the model from the collected depth maps is executed by one of the available VSLAM systems, such as the Real-Time Appearance based Mapping package (RTAB-Map) \cite{labbe14online} that we use in our simulations and experiments.
%, we use Real-Time Appearance based Mapping (RTAB-Map) \cite{labbe14online} package for executing Visual SLAM
% , in part because of its highly modular structure \jln{[??]} \mani{[]}. 
%mainly due to its highly modular structure and exhaustive documentation. 
%though the policies 
%In particular, this package can be replaced with little change to our algorithms by any VSLAM system based on Pose-Graph optimization \cite{kuemmerle11icra}. 
%This is because the past trajectory of the depth sensor which produced the current partial model of the structure provides information about regions that have been previously explored but found to be empty as opposed to unexplored regions.
%\jln{[?? Is this an explanation about pgo??]}
%\mani{[wanted to explain why is pgo required. But it is better postponed to determine flaws section]}

% \jln{
% [\emph{Contributions.}
% Compared to the previously cited papers, our proposed policies are not restricted by the size or geometry of the structure, as long as the localization error remains bounded \jln{[not clear what this means]}. 
% %
% In addition, we do not assume any prior model of the structure to be mapped. ] This paragraph 
% could now be removed I think, redundant.
% }
%\mani{[Based on the literature review undertaken by the authors, this is one of the first works on autonomous inspection of structures which does not require prior model of the environment.]}

In summary, the key contributions of this work are:
\begin{itemize}
\item a motion planning strategy to autonomously determine the boundaries 
of an unknown structure using a ground robot; 
%\jln{[using a ground robot? or does it really generalize?]}
%\mani{[Can work with Aerial platforms that can follow Assumption \ref{assumption: fixed camera}. Though, the process would have to repeated for fixed heights, much like mapping along each contour of the structure.]}
\item a novel algorithm to determine incomplete portions of the partially constructed model;
\item motion planning policies for automatically exploring and adding these missing portions to the model;
\item and an evaluation of the proposed policies via simulations. % \jln{[and real experiments - remove]}.
\end{itemize}
%\mani{[Can incremental construction of OctoMap be a contribution? This is required to preserve free space and unknown region information but it is only one function.]}
%\jln{[Could write something like: In addition, we develop as building blocks for 
%our system certain variations of classical algorithms,
%such as an incremental version of Octomap that could be useful in other contexts.]}

The rest of the paper is structured as follows. 
We begin with a detailed presentation of the problem in Section II. 
In Section III we present our policies for autonomously determining the boundaries of the unknown structure.
Section IV describes algorithms for detecting the missing portions and completing the model.
In Section V, we evaluate the proposed policies via simulations and present a comparison with the classic
frontier based exploration algorithm.
Finally, we discuss avenues for future work and conclude in Section VI.
perimeterExplo.tex
% !TEX root =  ../main.tex

\section{Perimeter Exploration}

% \begin{figure}
% \begin{algorithmic}[1] 
% \Procedure{lookAhead}{plan}
% 	\State costmap $\gets$ getLocalCostmap(lookAheadRange)
% 	\State r $\gets$ getRobotPose()
% 	\For{\textbf{each} cell \textbf{in} costmap}
% 		\If{getCost(cell) $>$ COST\_LETHAL}
% 			\State $w_{\theta} \gets$ angle(cell-r)-angle(r)
% 			\If{$abs(w_{\theta})<10^{\circ}$ \textbf{and} $w_{\theta}>max\_w_{\theta}$}
% 				\State obsDetected $\gets$ TRUE
% 				\State $max\_w_{\theta} \gets w_{\theta}$
% 				\State $\phi \gets$ angle(cell-r)
% 			\EndIf\label{cVCIf2}
% 		\EndIf\label{cVCIf1}
%     \EndFor\label{cVCFor1}
% 	\If{obsDetected == TRUE}
% 		\State arm\_angle $\gets \phi$
% 		\State plan $\gets$ emptyPlan() \Comment{Stops the robot at current pose}
% 	\EndIf\label{cVCIf3}
% \EndProcedure
% \end{algorithmic}
% \caption{The lookAhead procedure is used by the local path planner to halt the robot when a portion of the structure is detected ahead}\label{alg:lpp_lookAhead}
% \end{figure}

% At the beginning of the PE phase, we assume that 
% Initial Conditions \ref{initial: robot position} and \ref{initial: FOV} are satisfied, 
% and the only information available to the system is a point cloud produced by the 
% camera in its FoR, as shown on Fig. \ref{subfig:start point camera}.
In this section, we present our first contribution - a method to autonomously determine the boundaries
of an unknown structure. From Assumptions \ref{assumption: annulus} and \ref{assumption: level 
ground}, $z^{\fr g}=0$ and $z^{\fr g}=H_{\max}$ are bounding horizontal planes 
for the model. 
%along the $z$-axis.
The remaining problem is to determine the expansion of the structure in the 
$\vect{x}_{\fr g} \vect{y}_{\fr g}$ plane. 
%For a simple polygon in 2D, this can be determined by moving parallel to the perimeter of this polygon. 
%In a similar manner, 
To do this, the robot moves clockwise around the structure by determining online a discrete sequence of 
successive goals or waypoints. 
%Then, the robot moves approximately parallel to this plane so that the camera faces the structure 
%along the normal to this plane, which brings two benefits. %The motivation for viewing along the normal is twofold. 
It tries to keep the optical axis of the depth sensor approximately perpendicular 
to the structure, which maximizes the depth resolution at which a given portion of the structure is captured, % (since resolution typically decreases with depth), 
and increases the density of captured points.
%First, for depth sensors the density of received points is typically higher   % such as a Kinect
%when viewed along the normal. Second, the depth resolution decreases with depth.
%Hence, viewing along the normal maximizes the depth resolution at which a given portion 
%of the structure is captured. 
It also tries to maintain the camera center $\fr C$ on a smooth path at a fixed distance from the structure. 
% approximately equal to $D$,
%for the reasons discussed in the previous section.\jlnComment{Which ones? Is there a more precise reference, I can't find them.}


\subsection{Determination of the next goal}	\label{section: next goal}
% \maniComment{Concave areas are also handled by Section III-C}
% \jln{[For this subsection and the next, let's talk about the case with opposite convexity. I'm worried
% about this case setting a goal behind the robot, and also the local path planner moving away from
% the structure.]}
% \mani{[Describe section III-A and III-B as path planning for camera and
% finally a corresponding path is computed for the robot and achieved using
% a controller.]}
\begin{algorithm}[t]
\caption{Algorithm for computing the next goal for the camera using the current point cloud in camera FoR.
%from the sensing and perception layer. 
% \jln{[%$n_z$ for which frame? It is defined only the second time it is used... 
% %I would call it $e_z$ to avoid confusion with $n$, and you can define it with the FoRs above? 
% I removed $\vect{r}$, didn't seem to be really used. 
% \mani{I have put back $\vect{r}$. Also, shouldn't the algorithm return $\vect{r}$ instead of $\vect{n}$?}
% Aside: I wonder if the transform
% to the global frame should not be done earlier, when defining $\vect{n}$, the confusion
% arises because all the xy planes are aligned. In fact, shouldn't we transform to the robot FoR?]}
% % \mani{[Nav Stack requires the goal and heading $\vect{r}$ in global FoR but I agree that this is an implementation detail.]}
% \mani{[Even if all degrees of freedom of the manipulator is used, $goal$ is the destination for the camera in the camera FoR and $goal^{\fr g}$ is the corresponding destination for the robot in the global FoR. ]}
}
\begin{algorithmic}[1]
\Function{computeNextGoal}{cloud\_full} 
\State cloud $\gets$ PCLremoveGroundPlane(cloud\_full)
\State cloud\_slice $\gets$ filterForwardSlice(cloud)	\label{algo: forward slice}
\State $\overline{p}^{\fr c} \gets$ PCLcompute3Dcentroid(cloud\_slice)	\label{algo: centroid}
\State $[\vect{v}_{1},\vect{v}_{2}, \vect{v}_{3}; \lambda_{1},\lambda_{2},\lambda_{3}] \gets$ PCA(cloud\_slice)	\label{algo: PCA plane fitting}
\State $\vect{\tilde n} \gets \vect{v}_3 - (\vect{v}_3\cdot\vect{z}_{\fr c})\vect{v}_3 $ \Comment{Projection on the $\vect{x}_{\fr c} \vect{y}_{\fr c}$ plane}
\label{algo: normal vector}
\State $\vect{n} \gets \vect{\tilde n} \; \text{sign}(\vect{\tilde n} \cdot \overrightarrow{{\fr C} \overline{p}^{\fr C}}); \vect{n} \gets \vect{n}/\|\vect{n}\|$
\State $\vect{r} \gets \vect{z}_{\fr c} \times \vect{n}$ %\Comment{$\vec{n_z}$ is the unit vector along z-axis}
\label{algo: direction vector}
\State $goal \gets \overline{p}^{\fr c} - D \, \vect{n} + step \, \vect{r}$	\label{algo: final goal}
% \State $goal^{\fr g}, \vect{n}^{\fr g} \gets$ transformToGlobalFrame($goal$, $\vect{n}$)
% \State \textbf{return} $goal^{\fr g},\vect{n}^{\fr g}$
\State \textbf{return} $goal,\vect{n}$
\EndFunction
\end{algorithmic}
\label{alg:gpp_computeNBV}
\end{algorithm}

%To specify the next goal, we approximate the left part of the currently visible portion of the structure by a best fit plane. 
The pseudo-code to determine the next %goal for the center $\fr C$ 
position and orientation of the camera 
in our perimeter exploration algorithm is shown 
in Algorithm \ref{alg:gpp_computeNBV}.
%The input to our algorithm, whose pseudo-code is shown on Fig. \ref{alg:gpp_computeNBV}, 
The algorithm takes as input the current point cloud produced by the camera in its FoR. 
For its implementation we rely on the Point Cloud Library (PCL) \cite{rusu20113d}.
%First, a filter downsamples the point cloud \jln{[why?]}.
First, the ground plane is removed so that the resulting point cloud 
$\mathcal P$ contains only those points that belong to the structure. 
%Following assumptions \ref{assumption: fixed camera} and \ref{assumption: level ground}, a simple test on the $z$-coordinate is sufficient to remove the ground plane. 
Next, on line \ref{algo: forward slice}, we select a subset $\mathcal S$ of the point cloud 
referred to as the forward slice, which adjoins the part of the structure that must be 
explored next, see Fig. \ref{fig:next viewpoint}. 
Concretely, we choose $\mathcal S$ so that its $y^{\fr c}$-coordinates satisfy 
$y_{\max}^{\fr c} - \frac{y_{\max}^{\fr c}- y_{\min}^{\fr c}}{3} \leq y^{\fr c} \leq y_{\max}^{\fr c}$, 
where $y_{\min}^{\fr c}$ and $y_{\max}^{\fr c}$ are the minimum and maximum $y^{\fr c}$-coordinate 
values for all points in $\mathcal P$. 
%Recall from Fig. \ref{fig:assumptions} that the $y$-axis of the camera FoR is along its imaging plane.\jlnComment{sentence could be removed I think}
%\jln{[One option that might be better would be to introduce superscripts to denote the different FoR, for example $y^c$ for the $y$ in the camera FoR.]}
%This set $\mathcal S$, also referred to as the forward slice, adjoins the part 
%of the structure that must be explored next, see Fig. \ref{fig:next viewpoint}. 
%Hence, 
%
%The next viewpoint is chosen so that the camera faces along
%the perpendicular to that plane $\Pi$ which best fits $\mathcal S$. 
%We refer to such a viewpoint as an orthogonal viewpoint (OV). 
%The normal $\vec{n}$ to this plane would therefore be the negative 
%of the desired camera pose vector. 
%
On line \ref{algo: PCA plane fitting}, following \cite{mitra2004estimating}, we compute via 
Principal Component Analysis (PCA) %Following \cite{mitra2004estimating}, 
the normal direction to that plane $\Pi$ which best fits $\mathcal S$.
%is determined by finding a normal direction to this plane 
In more details, denote $\mathcal S =\left\lbrace p_i^{\fr c} : i = 1,2,\ldots,m\right\rbrace$ 
and define the covariance matrix $\mathbf{X} = \frac{1}{m}\sum_{i=1}^{m} 
(p_i^{\fr c}-\overline{p}^{\fr c})(p_i^{\fr c}-\overline{p}^{\fr c})^{T}$, 
where $\overline{p}^{\fr c} = \frac{1}{m} \sum_{i=1}^m p_i^{\fr c}$ is the centroid 
of $\mathcal S$ computed on line \ref{algo: centroid}. 
%From the eigen value decomposition
%\jln{[why not say eigenvalue decomposition?]} 
% of $\mathbf{X}$, 
%\jlnComment{It's not clear that numerically you should use SVD to compute the eigenvalues of X a symmetric matrix}
We compute the eigenvectors $[\vect{v}_1,\vect{v}_2,\vect{v}_3]$ of $\mathbf{X}$, 
ordered here by decreasing value of the eigenvalues $\lambda_1, \lambda_2, \lambda_3$. 
The eigenvector $\vect{v}_3$ for the smallest eigenvalue corresponds 
to the normal to the plane $\Pi$.
%the eigenvector $\vec{e_3}$ corresponding to the smallest eigenvalue, which is
%the normal of the plane $\Pi$.
%the eigenvalues $\lambda_1, \lambda_2, \lambda_3$ in decreasing order
%of magnitude and associated eigenvectors $\vec{e_1}, \vec{e_2}, \vec{e_3}$. 
%Now, $\vec{e_1}$ and $\vec{e_2}$ form the basis for $\Pi$ and $\vec{e_3}$  
%is the corresponding normal 
%\cite{mitra2004estimating}. 
%The projection of $\vec{e_3}$ on the $xy$ plane, 


The algorithm returns $\vect{n}$, computed from the projection of the normal vector $\vect{v}_3$
on the $\vect{x}_{\fr c} \vect{y}_{\fr c}$ plane, and taken to point 
%towards the half space containing $\overline{p}^{\fr c}$. 
in the direction of the vector $\overrightarrow{{\fr C} \overline{p}^{\fr C}}$.
This vector $\vect{n}$ defines the desired 
%goal vector $\vect{g}_{\fr c}$ for the 
orientation of the camera.
%parallel to $\vect{n}$ and pointing towards the half space 
%containing $\overline{p}^{\fr c}$.
%Its projection on the $\vect{x}_{\fr c} \vect{y}_{\fr c}$ plane, called 
%$\vect{n}$ on line \ref{algo: normal vector}, is taken as the \jln{opposite} of 
%the next desired orientation for $\vect{x}_{\fr c}$. 
%The ambiguity in the direction of $\vect{n}$ is resolved by imposing that $(\vect{n}^{\fr c})^T \overline{p}^{\fr c}<0$ 
%\jln{[this seems to contradict your previous sentence which seems to say that the 
%orientation of $v_3$ is well defined, so I don't see why (and how) we could choose again that of $\vect{n}$. 
%Also, be careful with vectors and points, see my comments in the file]}.
%%I don't like a dot product between a vector and a point. Should be $\vec{n} \cdot \vec{C\overline{p}}$, 
%%if you had defined $C$ the center of the camera FoR]
%On line \ref{algo: direction vector}, we compute the desired direction of $\vect{r}$ of $\vect{y}_{\fr c}$ 
%(defining the orientiation of the imaging plane) at the next goal $g$, 
%such that the exploration proceeds clockwise. %consistently 
%The cross-product $\vec{r} = \vec{n} \times \vec{n_z}$ of $\vec{n}$ with the unit vector 
%along the z-axis $\vec{n_z}$ provides the desired heading $\vec{r}$ for the robot 
%in the clockwise sense. 
%\jln{[Perhaps you can spend more time defining
%the FoRs above, including the unit vectors you need like $\vec{n_z}$. Then
%you could be more succinct here.]}
%Finally, the next goal 
% \maniComment{Have to discuss about adding $\vect{r}$}
%\jln{[Again, you cannot use $\vect{n}$ in the following because we don't know its direction, do we?]}
The algorithm also returns the next goal point %which is computed on line \ref{algo: final goal} 
$goal = \overline{p}^{\fr c} - D \, \vect{n} + step \, \vect{r}$ for the center $\fr C$ of the camera, 
where $D < \rho$ is the desired distance between the camera and the structure, 
$\vect{r} = \vect{z}_{\fr c} \times \vect{n}$ is computed on line \ref{algo: direction vector},
and $step = \frac{y_{\max}^{\fr c}- y_{\min}^{\fr c}}{6}$.
%
The term $step \, \vect{r}$, which is along the plane $\Pi$, is used to shift the $goal$ forward 
so that both sections of a corner fall in the FOV of the camera, as in the situation shown on Fig. \ref{fig:next viewpoint}. This prevents the algorithm from making slow progress around
corners.
%
%The vector $\vect{r}$ 
%The cross-product $\vect{r} = \vect{n} \times \vect{z}_{\fr c}$, computed on line
%\ref{algo: direction vector}, 
%provides the heading for the robot, 
%along the plane parallel to $\Pi$, in the clockwise sense. 
Finally, the computed camera pose is transformed into the global FoR to obtain the 
goal point $g^{\fr g}$ for the camera center $\fr C$.
We simplify the notation $g^{\fr g}$ to $g$ in the following, where we work in the
global reference frame.


% Finally, the computed camera pose
% %, with position at $goal$ and oriented along $\vect{g}_{\fr c}$,
% is used to determine the corresponding goal pose for the center $\fr R$ of 
% the robot, positioned at $g$ and oriented along $\vect{g}_{\fr r}$.}
% The value of $step$ is taken to be half the width of $\mathcal{S}$ or $\frac{y_{\max}^{c}- y_{\min}^{c}}{6}$.

%\jln{[Can we use $D$ in Assumption $2$ and get rid or $\rho$ there, or is the robot 
%at any point varying its distance to the structure autonomously? If $D$ is not fixed, 
%it should be an input to the algorithm and chosen by the robot somehow.]}
%A good choice of value for $D$ is the depth corresponding to the 
%optimal depth resolution of the camera. 
%\jln{{[Remove $\cdot$ to write $D \, \vec{n}$ 
%if $D$ is a scalar. Also, $\vec{g}$ is a point so write $g$ (a point plus a free vector 
%gives a point, not a vector): $g = \overline{p} + D \, \vec{n}$.]}}
%but the value of $D$ is constrained by the amount of clearance present around the structure, see Assumption \ref{assumption: annulus}. 
%\jln{[We kind of have two definitions of $D$ at this point...]}.
%%The third term is used to shift the goal forward along the new heading $\vec{r}$. 
%The value of $step$ is taken to be $\frac{y_{\max}^{c}- y_{\min}^{c}}{6}$ in our algorithm. 
%Fig. \ref{fig:next viewpoint} illustrates the computation of $\vec{g}$ when  a corner section of a structure is seen.
%\jln{[I feel that if we need space,
%the discussion above could be more succinct. Refer more to the lines in the Algorithm
%rather than repeating them perhaps.]}
%Finally, the computed goal is transformed into the global FoR and sent to the robot's navigation stack for execution. 
%The green arrows in Fig. \ref{subfig:look_ahead} and Fig. \ref{subfig:end_first_pass} show the computed next goal for the respective positions of the robot. 
%\jln{[Again these screenshots are hard to read. Think about using an illustrative manual representation instead.]}

\begin{figure}[!t]
\centering
\includegraphics[width=3.0in]{figures/Next_Viewpoint}
\caption{Top-down view illustrating the computation of the next goal. We show here
the situation for a corner section of the structure. The forward slice $\mathcal{S}$ is highlighted in 
red and the corresponding best fit plane $\Pi$ is shown as well. 
%The computed goal $g$ is shown as a green arrow.
%\jln{[g is a point, not a vector. Do not use an arrow.
%On the other hand $n$ is a vector, so you need to show its direction.]}
% \jln{[Fig. to discuss: 
% % add $\bar p$ and $\vect{n}$ labels to illustrate lines 4, 6. Replace $X, Y$ by $x^{\fr{c}}, y^{\fr{c}}$.
% Mark the goal point $g$ and the goal vector $\vect{x}_{\fr c}$. Remove the black arrow
% pointing towards the robot, since this could go the other way if the geometry
% has a different convexity.
% \mani{[Wouldn't it still point away from the structure?]}
% Also, with this orientation its unclear that the left red line is actually visible\ldots
% ]}. 
}
\label{fig:next viewpoint}
\end{figure}

%\jln{[The rest of this section is way to wordy and confusing. You need to be
%succinct and to the point, and avoid philosophical discussion. Plus, I don't get your potential
%field definition. Normally, you have a cost function $N(x)$ expressing the goal $\|x-g\|^2$,
%not $\|x-x_0\|^2$, as well as the obstacle potential field (typically we take them 
%to blow up close to the obstacles, say as $1/d_j(x)$, where $d_j(x)$ is the distance between
%$x$ and the obstacle $j$, whereas $x_j$ here is not properly defined because an obstacle
%has spatial extension. Please check
%the textbook by Thrun, Choset et al. for example. Then, we follow the negative gradient 
%of the potential field to reach the min cost. So for example for the term 
%$\frac{1}{2}\|x-g\|^2$ only, the gradient descent gives 
%$\dot x = -\nabla N(x) = \|x-g\| (g-x)$,
%in other words a vector field attracting the robot position $x$ towards
%$g$.]}


\subsection{Local path planning to the next goal}	\label{section: local path planner}
%Recall that the maximum height of the structure that is captured depends on the vertical
%angle of view and the distance of the robot from the structure. 
%
%The shortest path from the robot's current position $\vec{x_0}$ to the next goal $\vec{g}$
%An additional requirement is that every point on the path from the robot's
%current position to $\vec{g}$ maintains a perpendicular distance of $D$ from
%the structure. Consequently, every point would also be an OV. 
%
%For the situation shown in Fig. \ref{fig:next viewpoint}, the shortest path takes the
%robot closer to the structure and consequently the entire height of the structure
%may not be captured. 
%
%The construction of $\vec{g}$ ensures that it is an OV. 
%\jln{[Is that really true, for example for Fig. 5? The goal $g$ itself
%will not be an OV once it is reached if the right face is not visible
%any more.]} 
%that every point along the path between successive viewpoints is also an OV,
%
%\jln{I would talk about goal configuration for the camera (position+orientation), since this
%is really what you specify.}
In order to move the camera center $\fr C$ to $g$ while keeping it approximately
at the desired distance $D$ from the structure along the way, we use a 
%Instead, \jln{the robot moves to $g$} using a global \jln{[local?]} 
local path planner based on potential fields \cite{khatib1986real, Choset_2005_5167}.
%that maintains a constant distance $D$ from the structure.  
%are also OVs. To ensure this, we adapt the classic Potential Field approach for
%path planning \cite{khatib1986real}. 
A potential function encoding the structure as obstacles in the neighborhood 
of the camera, as well as the goal $g$, is sampled in the form of a cost map on 
local 2D grid of size $2\rho \times 2\rho$ centered on the camera's current position, %also called costmap, 
%of size $2\rho \times 2\rho$ with each cell of size $r\times r$ that is 
%is used to represent the structure as obstacles in the neighborhood of the robot, 
see Fig. \ref{fig:navigation function}.
%Measurements from the obstacle detection sensors of the robot are used 
%to update this cost map so that each cell has an occupancy value. 
%\jlnComment{removed comment about sensors: let's talk about this, should not ask too much}
%
Assumption \ref{assumption: annulus} guarantees that all 
the occupied cells in this cost map denote the structure itself. 
%
%Now, given a goal $g$, %and the current position of the robot $x$,
%a simple navigation
For $k$ occupied cells centered at $\{x_j\}_{j=1}^k$, the potential function $N(x)$ is defined as
\begin{equation} 		\label{eq: N definition}
N(x) = \alpha \|x-g\|^2 + \sum_{j=1}^{k}I_j(x) d_j(x),
\end{equation}
\begin{equation*}
\text{with } d_j(x) = \frac{1}{\beta\|x-x_j\|}; 
I_j(x) = 
 	\begin{cases} 
	    1 & \text{if } \| x - x_j\| \leq D \\
		0 & \text{otherwise,}
	\end{cases}
\end{equation*}
for some scalar parameters $\alpha, \beta$.
Here $d_j$ is the repulsion from the $j^{th}$ occupied cell, % present at $x_j$.
%A typical implementation of this algorithm uses a costmap to keep track of 
%static obstacles using a 2D occupancy grid \jln{[Ref? I don't think you should assume readers
%know what you mean by costmap.]}.
%Additionally, the influence of each occupied cell is limited
and is limited by $I_j$ to a neighborhood of radius $D$ around the cell.
%, also called its $D$-neighborhood. 
A path for the camera is obtained by following the negative gradient of $N$, i.e., 
$\dot x = -\nabla N(x)$.
%the goal $\vec{g}$ and moving along the gradient of $N(\vec{x})$ until the 
%path reaches the starting point $\vec{x_0}$. Therefore, the orientation of 
%the robot at any point $\vec{x}$ along this path is given by the gradient of 
%$N(\vec{x})$, which can be written in the form
Denoting $J_{x} = \left\lbrace j : I_j(x) = 1 \right\rbrace $ the occupied cells in the 
$D$-neighborhood of $x$, we have
% \begin{equation}
% -\nabla N(\vec{x}) = 2 \alpha \|\vec{x}-\vec{g}\|(\vec{g}-\vec{x}) - \frac{2}{\beta\|\vec{x}-\vec{x}_{j^*}\|^2}(\vec{x}-\vec{x}_{j^*})
% \end{equation}
% \begin{equation}
% \frac{-2}{\beta\|\vec{x}-\vec{x}_{j^*}\|^2}(\vec{x}-\vec{x}_{j^*}) = \sum_{l \in J_{\vec{x}} } \frac{-2}{\beta\|\vec{x}-\vec{x}_l\|^2}(\vec{x}-\vec{x}_l)
% \end{equation}
\begin{equation}
-\nabla N(x) = 2 \alpha (g-x) + \sum_{i \in J_{x} } \frac{1}{\beta\|x-x_i\|^{3}}(x-x_i).
\label{eq:gradient}
\end{equation}
% where $\vec{x}_{j^*}$ is the resultant repulsion caused due to all the occupied 
% cells in the $D$-neighborhood of $\vec{x}$.
%, $A$ is the magnitude of attraction towards
%$\vec{x_0}$ and $B$ is the magnitude of repulsion from the resultant $\vec{x_{j^*}}$.
%\maniComment{Corrected first term as well.}
%\jln{[I don't agree with your exponent $2$ instead of $3$ in the sum above.
%Take for example the 1D case. With your formula you get $(x-x_i)/|x-x_i|^2 = 1/(x-x_i)$, instead
%of $1/(x-x_i)^2$.]}

Let $Q = \left\lbrace x : J_{x} \neq \emptyset \right\rbrace $ denote the region 
that is at distance at most $D$ from the structure. Assuming a small value of $\beta$, the
summation term in \eqref{eq:gradient} is dominant whenever $x \in Q$ and pushes
the path away from the structure. However, this term vanishes as soon as $x \notin Q$.
Then, assuming that the camera starts at $x_0$ on the boundary $\partial Q$ of $Q$, it remains
approximately on $\partial Q$ if $g-x$ points toward the interior of $Q$. 
% \maniComment{$g$ can't be far away from $Q$. Measurement error and localization error are only sources.}
It is possible that this condition is not satisfied by the point $g$ computed in the 
previous subsection, in which case we replace $g$ by $g_1$, which is obtained by 
selecting a new $goal = \overline{p}^{\fr c} - D' \, \vect{n} + step \, \vect{r}$ %to the global FoR
%\jln{[where is step r and the transformation to global FoR?]}
for $D'<D$ such that this condition is satisfied. The path will then slide on $\partial Q$
until %it intersects the line $\bar p + \mathbb R \vect{n}$ \jlnComment{what about step $r$?} \cite{Cortes:IEEE08:nonsmoothDS}.
%\jln{[Consider: The path will then slide on $\partial Q$ until }
it reaches its goal \cite{Cortes:IEEE08:nonsmoothDS}.
% \mani{[The possibility of $g \in Q$ needs to be discussed.]}
%
%A small value for $\beta$ makes the summation term in \eqref{eq:gradient} dominant 
%whenever $J_{x} \neq \emptyset$, see Fig. \ref{fig:navigation function}. 
%Let $Q = \left\lbrace x : J_{x} \neq \emptyset \right\rbrace $ denote the region 
%that is closer than $D$ to the structure and $L_{x_1x_2}$ denote the line 
%segment between $x_1$ and $x_2$. A valid path $\mathcal A$ following gradient descent
%is such that $Q \cap \mathcal A$ is minimum. Consequently, the distance of the path
%from the structure is maintained at $D$ if 
%% effectively inflates each occupied cell by a radius $D$
%% and forces the resulting path to maintain at least $D$ distance from the structure,
%% see Fig. \ref{fig:navigation function}. Additionally, if
%% \begin{equation}
%% \label{eq:goal direction}
%% (\vec{g}-\vec{x}_0)\cdot(\vec{g}-\vec{x}_{j^*}) \geq 0
%% \end{equation}
%% \begin{equation}
%\begin{align}
%L_{x_0 g} \cap Q &\neq \emptyset \label{eq:goal direction}\\
%x_0,g &\in \partial Q \label{eq:goal conditions}
%\end{align}
%% \end{equation}
%% the path remains exactly $D$ distant from the structure. 
%However, it is possible that $g \in Q$. In which case, the path $\mathcal A$
%is trimmed to new goal $g_1$ such that $g_1 \in \partial Q$. Since
%$x_0$ is the goal from the previous iteration, $x_0 \in \partial Q$. 
%Finally, observe that our computation of $g$ ensures that the goal is close
%to $Q$. 
% Therefore, the computed path is trimmed to a new goal $\vec{g'}$ by 
% checking the value $I_j(\vec{x})$ for each point in the path, see Fig.
% \ref{fig:next viewpoint}.
Finally, this path for the center $\fr C$ of the camera is used to compute 
a corresponding path for the center $\fr R$ of the robot that needs to be 
tracked using a platform specific controller.
% , whichs hould guarantee a maximum deviation for adequate performance.
%which adheres to the global plan with a bounded maximum deviation $\tau$. 
%As a result, the maximum height of the structure that is guaranteed to be captured is given by
%$H_{\max} = 2(D-\tau)\tan{(\phi/2)}$.
%\jln{$H_{\max}  = z^{\fr g}_{\fr C}+(D-\tau) \tan{\phi/2}$}.
% As a result, the maximum height of the structure that is guaranteed to be captured is 
% also marginally reduced.
%We will refer to those cells which have at least one 
%occupied cell in its $r$-neighborhood as \emph{adherent cells}.
%By choosing a small value for $\beta$, the scalar $B$ would be large and 
%therefore the repulsion would be the dominant term for all adherent cells. 
%For the remaining free cells which are at least $r$ meters away from the 
%structure, the gradient points towards $\vec{x_0}$. Now, if $\vec{g}$ is an 
%adherent cell, then the gradient is along $\vec{x}-\vec{x_{j^*}}$ which 
%takes the path to a free cell. At this point, if 
%\begin{equation}
%\label{eq:goal direction}
%(\vec{x}-\vec{x_0})\cdot(\vec{x}-\vec{x_{j^*}}) \geq 0
%\end{equation} 
%then the attraction and repulsion from $\vec{x_0}$ and $\vec{x_{j^*}}$ 
%would balance such that the path sticks to the boundary of adherent cells. By 
%setting the value of $r$ to $D$, the computed paths would always remain at 
%least $D$ meters away from the structure and consequently each point on this path would 
%also be an OV, see Fig. \ref{fig: navigation function} \jln{[??]}. Geometrically, Eq. %\ref{eq:goal direction}
%would hold if $\vec{g}$ is in the right half plane in Robot FoR. Moreover, the
%definition of OV is with respect to a plane that approximates $\mathcal{S}$. Therefore,
%points on paths which take a small negative value for Eq. \ref{eq:goal direction} are
%also considered OV. The remaining case is when the $y$-coordinate of $\vec{g}$ has a large
%positive value in Robot FoR. This can not occur since the camera is 
%pointed to the right of the robot.

\begin{figure}[!t]
\centering
\includegraphics[width=3.5in]{figures/potential_field}
\caption{The potential field for a goal at $(4,3)$ with $D=3$ is shown 
as a heat map and the corresponding gradient vectors are shown as a vector field.
%\jln{[Cut the left of $x=-2$
%to get a better zoom on the right part?]}
%
%\jln{[With the robot turning clockwise, in the FoR of the robot, can the goal really be in the lower left quadrant?]}
%\mani{Add labels and title for the plot}
%. The computed path is shown as a black line.\mani{Overlay with the vector field if possible}
%\jln{[I don't understand this figure. It would be useful to show $\Pi$ for example.]}
}
\label{fig:navigation function}
\end{figure}
%A typical implementation of this algorithm uses a costmap to keep track of static obstacles using a 2D grid.
%In particular, a local costmap contains the occupancy 
%information of the region around the current position of the robot. 
%This information is obtained from sensors such as LIDAR, RADAR and 
%ultrasonic range finders. \mani{In general, most robots use a 
%Navigation Function based global path planner and a platform specific local
%path planner.Hence, we expand the costmap up to $D$ meters with 
%exponentially decreasing cost. As a result, the global path which is 
%compputed using Gradient Descent algorithms is forced to stay $D$ meters 
%away from the structure as long as possible,} 
%illustrated in Fig. \ref{fig:exploration}.

%This way, our policies remain generic to the type of mobile robot used for 
%carrying the camera. But due to the large step size, the actual trajectory 
%followed by the robot might not ensure that the camera stays a constant $D$
%meters away from the structure, especially while negotiating corners. On
%the other hand, reducing the step size would increase the number of 
%decisions and consequently decrease the overall speed of the robot. We
%efficiently get around this problem by modifying the costmap parameters 
%used by the robot's navigation stack.



\subsection{Replanning due to the structure interferring} \label{section: obstacle ahead}

%%%%%

\begin{figure}[!t]
\centering
\subfloat[ ]{
	\includegraphics[width=3.0in]{figures/Lookahead}
	\label{subfig:look_ahead_gazebo}
}
\hfil
\subfloat[]{
	\includegraphics[width=3.0in]{figures/LookAheadImage}
	\label{subfig:look_ahead_camera}
}
\caption{ 
a) While the robot is following the structure, its forward facing sensors detect
an obstacle ahead (robot configuration shown in faded colors). This obstacle is 
outside the field of view of the camera, shown in b).
%The position of the robot when the structure ahead is detected is shown in faded colors. 
%At that point, the system detects the structure ahead, which is outside the field of view of
%the camera, shown in b).
The position of the robot at the next waypoint along the new direction to explore, 
determined by using the arm to scan ahead, is shown in bright colors.}
% As the system detects a portion of the structure ahead of the robot, 
% the camera rotates and scans ahead to determine the new direction to explore. 
%b) Image captured by the camera when the system detects the structure ahead.   
%\jln{[$R$ was used for range. I changed that above.]}
\label{fig:exploration}
\end{figure}

%%%%%

\begin{figure}[!t]
\centering
\includegraphics[width=3.0in]{figures/concave_surfaces}
	\caption{When the robot is currently following section A of the structure, a later section B of the structure 
could interfere with the planned path. 
%     A non-convex corner formed by sections A and B  of the structure is detected using range finding sensors ahead of the robot. 
In general, the angle $\omega$ made by section B with respect to section A satisfies $\omega \in [0,\pi)$. Also, the two sections could be connected to
form a non-convex corner.
% \jln{[Is $\theta$ on the figure your $\psi$? Explain what it is. Certainly not the
% $\theta$ of paragraph D...]}
}
\label{fig:concave_surfaces}
\end{figure}

%%%%%

Assumption \ref{assumption: annulus}
guarantees that the robot can move sufficiently freely around the structure, but this does not
prevent the structure itself from interfering with the path planned above.
%The exploration policy as described so far uses only the information provided 
%by the depth sensing camera. Moreover, these cameras typically have a limited field of view \jln{[and so?]}. 
Consider the situation shown in Fig. \ref{subfig:look_ahead_gazebo}. The wall ahead of the robot does 
not fall into the FOV of the camera due to the limited horizontal angle of view, yet the robot should not approach 
this wall closer than a distance $D$.
%, shown in Fig. \ref{subfig:look_ahead_camera},
%when the robot is at a distance $D$ from the wall ahead, yet the robot should not move
%closer to the structure. 
%This limitation, due to the limited horizontal angle of view of the camera, is
%removed by using the cost map constructed previously.
%On the other hand, a number of range finding sensors are used by the robot's 
%navigation stack for obstacle avoidance. 
%If cells along the current heading, within the range of the camera, are occupied,
Hence, if the robot detects obstacles in its $D$-neighborhood, it is stopped at
its current position and the yaw motion of the camera is used to scan ahead and face the 
new section of the structure. 
More precisely, as illustrated in Fig. \ref{fig:concave_surfaces}, we use the costmap from the previous subsection 
to turn the camera to face along the direction from the robot center $\fr R$ to the first occupied cell in 
the $D$-neighborhood of the robot. The next goal is then recomputed using the newly 
captured point cloud. 
% \mani{As illustrated in Fig. \ref{fig:concave_surfaces}, when the robot is following a section A of the structure,
% a previously unseen section B can intersect the $D$- neighborhood of the robot only
% at an angular position $\phi \in (-\frac{\pi}{2}, \frac{\pi}{2}]$.
% where $\psi$ is a margin for ignoring the currently following section} \jln{[??]},
% see Fig. \ref{fig:concave_surfaces}. 
% In particular, for the case shown in Fig. \ref{fig:concave_surfaces}, the robot detects obstacles at $\phi = -\frac{\pi}{2} + \omega$ and therefore, the yaw motion of the camera is used to face the structure along the angle 
% $\phi$ with respect to the robot.} 
% \mani{Recall that we assume that the robot can indeed detect obstacles ahead of it at a distance $D$.
% Hence, if the robot detects obstacles in its $D$-neighborhood, 
% satisfying the above condition on the angular position of the obstacle,
% the robot is stopped at its current position and the yaw motion of the camera 
% is used to face the structure at that maximum angle $\phi$ at which an obstacle was
% detected.} 
%\jlnComment{Still don't understand these 2 sentences, nor what $\psi$ is}
%

% \maniComment{covers all concave junctions}
%Alternatively, if the platform does not have a robotic arm, then the robot is
%turned in place until the camera faces the structure ahead. % this contradicts the assumption on the yaw motion being available
%Moreover, the camera is brought 
%back to point to the right of the robot once the new goal is computed.
% \jln{[You had:
% ``Note that this goal $\vec{g}$ does not satisfy \eqref{eq:goal direction} and therefore
% some points on the path would be more than $D$ distant from the structure''.
% I don't get this. We are stopping because the camera is at $D$ from the wall ahead.
% So it's not clear that we'll move away by more than $D$.]
% }
% \mani{[Assume the range $R$ of both LIDAR and camera is greater than $D$. Now, 
% using costmap we can lookahead by upto $R$. I was using $R$ for the lookahead
% and hence my confusion. I have corrected it to $D$ now.
% ]}
% \mani{[Need to explicitly mention about concave structures here.]}
%Since these cases occur only at few places, this does not significantly
%affect the system. 
%(illustrated in Fig. \ref{subfig:look_ahead}).
% The range of this \uwave{look-ahead} is determined by the 
%size of the local costmap and is bounded by the range of the camera used \jln{[??]}.
% We call this action of scanning ahead and following a new direction of 
% exploration as \emph{discontinuities}. 
%Note that there are two possible cases
%when a discontinuity occurs. First, the location could be a concave corner of 
%the structure and this corner would be mapped when the camera scans using the
%yaw motion. In the second case, if the range of the camera is sufficiently 
%long, then the system might skip entire sections of the structure 
%as shown in Fig. \ref{subfig:look_ahead}. These regions, which we refer to as 
%cavities, will be explored in the CE phase.


\subsection{End of the PE phase}

\begin{figure}[!t]
\centering
	\includegraphics[width=3.0in]{figures/end_of_first_pass_diag}
	\caption{The angle $\theta$ subtended by the path $\widehat{\fr O \fr R}$ at a reference point $\fr P$ 
    inside the structure can be used to detect the end of the PE phase.
}
\label{fig:end_first_pass}
\end{figure}

The end of the PE phase is determined by monitoring the angle $\theta$ 
subtended by the robot's trajectory with respect to a reference point $\fr P$
inside the structure, see Fig. \ref{fig:end_first_pass}. 
The point $\fr P$ is chosen from the first depth image at the start of the PE phase. 
As shown in Fig. \ref{fig:end_first_pass}, the ray 
$\fr P \fr R$ completes one full rotation in the clockwise manner with respect to ray 
$\fr P \fr O$ only at the end of the first pass. 
%\jln{[Define O, perhaps much earlier above in the problem statement.]}
Note that $\theta$ need not monotonically increase and it depends on the geometry 
of the structure. But since by Initial Condition \ref{initial: robot position} 
there exists a plane $NN'$ that initially separates the robot 
and the structure, $\theta$ can complete a full rotation only at the end 
of the first pass. Additionally, after $\theta$ completes a full rotation, 
we continue to follow the structure until the vSLAM module achieves a global loop closure.

Overall, our strategy attempts to maintain as much as possible a viewpoint 
orthogonal to the structure, even though it replans for a new goal according
to Algorithm \ref{alg:gpp_computeNBV} only at discrete times.
%Optionally, $\vec{g}$ can be shifted forward along the new heading 
%$\vect{r}$ by $\frac{y_{\max}^{c}- y_{\min}^{c}}{6}$ in order to speed up the exploration.
%This also facilitates a hierarchical planner with the global path planner providing a platform independent path based on our policy
%and a platform specific local path planner robustly following this global plan.
Note that only the computation of the next goal happens at discrete instants but the vSLAM 
module updates the model at a higher rate as per the capabilities of the hardware. 
%\jln{[this last sentence should be moved to later in the text because it is not understandable at this point.]}


%Our policies make the robot skip cavities and remain along the 
%perimeter of the structure for the following reasons. 
%\jln{[This is getting way too long!]}
%Firstly, this reduces the time to achieve the first loop closure for the \jln{vSLAM} system. 
%Owing to our exploration policy, the system continuously sees unexplored 
%regions of the structure. Thus, the uncertainty in the constructed model 
%remains high due to lack of loop closures. The system is assured to obtain a 
%loop closure only at the end of the PE phase when the robot returns to the 
%neighborhood of its starting point. Secondly, the uncertainty in the model 
%is comparable to the size of these cavities. Thus, mapping these confined 
%spaces is better suited for the second pass. On the other hand, if the 
%localization accuracy is sufficiently high, then the range of the look-ahead 
%can be scaled down thereby controlling the size of cavities that would be 
%skipped. In this way, the robot moves along the perimeter of the structure 
%while the Visual SLAM system simultaneously builds a 3D model. 
%\jln{[Is all this discussion really necessary? One, it's getting too philosophical.
%Two, it's pretty hard to get you point, even for someone like me who is 
%familiar with the problem.]} 
simulations.tex
% !TEX root =  ../main.tex

\section{Simulations and Results}

We evaluate the performance of our policies via 3D simulations for different sizes of the 
structure, camera range values and localization accuracy levels for the robot.
% by comparing with the classic frontier based exploration algorithm. Additionally, we also 
% evaluate the ability of our policies to handle 
%Our motion planning policies are implemented as a global and local path planner and 
The implementation of our motion planning policies is 
integrated with the Robot Operating System (ROS) Navigation Stack \cite{rosnav}, %\jln{[ref to ROS webpage]}
which is supported by many mobile ground robots.
All the simulations are performed using the Gazebo simulator \cite{gazebo}. 
%on a Dell OptiPlex desktop \mani{[Is specs of computer required?]}. 
The vSLAM algorithm used is RTAB-Map \cite{labbe14online}.

The simulations are carried out with publicly available models of a 
Clearpath Husky A200 robot and a Kinect depth sensor whose range
can be varied \cite{gazebo_plugins}, see Fig. \ref{fig:initial conditions}. 
A UR5 robotic arm is used to carry the sensor, but only yaw motions of the 
arm are allowed, as described in Section \ref{section: statement}.
%A Husky robot fitted with a UR5 robotic arm is used as the mobile manipulator in the
%simulations. A simulated Kinect sensor, available as part of Gazebo Plugins Package
%\cite{gazebo_plugins}, is used in the simulations which allows the range to be 
%varied using a parameter.
For illustration purposes, we consider artificial structures made of short wall-like 
segments. We refer to the structure used in most of the previous illustrations as the 
Small $\Gamma$ model. The Large $\Gamma$ model has the same shape as Small $\Gamma$ 
but is twice the size. We also illustrate the effectiveness of our policy
for a realistic model of a house, and compare its performance with that of the 
classic Frontier-Based Exploration (FBE) algorithm \cite{Yamauchi:1997:FAA:523996.793157}.
We have included a supplementary MP4 format video, which shows the simulation of a Husky robot following our policies for mapping the Small $\Gamma$ model using a Kinect sensor with a range of $4.5\text{m}$. % This will be available at http://ieeexplore.ieee.org.

% \begin{itemize}
% \item Husky Robot setup	- high localization error
% \item Turtlebot Robot setup - low localization error
% \item SmallJ world - small structure, single cavity
% \item LargeA world - large structure, with multiple sized cavities
% \item Range 4m - Typical range of depth sensing camera
% \item Range 12m - Kinect v2 under ideal conditions
% \end{itemize}

%\subsection{System Architecture}
% Give details of the robots used - Turtlebot and Husky. 
% The proposed policies are for a Mobile ground robot that is equipped with a depth sensor for mapping.
% Hence, we test our policies, using simulations on a Husky robot that is fitted with a Mico Arm and a Kinect sensor mounted on it. 
% The Husky robot comes equipped with a Global Positioning System (GPS) for localization in outdoor environments. 
% Additionally, it is equipped with an Inertial Navigation System(INS) composed of an IMU and wheel encoders. 
% An Extended Kalman Filter implementation is used to fuse measurements from the INS and GPS modules to provide a bounded error localization. 
% Additionally, we have a SICK LIDAR for detecting obstacles and building a 2D costmap that is used by the Navigation Stack which runs on an on-board computer. 
% The visual SLAM system and our motion planning policies are implemented on a separate Laptop on-board which could optionally broadcast messages over Wi-Fi for remote visualization purposes. 
% We use the Robot Operating System (ROS) throughout our work and the simulations are performed using Gazebo Simulator. 
% We have developed our policies as a ROS package consisting of a Global Path Planner and a Local Path Planner which are to be used with the ROS Navigation stack.

% The system architecture described here is just a sample implementation but our policies are agnostic to the hardware used. 
% Hence, our system can be readily adapted to run on any robot that can be controlled via the ROS Navigation Stack. 
% Additionally, our system only requires an assembled point cloud from the external visual SLAM. Hence, the depth sensor can be replaced by stereo cameras or sequence of images or any other sensor which is supported by RTAB-Map. 
% \mani{Need to add exact specifications of the hardware}

% \begin{figure}[!t]
% \centering
% \includegraphics[width=2.5in]{husky_with_mico_arm.jpg}
% \caption{The test platform used for the experiments}
% \label{fig:robot_setup}
% \end{figure}

\subsection{Structure Size and Camera Range}

\begin{table}[!t]
% increase table row spacing, adjust to taste
\renewcommand{\arraystretch}{1.3}
% if using array.sty, it might be a good idea to tweak the value of
% \extrarowheight as needed to properly center the text within the cells
\caption{Simulation results for different sizes of the structure and range of the camera}
\label{tbl: simulation results}
\centering
% Some packages, such as MDW tools, offer better commands for making tables
% than the plain LaTeX2e tabular which is used here.
\begin{tabular}{|c|c|c|c|c|}
\hline
Model & Perimeter & Camera Range & Path Length \\
\hline
Small $\Gamma$  & $42\text{m}$ & $4.5\text{m}$ & $72.08\text{m}$  \\
Small $\Gamma$  & $42\text{m}$ & $12.0\text{m}$ & $53.79\text{m}$ \\
Large $\Gamma$  & $84\text{m}$ & $4.5\text{m}$ & $106.23\text{m}$ \\
Large \large{a} & $94\text{m}$ & $4.5\text{m}$ & $179.65\text{m}$ \\
\hline
\end{tabular}
\end{table}

The relative size of the structure with respect to the range of the camera affects the trajectory
determined by our algorithms. Fig.~\ref{fig:relative size} shows simulation results for $4$ scenarios.
With a camera range of $4.5\text{m}$, the Large $\Gamma$ model is completely mapped at
the end of the PE phase. For the Small $\Gamma$ model a cavity remains, which is subsequently
explored during the CE phase. 
%On the other hand, increasing the range of the camera to say $12\text{m}$ also modifies the trajectory followed by the robot, 
Increasing the camera range to say $12\text{m}$ allows the Small $\Gamma$ structure to be mapped
at the end of the PE phase as well, see Fig. \ref{subfig:small gamma husky 12}. 
Fig. \ref{subfig:large a husky 4.5} shows that the robot following our policies is able 
to map large structures with multiple cavities of different sizes. 
Table \ref{tbl: simulation results} lists the path lengths obtained for the different test cases.
%The time taken to determine the flaws increases with increase in the size of the structure. 
% The details of the structures used in the simulations are shown in Table 
% \ref{tab: structure details}. 
%\mani{[Add a table with structure size, voxels in the model, time to compute frontiers, length of the path taken]}
% The height of the structure is kept constant while the expansion of the structure
% in the $xy$ plane is increased. Also, more number of cavities of different 
% geometries are added to these artificial structures. As shown in Fig. 
% \ref{subfig:results Husky LargeA}, our policies achieve full coverage for large 
% structures with various cavities. 

% Time to compute flaws would increase with size of structure. 
% Comparison between Small structure with one cavity and a large one with multiple cavities.
% Height of the cavity is limited but only the expansion of the structure is
% increased. One large square cavity and another narrow long cavity.
% Algorithm performs equally well for large and small structures. With bounded
% localization, the vSLAM system performs well for large and small structures.
% Show the two re

%\begin{figure}[t]
%\subfloat[Large and narrow cavities]{
%	\includegraphics[width=1.5in]{ReconstructedModelHuskyLargeA.eps}
%	\label{subfig:results Husky LargeA}
%}
%\hfil
%% \subfloat[]{
%% 	\includegraphics[width=1.8in]{ReconstructedModelHuskyLargeA.eps}
%% 	\label{subfig:results Husky LargeA}
%% }
%% \hfil
%\subfloat[multiple narrow cavities]{
%	\includegraphics[width=1.5in]{ReconstructedModelHuskySwastik.png}
%	\label{subfig:results Husky Swastik}
%}
%\caption{
%% Reconstructed model using our policies for a Husky robot with low localization
%% accuracy and a Turtlebot platform with high localization accuracy. 
%Our policies are able to handle complex shapes with different cavity sizes and locations}
%\label{fig:simulation results}
%\end{figure}

% \subsection{Range of the Camera}

% the range of the camera determine what would be a cavity and not. 
% Show that with a long range camera, small cavities are mapped in the first phase itself?
% Definition of what makes a cavity depends on the relative scale between
% robot length and camera range?

\begin{figure}[t]
\subfloat[Model: Small $\Gamma$, Range: $4.5m$]{
	\includegraphics[width=1.5in]{figures/SmallGamma_Husky_4_5}
	\label{subfig:small gamma husky 4.5}
}
\hfil
\subfloat[Model: Small $\Gamma$, Range: $12m$]{
	\includegraphics[width=1.5in]{figures/SmallGamma_Husky_12}
	\label{subfig:small gamma husky 12}
}
\hfil
\subfloat[Model: Large $\Gamma$, Range: $4.5m$]{
	\includegraphics[width=1.5in]{figures/LargeGamma_Husky_4_5}
	\label{subfig:large gamma husky 4.5}
}
\hfil
\subfloat[Model: Large {\large a}, Range: $4.5m$]{
	\includegraphics[width=1.5in]{figures/LargeA_Husky_4_5}
	\label{subfig:large a husky 4.5}
}
\caption{
The projection of the reconstructed model on the $\vect{x}_{\fr g}\vect{y}_{\fr g}$ plane is shown in black and the trajectory followed by the robot based on our policies is shown in blue. 
}
\label{fig:relative size}
\end{figure}

\subsection{Localization accuracy}

\begin{table}[t]
% increase table row spacing, adjust to taste
\renewcommand{\arraystretch}{1.3}
% if using array.sty, it might be a good idea to tweak the value of
% \extrarowheight as needed to properly center the text within the cells
\caption{Simulation results for different levels of localization accuracy}
\label{tbl: simulation results 2}
\centering
% Some packages, such as MDW tools, offer better commands for making tables
% than the plain LaTeX2e tabular which is used here.
\begin{tabular}{|c|c|c|c|c|}
\hline
$k$ & $n_k$ & $\mu$ & $\sigma$ & $\max$ \\
\hline
$0.00$ & $65520$  & $0.05\text{m}$ & $ 0.04\text{m}$ & $0.20\text{m}$ \\
$0.25$ & $72636$  & $0.08\text{m}$ & $ 0.06\text{m}$ & $0.27\text{m}$ \\
$0.50$ & $82728$  & $0.11\text{m}$ & $ 0.09\text{m}$ & $0.40\text{m}$ \\
$0.75$ & $111321$ & $0.16\text{m}$ & $ 0.13\text{m}$ & $0.94\text{m}$ \\
\hline
\end{tabular}
\end{table}

% \jln{[Try to change the odometry noise on the Husky rather than introduce a Turlebot.]}
% The Husky robot suffers from high noise in \jln{wheel} odometry, especially during turn-in-place maneuvers.
% \jlnComment{What is the purpose of this first sentence?}
% Therefore, it 
The Husky robot combines data from an Inertial Measurement Unit (IMU), a GPS module and 
wheel odometry to achieve a relatively small localization error overall.
%bounded error localization. 
In order to evaluate the impact of localization accuracy on our algorithms, we simulate 
the effect of large wheel slippage % in outdoor environments,
by introducing a zero mean additive Gaussian white noise to each of the wheel encoder measurements, with a variance equal to $k(v_x+\omega_z)/2$, where $v_x$ is the linear velocity of the robot, $\omega_z$ is its yaw rate and $k$ is a proportionality constant, also called noise level in the following.
% proportional to the mean of the linear and angular velocity of the robot. 
Increasing $k$ results in a poorer alignment of the point clouds, but all portions 
of the structure, except the horizontal faces, are still captured in the reconstructed 
model, see Fig. \ref{fig:localization accuracy}. Note that our policies compute the next waypoint at 
discrete times and therefore assume that the drift in localization between waypoints 
is sufficiently small so that the robot reaches the next waypoint with the 
camera facing the structure.

We use the CloudCompare\cite{cloud_compare} software to compute the distortion in the reconstructed model $\mathcal{C}_k$, for a noise level $k$, with respect to a reference point cloud $\mathcal{C}_R$ which is 
generated using a different mobile platform with almost perfect localization. 
First, we register $\mathcal{C}_k$ to $\mathcal{C}_R$ using an Iterative Closest Point (ICP) algorithm \cite{rusinkiewicz2001efficient}. %\jln{[Ref?]}. 
We then define for every point in 
$\mathcal{C}_k$, its error to be the distance to the nearest neighbor in $\mathcal{C}_R$. Table \ref{tbl: simulation results 2} lists the simulation results for mapping the Small $\Gamma$ model with different noise
levels $k$, where $n_k$ is the number of points in $\mathcal{C}_k$ and $\mu, \sigma, \max$ are respectively the mean, standard deviation and maximum value of the errors of all points in $\mathcal{C}_k$. 
% \jlnComment{Is this fig. just plotting the numbers of table II ?? Then remove it, redundant.}
%As shown in Fig. \ref{fig:localization accuracy graph}, 
The table indicates that both the mean and standard deviation of the errors increase with the noise level.
% This model can be improved through post-processing using targeted computer 
% vision techniques \cite{Furukawa:PAMI10:stereopsis} to get a more accurate model. 
%
% Let $\mathcal{C}_k = \{p_i : i = 1,2,\ldots,n_k \}$ denote the assembled point cloud for a noise level $k$ and $\mathcal{C}_R = \{q_i : i = 1,2,\ldots,n_R \}$ denote the reference point cloud generated by uniformly sampling the simulated structure. We can then define a distance function $d(p,\mathcal{C}_R)$
%
% In particular, the IMU operating at $50\text{Hz}$ is essential for 
% estimating the orientation of the robot. Therefore, we exaggerate the error in 
% localization by simulating the IMU to operate at $5\text{Hz}$. 
% As shown in Fig. \ref{subfig:results Husky SmallGamma 4.5 NoisyOdom}, the high noise 
% in the localization of the robot results in poor alignment of the point clouds
% but all portions of the structure, except the horizontal faces, are captured in the 
% reconstructed model, see Fig. \ref{subfig:results Husky SmallGamma 4.5 NoisyOdom Model}.

% Note that while advancements in vSLAM techniques can improve the alignment of the point 
% clouds and consequently the quality of the model, our policies ensure the completeness
% \jlnComment{[I'd remove this last sentence. Disputable claim, and not clear what it adds.]}
% of the model, subject to physical limitations of the platform.

% As a result
% the point clouds captured around corners are not perfectly aligned in the reconstructed model. In order to
% compare, a Turtlebot fitted with a Kinect sensor at the same height as in the Husky robot is used
% to map the Small $\Gamma$ model. The low odometry noise in Turtlebot results in better alignment
% in the reconstructed model. As discussed in Section II, the quality of the point cloud alignment is dependent 
% on the vSLAM module used but using our policies, all parts of the structure, except the horizontal faces,
% are captured in the reconstructed model in either platforms. \jln{[Illustration? I suppose this paragraph
% is not finished]}

% The 4-wheel differential drive system in husky results in high noise in odometry
% especially during turn-in-place movements. An Inertial Navigation System(INS) along 
% The accuracy of the reconstructed model is dependent on the vSLAM module and in turn its dependence on the localization accuracy. The 
% We consider two robots - Turtlebot fitted with an additional Kinect facing to
% the right and a Husky robot with a Kinect mounted on a UR5 robotic arm. The 
% Husky additionally has a 2D LIDAR for obstacle detection. Due to the 4-wheel 
% differential drive system in Husky, the localization is poor especially 
% during turn-in-place movements. As illustrated in Fig. \ref{subfig:results Husky SmallJ} and Fig. \ref{subfig:results Turtlebot SmallJ}, the error in the
% model constructed using the Turtlebot is lesser. The error in the constructed
% model is a result of poor registration and could be removed by improvements 
% in the vSLAM module. But both the models capture all portions of the structure.
% As discussed in Section II, the objective of this work is to ensure that all
% sections of the structure are eventually captured which our algorithm achieves.
% Comparison between Indoor environment - Turtlebot
% and outdoot environment - Husky with UR5 arm.
% Husky robot has a large odometry error especially when turning on the spot.
% As a result, the SLAM module fails to build a well registered model
% around corners. Can this be rectified by post-processing using SfM techniques?
% Turtlebot is used as a reference baseline?

\begin{figure}[!t]
\subfloat[Reconstructed model with $k=0.75$]{
	\includegraphics[width=1.5in]{figures/SmallGamma_Husky_4_5_Noise_0_75_model}
	\label{subfig:results Husky SmallGamma 4.5 NoisyOdom Model}
}
\hfill
\subfloat[Model: Small $\Gamma$, Range: $4.5\text{m}$]{
	\includegraphics[width=1.5in]{figures/SmallGamma_Husky_4_5_Noise_0_75}
	\label{subfig:results Husky SmallGamma 4.5 NoisyOdom}
}
\caption{
% Reconstructed model using our policies for a Husky robot with low localization
% accuracy and a Turtlebot platform with high localization accuracy. 
a) Large errors in localization results in poor alignment, although all portions of the structure have been captured in the model, see Fig. \ref{subfig:small gamma husky 4.5 model} for comparison. b) The projection of the reconstructed model on the $\vect{x}_{\fr g}\vect{y}_{\fr g}$ plane, when compared to Fig. \ref{subfig:small gamma husky 4.5}, shows the distortion introduced due to the noisy wheel odometry.}
\label{fig:localization accuracy}
\end{figure}

% \begin{figure}[!t]
% 	\centering
% 	\includegraphics[width=3.0in]{figures/SmallGamma_Husky_4_5_Noise_graph}
% 	\caption{The mean and standard deviation of the errors increase with increase in noise levels.}
% 	\label{fig:localization accuracy graph}
% \end{figure}

\subsection{Comparing with Frontier-based Exploration}

The Frontier Exploration\cite{rosfrontier} package available in ROS
relies on a 2D LIDAR to build an occupancy grid that is used to compute
the frontiers. The package requires the user to define a 2D polygon 
that encloses the structure. The algorithm then explores until there
are no more frontiers inside the user-defined polygon. 
%We use the Kinect sensor is simultaneously used to build a 3D model using RTAB-Map \cite{labbe14online}.
In comparison to the FBE algorithm, our algorithms
\begin{itemize}
\item do not require a user defined bounding polygon;
\item maintain as much as possible a fixed distance from the structure (during the PE phase),
thereby ensuring that all portions up to a height of $H_{\max}$ are mapped;
\item consistently explore the structure in the clockwise direction, which can
be important from a user perspective to understand the behavior of the robot. 
On the other hand, the trajectory prescribed by the FBE algorithm depends on the 
size of the user-defined bounding polygon. A large bounding polygon will cause the 
robot to explore areas far away from the structure and will possibly not maintain 
a fixed direction of exploration.
\item produces the same path every time for a given structure whereas the path computed 
by the FBE algorithm could differ greatly between two trials. Also, the robot often
gets stuck while using the FBE algorithm as the computed waypoints are often too close to
the structure. 
\end{itemize}

\begin{table}[!t]
% increase table row spacing, adjust to taste
\renewcommand{\arraystretch}{1.3}
% if using array.sty, it might be a good idea to tweak the value of
% \extrarowheight as needed to properly center the text within the cells
\caption{Comparison between our policy and frontier based exploration}
\label{tbl: simulation results 3}
\centering
% Some packages, such as MDW tools, offer better commands for making tables
% than the plain LaTeX2e tabular which is used here.
\begin{tabular}{|c|c|c|c|}
\cline{3-4}
\multicolumn{2}{c|}{} & Proposed Policy & FBE \\
\hline
\multirow{3}{*}{Small $\Gamma$} & Path Length            & $72.08\text{m}$  & $49.78\text{m}$  \\\cline{2-4}
								& Unique closest point set size & $6,063$          & $5,398$          \\\cline{2-4}
								& Mean Error             & $0.05\text{m}$ & $0.12\text{m}$\\\cline{2-4}
% & Quality     & $Y\%$ & $Y\%$ \\
\hline
\multirow{3}{*}{House} & Path Length & $59.89\text{m}$ & $47.55\text{m}$ \\\cline{2-4}
					   & Unique closest point set size  & $9,182$ & $7,402$ \\\cline{2-4}
					   & Mean Error             & $0.05\text{m}$ & $0.12\text{m}$\\\cline{2-4}
% & Quality     & $Y\%$ & $Y\%$ \\
\hline
\end{tabular}
\end{table}

\begin{figure}[t]
\subfloat[Proposed Policy]{
	\includegraphics[width=1.50in]{figures/SmallGamma_Husky_4_5_model}
	\label{subfig:small gamma husky 4.5 model}
}
\hfil
\subfloat[Frontier based Exploration]{
	\includegraphics[width=1.50in]{figures/SmallGamma_Husky_4_5_FBE_model}
	\label{subfig:small gamma husky 4.5 FBE model}
}
\hfil
\subfloat[Proposed Policy]{
	\includegraphics[width=\linewidth]{figures/House_Model}
	\label{subfig:house husky 4.5 model}
}
\hfil
\subfloat[Frontier based Exploration]{
	\includegraphics[width=\linewidth]{figures/House_FBE_Model}
	\label{subfig:house husky 4.5 FBE model}
}
\hfil
\subfloat[Model: Small $\Gamma$, Range: $4.5m$]{
	\includegraphics[width=1.5in]{figures/SmallGamma_Husky_FBE_4_5}
	\label{subfig:small gamma husky 4.5 FBE}
}
\hfil
\subfloat[Model: House, Range: $4.5m$]{
	\includegraphics[width=1.5in]{figures/House_Husky_FBE_4_5}
	\label{subfig:house husky 4.5 FBE}
}
\hfil
\caption{(a-d) Comparison of the reconstructed model using our policies and FBE. 
(e,f) The trajectory prescribed by FBE for the Small $\Gamma$ and 
House structure is shown in blue.%\mani{add user defined bounding polygon} 
}
\label{fig:simulation FBE}
\end{figure}

In order to get a quantitative measure of the structure coverage, 
we use again the CloudCompare software to compute essentially the projection
of the reconstructed model $\mathcal{C}$ on the reference point cloud $\mathcal{C}_R$. Namely, for each point in $\mathcal{C}$ we compute the closest point in $\mathcal{C}_R$.
%
%the closest point set of the reconstructed model $\mathcal{C}$ in $\mathcal{C}_R$, %which is determined by taking the closest point in $\mathcal{C}_R$ 
%for every point in %$\mathcal{C}$. This set can be considered as the 
%projection of $\mathcal{C}$ onto $\mathcal{C}_R$. 
%
Note that multiple points in $\mathcal{C}$ can have the 
same closest point in $\mathcal{C}_R$. In this case, we remove these 
duplicate points to obtain the \emph{unique closest point set}.
Then, as long as the reconstructed model aligns relatively well with the 
reference model, the cardinality of the unique closest point set 
%is a good 
is taken as our estimate of the structure coverage. 
Table \ref{tbl: simulation results 3} compares the level of structure
coverage achieved by our policies and FBE with a camera range of $4.5\text{m}$ 
for two of the environments considered.
For the Small $\Gamma$ model, our reference point cloud has $6,116$ points 
with a minimum distance of $0.1\text{m}$ between points. For the House model, 
our reference point cloud has $10,889$ points with a minimum distance
of $0.1\text{m}$ between points. Since the height of the House model is more 
than $H_{\max}$, we only take the portion of the reconstructed model up to 
the height $H_{\max}$ for computing the structure coverage and mean error 
for the two algorithms. 

Table \ref{tbl: simulation results 3} shows that 
our policies achieve a higher level of structure coverage than FBE 
for the environments considered and our proposed coverage metric.
% \jln{[Explain what model size mean in that table! and why it corresponds to higher coverage.]}
%
Note also that the smooth trajectory prescribed by our policies is beneficial to the vSLAM module to achieve a better 
alignment and a lower value for the mean error in the 
reconstructed model, especially if the robot localization accuracy 
is poor.
%
A visual inspection of Fig. \ref{subfig:small gamma husky 4.5 model} 
and Fig. \ref{subfig:small gamma husky 4.5 FBE model} shows the improvement 
in performance of the vSLAM module when using our algorithms compared to FBE.
%while using platforms such as Husky or in outdoor environments. 

% An example of a floating figure using the graphicx package.
% Note that \label must occur AFTER (or within) \caption.
% For figures, \caption should occur after the \includegraphics.
% Note that IEEEtran v1.7 and later has special internal code that
% is designed to preserve the operation of \label within \caption
% even when the captionsoff option is in effect. However, because
% of issues like this, it may be the safest practice to put all your
% \label just after \caption rather than within \caption{}.
%
% Reminder: the "draftcls" or "draftclsnofoot", not "draft", class
% option should be used if it is desired that the figures are to be
% displayed while in draft mode.
%
%\begin{figure}[!t]
%\centering
%\includegraphics[width=2.5in]{myfigure}
% where an .eps filename suffix will be assumed under latex, 
% and a .pdf suffix will be assumed for pdflatex; or what has been declared
% via \DeclareGraphicsExtensions.
%\caption{Simulation Results}
%\label{fig_sim}
%\end{figure}

% Note that IEEE typically puts floats only at the top, even when this
% results in a large percentage of a column being occupied by floats.


% An example of a double column floating figure using two subfigures.
% (The subfig.sty package must be loaded for this to work.)
% The subfigure \label commands are set within each subfloat command, the
% \label for the overall figure must come after \caption.
% \hfil must be used as a separator to get equal spacing.
% The subfigure.sty package works much the same way, except \subfigure is
% used instead of \subfloat.
%
%\begin{figure*}[!t]
%\centerline{\subfloat[Case I]\includegraphics[width=2.5in]{subfigcase1}%
%\label{fig_first_case}}
%\hfil
%\subfloat[Case II]{\includegraphics[width=2.5in]{subfigcase2}%
%\label{fig_second_case}}}
%\caption{Simulation results}
%\label{fig_sim}
%\end{figure*}
%
% Note that often IEEE papers with subfigures do not employ subfigure
% captions (using the optional argument to \subfloat), but instead will
% reference/describe all of them (a), (b), etc., within the main caption.


% An example of a floating table. Note that, for IEEE style tables, the 
% \caption command should come BEFORE the table. Table text will default to
% \footnotesize as IEEE normally uses this smaller font for tables.
% The \label must come after \caption as always.
%
%\begin{table}[!t]
%% increase table row spacing, adjust to taste
%\renewcommand{\arraystretch}{1.3}
% if using array.sty, it might be a good idea to tweak the value of
% \extrarowheight as needed to properly center the text within the cells
%\caption{An Example of a Table}
%\label{table_example}
%\centering
%% Some packages, such as MDW tools, offer better commands for making tables
%% than the plain LaTeX2e tabular which is used here.
%\begin{tabular}{|c||c|}
%\hline
%One & Two\\
%\hline
%Three & Four\\
%\hline
%\end{tabular}
%\end{table}


% Note that IEEE does not put floats in the very first column - or typically
% anywhere on the first page for that matter. Also, in-text middle ("here")
% positioning is not used. Most IEEE journals use top floats exclusively.
% Note that, LaTeX2e, unlike IEEE journals, places footnotes above bottom
% floats. This can be corrected via the \fnbelowfloat command of the
% stfloats package.


statement.tex
% !TEX root =  ../main.tex

\section{Problem Statement and Assumptions} %Formulation
\label{section: statement}
%\jln{[What do we need from the sensing pipeline? A sequence of point clouds
%with associated camera positions, etc.]}\mani{[I think this is clarified now.]}

Consider the problem of constructing a 3D model of a given structure 
of finite size, such as a monument or a building for example. Initially, no
approximate model of the structure nor map of the environment is available, 
and the actual size of the structure is also unknown. 
We address the following question: ``How should a mobile robot carrying 
a depth sensing camera, such as a Kinect, move in order to reconstruct 
a complete 3D model of this structure?''. The sensor collecting depth and 
luminance images of the scene allows the robot to build the 3D model 
on the fly using available SLAM algorithms, such as RTAB-Map \cite{labbe14online} 
or RGBD-SLAM \cite{Endres:TRO2014:RGBDSLAM}. These algorithms assemble
the sequence of points clouds captured by the sensor (also called 
camera in the following), producing a registered global point cloud or a 3D occupancy grid stored in an 
OctoMap% \jlnComment{changed from octomap: you use capital letters later, choose}
\cite{hornung13auro}. 
Note that the type of sensor (monocular camera%\cite{engel2014lsd}
, stereo camera rig with IMU, etc\ldots) used depends on the SLAM algorithm. Any 
of these algorithms can be used with our policies as long as the SLAM module can 
additionally return the sequence of point clouds and corrected camera positions 
following registration.
%\jln{[A bit confusing because the purpose of SLAM is to merge the views already. Can ``represent the model as a'' be replaced by ``returns a''?]}.
The remaining problem that we consider here is to determine the trajectory of the camera such 
that the entire visible portion of the structure is eventually captured in the model.
The key challenge is to develop strategies that are applicable for any 
type of structure while respecting the physical limitations of the platform.
%(from hereafter simply referred as camera). 
%
%%\jln{In other word, we assume for simplicity that the 
%%sensor collects depth and luminance images of the scene, although this work 
%%can be extended to standard cameras if an appropriate visual SLAM 
%algorithm is available.}

%With no prior map of the environment, we need a Visual SLAM system to build 
%the 3D model on the fly. 
% In this paper, we use RTAB-Map for assembling the sequence of point clouds 
% produced by the depth sensing camera (from hereafter simply referred as camera). 
% Hence, the problem simplifies to controlling the trajectory of the camera 
% such that the entire visible portion of the structure is covered. The key 
% challenge however is to develop strategies that are applicable for any 
% type of structure while respecting the limitations of the platform.
%
%\jlnComment{Perhaps we should say a word here on extending the work to standard cameras.}
%\mani{[SLAM using Monocular cameras is not suitable for our purposes. The density of points is very poor. Its not surprising as CV techniques for dense visual slam using monocular cameras is not yet solved. We can work with Stereo cameras though.]}

\begin{figure}[!t]
\centering
\subfloat[]{
	\includegraphics[width=1.62in]{figures/Initial_Conditions}
	\label{subfig:initial conditions}
}
\hfil
\subfloat[]{
	\includegraphics[width=1.62in]{figures/Initial_Image}
	\label{subfig:initial image}
}
\caption{The starting configuration of the robot needs to satisfy Initial Conditions
\ref{initial: robot position} and \ref{initial: FOV}.
a) At the beginning, the structure and the robot can be separated by a plane. 
b) The initial image as seen by the camera. 
Initially, the robot only knows that the structure in the FOV of its camera 
is the one that should be mapped.
%This is the only information available to the system when it starts.
}
\label{fig:initial conditions}
\end{figure}


In order to specify to our system which structure is to be mapped, 
we require that the initial configuration of the robot with respect
to the structure satisfy the following two basic conditions, illustrated
in Fig. \ref{fig:initial conditions}.
%Since the system does not have any means to determine which structure is to be mapped,
%we impose certain initial conditions. A valid starting configuration satisfying these initial
%conditions is shown in Fig. \ref{fig:initial conditions}.

\begin{init}	\label{initial: robot position}
The robot is placed fully outside the structure, so that there exists 
a 2D plane separating the robot and the structure.
\end{init}

\begin{init}	\label{initial: FOV}
%The robot has the camera facing to its right 
%and 
The structure to be mapped is present in the field-of-view (FOV) of the camera. 
% \jlnComment{[I removed ``right'' in IC2, which needs FoR to be defined]} 
\end{init}
% Following discussion not quite closed:
% --------------------------------------
%\jln{[Couldn't there be other structures in the field of view if the operator
%selects the one of interest manually on the initial image?]}
%\mani{This would actually be related to Assumption \ref{assumption: annulus}. The robot would have to be approximately midway of the width of the annulus.}
%\mani{Option 1) Only the structure is present within the range of the depth sensor. The RGB image could contain distant structures. Option 2) If the range of the sensor is large, the operator must input the width of the annulus of free space and select the structure of interest.}
%\jln{[About this, perhaps we should illustrate in the simulations that it works even if there are surrounding objects, as long as they are far enough.]} 

Next, note that depending on the set of all configurations that are reachable 
by the mobile sensor mounted on a specific physical platform, some parts of the 
structure might not be visible at all, and hence cannot be mapped by any 
algorithm implemented on this platform. For concreteness, we make 
the following assumption to describe our scenario and algorithms, but 
other situations could be handled with the generic tools developed in this paper.

% \jlnComment{It seems in your changes you had removed the def. of FoR acronym?}
\begin{assum}\label{assumption: fixed camera}
The camera is mounted on a mobile ground robot 
% The position of the camera in the robot's Frame of Reference (FoR) is kept constant.
such that the camera center $\fr C$ lies directly above the center $\fr R$ of 
the robot's base Frame of Reference (FoR), at a constant height.
%\maniComment{because while checking interference with structure, we do so w.r.t $\fr R$}
%\jln{[What about $x,y$?]}.
Moreover, the relative pitch and roll of the camera with respect to the 
robot FoR are kept fixed, while the relative yaw is unconstrained.
%\jln{[Here perhaps we need to clarify the reachable space of the robot. And perhaps a figure illustrating the 2 frames of references (FoR) and rotation axis. Another possibility, if we use it in the following, is to introduce in this section a FoR for the robot and the camera formally.]}
\end{assum}

\begin{figure}[!ht]
	\includegraphics[width=\linewidth]{figures/FoR}
	\caption{The camera is kept at a constant height above the robot's base. The red, green and blue lines correspond to the $x$, $y$ and $z$ axes respectively and both the camera and robot can rotate along their $z$ axes. The yellow region corresponds to the view frustum of the camera. 
% \jln{[Ambiguities on the figure: why show a rotation of Robot FoR? Also could show $\vect{x}_{\fr c} \neq -\vect{y}_{\fr r}$ to remind the reader that the camera can rotate around $\vect{z}_{\fr c}$.]}
}
\label{fig:assumptions}
\end{figure}

Based on Assumption \ref{assumption: fixed camera}, Fig. \ref{fig:assumptions} shows our
conventions for the different FoR used.
%the different FoR that are used in our policies. %The global FoR is an inertial frame and the 
%assembled point cloud is in this FoR. 
The global point cloud is assembled in a global FoR $\fr G \vect{x}_{\fr g} \vect{y}_{\fr g} \vect{z}_{\fr g}$.
% \jlnComment{was inertial important?}
Note that we use bold font to represent vectors.
The robot FoR $\fr R \vect{x}_{\fr r} \vect{y}_{\fr r} \vect{z}_{\fr r}$ (forward, left, up) 
is attached to a point $\fr R$ on the base of the mobile robot and moves along with it. 
%As shown by the gray link, 
The camera FoR $\fr C \vect{x}_{\fr c} \vect{y}_{\fr c} \vect{z}_{\fr c}$, 
with $\vect{z}_{\fr c}=\vect{z}_{\fr r}$, is rigidly attached to the robot except 
for the yaw motion, which is left unconstrained. 
The imaging plane of the camera is defined by $\vect{y}_{\fr c} \vect{z}_{\fr c}$, with
$\vect{x}_{\fr c}$ pointing towards the front of the camera on the optical axis.
Coordinates in the camera, robot and global FoR are denoted using superscripts as 
$\vect{v}^{\fr c}$, $\vect{v}^{\fr r}$ and $\vect{v}^{\fr g}$ respectively for a vector $\vect{v}$.
% \mani{Add superscripts and unit vector definitions}

We make two additional assumptions for simplicity of exposition.
First, let $\rho$ be the maximum distance that we wish to allow between the structure
and the camera when capturing point clouds. This distance could be the range of the
camera or a shorter distance for which the resolution is higher.
The next assumption guarantees that there exists collision free 
paths around the structure.
% such that the minimum di perpendicular\jlnComment{horizontal?} distance between
%this path and the structure is the constant $D$. 
%Therefore, it is sufficient to provide a method to compute this path online.
%\mani{Also, if the clearance around the structure is not sufficient, then the value of parameter $D$ can be reduced but this would reduce the maximum height of the structure that can be mapped.}
%\maniComment{This removes the need to detect failure and quit.}
\begin{assum}	\label{assumption: annulus}
The distance of the closest obstacle from the structure is at least $2 \rho$.
%$2D$, where $D = 
%. Here $H_{\max}$ is the maximum
%height of the structure to be mapped, $\phi$ is the vertical angle of
%view of the camera and ,
%where $\rho$ is the range of the camera.}
%\mani{There exists a collision free path around the structure to be mapped
%such that the minimum distance of any point on this path from the structure
%is at least $D$, where the value of $D$ is manually estimated by the clearance present around the structure.
%}
%Moreover, the clearance around the structure is at least $2D$ meters, where 
%$D$ is the maximum distance the robot can be from the structure. Naturally, 
%this value is bounded by 
%An annulus of free space is present around the structure to be mapped. 
%The width of this annulus is determined by 
%the range of the camera used for mapping.}
%\jln{[Specify more precisely what you mean, or move out of formal assumption.]}
\end{assum}

%\jln{Note that regarding Initial Condition \ref{initial: FOV}, if several disconnected
%structures are initially present in the FOV, the operator should select the one
%of interest.}

The next assumption simplifies the problem of detecting, tracking and removing the 
ground surface from point clouds.
\begin{assum}\label{assumption: level ground}
The structure and the robot are placed on a horizontal surface 
(so $\vect{z}_{\fr c}=\vect{z}_{\fr r} = \vect{z}_{\fr g}$).
\end{assum}

A consequence of these assumptions is that relatively horizontal surfaces that
are at the same height or above the camera center cannot be mapped, and the maximum
height of the structure that can be mapped is 
$H_{\max} = z^{\fr g}_{\fr C}+\rho \tan{\psi/2}$,
%given by $H_{\max} = 2R \tan{\phi/2}$, where
where $\psi$ is the vertical angle of view of the camera 
and $z^{\fr g}_{\fr C}$ the height of the camera.
%\maniComment{height? or z-coordinate?}
%\jln{[only if the height of the camera is half of this? otherwise $h+R \tan{\phi/2}$ for $h \leq R \tan{\phi/2}$?]}.
%the range of the camera, \mani{and the value of $D$},
%\jln{and how far the robot can be from the structure}, 
%see Fig. \ref{fig:assumptions}. 
%
%Assumption \ref{assumption: level ground} simplifies the problem of detecting, 
%tracking and removing the ground surface from the point cloud, but could
Assumption \ref{assumption: level ground} could be removed by using recent classification systems 
that can differentiate between ground and non-ground regions \cite{zhou2012self} to pre-process 
the point clouds before sending them to our system.
%. Recent advances
%in autonomous navigation have developed classification systems that
%can differentiate between ground and non-ground regions \cite{zhou2012self}. These
%techniques can be used to pre-process the point clouds before sending to our
%system.
%\jln{It could be removed if the system had some other means to differentiate between the points belonging to the structure and the points belonging to the ground surface} \jln{[There should be some ref. on ground detection that we could cite].}
%This is required since the system has no means to differentiate between the points
%generated from the structure and the points corresponding to the ground 
%surface.
%, a procedure that we rely on to determine the boundary of the structure. 
%\jln{Last sentence not really clear.]}

Finally, there are additional implicit assumptions that we state informally.
First, since we rely on an external mapping module to build the 3D model, 
the conditions that allow this module to operate sufficiently reliably must 
be met. For example, vSLAM generally requires appropriate scene 
illumination and the presence of a sufficiently rich set of visual features.
Second, we concentrate on the reconstruction of the details of the model 
at a scale comparable with or larger than the typical length of the robot.
If features at a smaller scale need to be included, e.g., fine structural
details on a wall, our system could be augmented with a more local planner
for a robotic arm carrying the sensor \cite{kriegel2013efficient, Dornhege13advro},
as well as targeted computer vision techniques \cite{Furukawa:PAMI10:stereopsis}.
%\mani{[this is also post-processing. I have added this in the SfM section in intro.}
Finally, for reasons explained in Section \ref{section: obstacle ahead}, we assume 
that the robot is equipped with sensors capable of detecting obstacles 
in a $180^{\circ}$ region ahead of it and within a distance of $\rho$.
%$D$.}
% \jln{[Note: previous sentence is temporary: $D$ is not yet defined...]}

%\jln{[Not just a matter of scale? Local planer for a  filling out a volume 
%of known size. Holes due to occlusions. Vs. cavities due to finite range
%of the camera?]}\mani{[On further thought, this may not be straightforward. But is the 
%logical next step for us.]}

%\begin{assum}\label{assumption: slam assumptions}
%Since we use an external Visual SLAM system, its limitations are also preserved.
%The RTAB-Map package used in this paper relies on visual features and therefore different %portions of the structure must have visibly different appearance.
%\end{assum}

%\begin{assum}\label{assumption: occlusions}
%A complex 3D structure causes many holes due to occlusions which can only be removed by
%viewing from specific angles. Given an incomplete model, techniques used for 3D
%reconstruction of objects can be applied to local portions of the model to fill these
%types of holes. Therefore, we assume simple wall-like structures.
%\end{assum}

main.bbl
% Generated by IEEEtran.bst, version: 1.13 (2008/09/30)
\begin{thebibliography}{10}
\providecommand{\url}[1]{#1}
\csname url@samestyle\endcsname
\providecommand{\newblock}{\relax}
\providecommand{\bibinfo}[2]{#2}
\providecommand{\BIBentrySTDinterwordspacing}{\spaceskip=0pt\relax}
\providecommand{\BIBentryALTinterwordstretchfactor}{4}
\providecommand{\BIBentryALTinterwordspacing}{\spaceskip=\fontdimen2\font plus
\BIBentryALTinterwordstretchfactor\fontdimen3\font minus
  \fontdimen4\font\relax}
\providecommand{\BIBforeignlanguage}[2]{{%
\expandafter\ifx\csname l@#1\endcsname\relax
\typeout{** WARNING: IEEEtran.bst: No hyphenation pattern has been}%
\typeout{** loaded for the language `#1'. Using the pattern for}%
\typeout{** the default language instead.}%
\else
\language=\csname l@#1\endcsname
\fi
#2}}
\providecommand{\BIBdecl}{\relax}
\BIBdecl

\bibitem{Jacobi201580}
M.~Jacobi, ``Autonomous inspection of underwater structures,'' \emph{Robotics
  and Autonomous Systems}, vol.~67, no.~C, pp. 80--86, May 2015, special issue
  on Advances in Autonomous Underwater Robotics.

\bibitem{el2004detailed}
S.~F. El-Hakim, J.~A. Beraldin, M.~Picard, and G.~Godin, ``Detailed {3D}
  reconstruction of large-scale heritage sites with integrated techniques,''
  \emph{IEEE Computer Graphics and Applications}, vol.~24, no.~3, pp. 21--29,
  May-June 2004.

\bibitem{labbe14online}
M.~Labbe and F.~Michaud, ``{Online Global Loop Closure Detection for
  Large-Scale Multi-Session Graph-Based SLAM},'' in \emph{Proceedings of the
  IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
  Chicago, IL, September 2014.

\bibitem{Endres:TRO2014:RGBDSLAM}
F.~Endres, J.~Hess, J.~Sturm, D.~Cremers, and W.~Burgard, ``{3-D} mapping with
  an {RGB-D} camera,'' \emph{IEEE Transactions on Robotics}, vol.~30, no.~1,
  pp. 177--187, February 2014.

\bibitem{Bajcsy:IEEE88:activePerception}
R.~Bajcsy, ``Active perception,'' \emph{Proceedings of the IEEE}, vol.~76,
  no.~8, pp. 966--1005, August 1988.

\bibitem{Yamauchi:1997:FAA:523996.793157}
B.~Yamauchi, ``A frontier-based approach for autonomous exploration,'' in
  \emph{Proceedings of the 1997 IEEE International Symposium on Computational
  Intelligence in Robotics and Automation}, ser. CIRA '97, 1997, pp. 146--151.

\bibitem{kriegel2013efficient}
S.~Kriegel, C.~Rink, T.~Bodenm{\"u}ller, and M.~Suppa, ``Efficient
  next-best-scan planning for autonomous {3D} surface reconstruction of unknown
  objects,'' \emph{Journal of Real-Time Image Processing}, pp. 611--631,
  December 2015.

\bibitem{krainin2011autonomous}
M.~Krainin, B.~Curless, and D.~Fox, ``Autonomous generation of complete {3D}
  object models using next best view manipulation planning,'' in \emph{IEEE
  International Conference on Robotics and Automation (ICRA)}, Shanghai, China,
  May 2011, pp. 5031--5037.

\bibitem{Lin:civEng2015:UAVconstructionMonitoring}
J.~Lin, K.~Han, and M.~Golparvar-Fard, ``A framework for model-driven
  acquisition and analytics of visual data using {UAV}s for automated
  construction progress monitoring,'' in \emph{Computing in Civil Engineering},
  Austin, Texas, June 2015, pp. 156--164.

\bibitem{bircher2015structural}
A.~Bircher, K.~Alexis, M.~Burri, P.~Oettershagen, S.~Omari, T.~Mantel, and
  R.~Siegwart, ``Structural inspection path planning via iterative viewpoint
  resampling with application to aerial robotics,'' in \emph{IEEE International
  Conference on Robotics and Automation (ICRA)}, Seattle, Washington, May 2015,
  pp. 6423--6430.

\bibitem{Acar:TRO06:coverage}
E.~Acar, H.~Choset, and J.~Lee, ``Sensor-based coverage with extended range
  detectors,'' \emph{IEEE Transactions on Robotics}, vol.~22, no.~1, pp.
  189--198, Feb 2006.

\bibitem{Lim14CrackInspection}
R.~Lim, H.~M. La, and W.~Sheng, ``A robotic crack inspection and mapping system
  for bridge deck maintenance,'' \emph{IEEE Transactions on Automation Science
  and Engineering}, vol.~11, no.~2, pp. 367--378, April 2014.

\bibitem{englot2012sampling}
B.~Englot and F.~S. Hover, ``Sampling-based coverage path planning for
  inspection of complex structures,'' in \emph{International Conference on
  Automated Planning and Scheduling (ICAPS)}, Sau Paulo, Brazil, June 2012, pp.
  29--37.

\bibitem{Sheng08CrawlerInspection}
W.~Sheng, H.~Chen, and N.~Xi, ``Navigating a miniature crawler robot for
  engineered structure inspection,'' \emph{IEEE Transactions on Automation
  Science and Engineering}, vol.~5, no.~2, pp. 368--373, April 2008.

\bibitem{gazebo}
Gazebo robot simulator. \url{http://gazebosim.org/}. Accessed: 2015-12-29.

\bibitem{Frahm:2010:BRC:1888089.1888117}
J.-M. Frahm, P.~Fite-Georgel, D.~Gallup, T.~Johnson, R.~Raguram, C.~Wu, Y.-H.
  Jen, E.~Dunn, B.~Clipp, S.~Lazebnik, and M.~Pollefeys, ``Building {R}ome on a
  cloudless day,'' in \emph{Proceedings of the 11th European Conference on
  Computer Vision: Part IV}, ser. ECCV'10, 2010, pp. 368--381.

\bibitem{wu2011visualsfm}
C.~Wu. (2011) {VisualSFM}: A visual structure from motion system.
  \url{http://ccwu.me/vsfm/}. Accessed: 2015-12-27.

\bibitem{Furukawa:PAMI10:stereopsis}
Y.~Furukawa and J.~Ponce, ``Accurate, dense, and robust multiview stereopsis,''
  \emph{IEEE Transactions on Pattern Analysis and Machine Intelligence},
  vol.~32, no.~8, pp. 1362--1376, Aug 2010.

\bibitem{kuemmerle11icra}
R.~Kuemmerle, G.~Grisetti, H.~Strasdat, K.~Konolige, and W.~Burgard, ``g2o: A
  general framework for graph optimization,'' in \emph{IEEE International
  Conference on Robotics and Automation (ICRA)}, Shanghai, China, May 2011, pp.
  3607--3613.

\bibitem{shade2011choosing}
R.~Shade and P.~Newman, ``Choosing where to go: Complete {3D} exploration with
  stereo,'' in \emph{IEEE International Conference on Robotics and Automation
  (ICRA)}, Shanghai, China, May 2011, pp. 2806--2811.

\bibitem{shen2012autonomous}
S.~Shen, N.~Michael, and V.~Kumar, ``Autonomous indoor {3D} exploration with a
  micro-aerial vehicle,'' in \emph{IEEE International Conference on Robotics
  and Automation (ICRA)}, Seattle, Washington, May 2012, pp. 9--15.

\bibitem{heng2015efficient}
L.~Heng, A.~Gotovos, A.~Krause, and M.~Pollefeys, ``Efficient visual
  exploration and coverage with a micro aerial vehicle in unknown
  environments,'' in \emph{IEEE International Conference on Robotics and
  Automation (ICRA)}, Seattle, Washington, May 2015, pp. 1071--1078.

\bibitem{Atanasov:ICRA2015:activeSLAM}
N.~Atanasov, J.~{Le Ny}, K.~Daniilidis, and G.~J. Pappas, ``Decentralized
  active information acquisition: Theory and application to multi-robot
  {SLAM},'' in \emph{IEEE International Conference on Robotics and Automation
  (ICRA)}, Seattle, Washington, May 2015, pp. 4775--4782.

\bibitem{hornung13auro}
A.~Hornung, K.~M. Wurm, M.~Bennewitz, C.~Stachniss, and W.~Burgard,
  ``{OctoMap}: An efficient probabilistic {3D} mapping framework based on
  octrees,'' \emph{Autonomous Robots}, vol.~34, no.~3, pp. 189--206, April
  2013.

\bibitem{zhou2012self}
S.~Zhou, J.~Xi, M.~W. McDaniel, T.~Nishihata, P.~Salesses, and K.~Iagnemma,
  ``Self-supervised learning to visually detect terrain surfaces for autonomous
  robots operating in forested terrain,'' \emph{Journal of Field Robotics},
  vol.~29, no.~2, pp. 277--297, 2012.

\bibitem{Dornhege13advro}
C.~Dornhege and A.~Kleiner, ``A frontier-void-based approach for autonomous
  exploration in {3D},'' \emph{Advanced Robotics}, vol.~27, pp. 459--468, 2013.

\bibitem{rusu20113d}
R.~B. Rusu and S.~Cousins, ``{3D} is here: Point cloud library ({PCL}),'' in
  \emph{IEEE International Conference on Robotics and Automation (ICRA)},
  Shanghai, China, May 2011, pp. 1--4.

\bibitem{mitra2004estimating}
N.~J. Mitra, A.~Nguyen, and L.~Guibas, ``Estimating surface normals in noisy
  point cloud data,'' \emph{International Journal of Computational Geometry \&
  Applications}, vol.~14, no. 04n05, pp. 261--276, 2004.

\bibitem{khatib1986real}
O.~Khatib, ``Real-time obstacle avoidance for manipulators and mobile robots,''
  \emph{The International Journal of Robotics Research}, vol.~5, no.~1, pp.
  90--98, 1986.

\bibitem{Choset_2005_5167}
H.~Choset, K.~M. Lynch, S.~Hutchinson, G.~A. Kantor, W.~Burgard, L.~E. Kavraki,
  and S.~Thrun, \emph{Principles of Robot Motion: Theory, Algorithms, and
  Implementations}.\hskip 1em plus 0.5em minus 0.4em\relax Cambridge, MA: MIT
  Press, June 2005.

\bibitem{Cortes:IEEE08:nonsmoothDS}
J.~Cortes, ``Discontinuous dynamical systems,'' \emph{IEEE Control Systems
  Magazine}, vol.~28, no.~3, pp. 36--73, June 2008.

\bibitem{rosnav}
{ROS} navigation stack. \url{http://wiki.ros.org/navigation}. Accessed:
  2015-11-26.

\bibitem{gazebo_plugins}
Gazebo plugins. \url{http://wiki.ros.org/gazebo\_plugins}. Accessed:
  2015-12-22.

\bibitem{cloud_compare}
Cloud{Compare} (version 2.6.0) [{GPL} software]. (2016). Retrieved from
  http://www.cloudcompare.org/.

\bibitem{rusinkiewicz2001efficient}
S.~Rusinkiewicz and M.~Levoy, ``Efficient variants of the {ICP} algorithm,'' in
  \emph{Proceedings of the Third International Conference on 3-D Digital
  Imaging and Modeling, 2001}, Quebec City, Canada, May 2001, pp. 145--152.

\bibitem{rosfrontier}
{ROS} frontier exploration. \url{http://wiki.ros.org/frontier\_exploration}.
  Accessed: 2015-11-26.

\end{thebibliography}
