% Template article for preprint document class `elsart'
% with harvard style bibliographic references
% SP 2006/04/26

\documentclass[12pt]{article}

\usepackage{graphicx}
% or use the epsfig package if you prefer to use the old commands
% \usepackage{epsfig}
\usepackage{subfigure}

% The amssymb package provides various useful mathematical symbols
\usepackage{amssymb}
\usepackage{apalike}

% The lineno packages adds line numbers. Start line numbering with
% \begin{linenumbers}, end it with \end{linenumbers}. Or switch it on
% for the whole article with \linenumbers.
\usepackage{lineno}



%\linenumbers
\begin{document}

\title{Fusion of Incomplete Sensor Data for \\ Mobile Robot Environment Modeling}
%TITLE


\author{Luz~A.~Torres-M\'endez\footnote{This work was done when the first author was a PhD
student at McGill University. She is currently working as a full-time researcher at CINVESTAV-Unidad Saltillo located in Ramos Arizpe, Coahuila, Mexico.} , Gregory Dudek \\  Centre for Intelligent Machines, McGill University, \\ Montreal, QC, H3A 2A7, CA \\ abril.torres@cinvestav.edu.mx,dudek@cim.mcgill.ca}
\date{}
\maketitle

\begin{abstract}
This article presents a complete framework for building a 3D model of a large-scale
indoor environment by fusing incomplete sensor data. The incomplete
sensor data is obtained by a mobile robot navigating the environment.
The geometry of large-scale environments is usually extracted by acquiring
a huge amount of range data and registering it.  For many real systems, this data 
acquisition process is  time and energy consuming and also physically demanding.
The framework presented here overcomes this problem by allowing a robot to rapidly 
collect a set of intensity images and a very small amount of range information. 
The data registration of the intensity and range data is achieved by using a image-based 
registration algorithm and then the
missing geometric structures are inferred from the intensity images using a 
statistical learning method. This method basically integrates and analyzes the statistical 
relationships between the visual data and the available depth on terms of small patches. 
Finally, the integration of multiple panoramas with depth at different robot poses is
achieved by using a hybrid feature matching approach from the literature. Experiments on 
real-world data are conducted under different configurations to demonstrate the 
feasibility of the method.
\end{abstract}

%\keyword{environment modeling \sep sensor fusion \sep markov random fields}
%\begin{keyword}
% keywords here, in the form: keyword \sep keyword
%environment modeling \sep sensor fusion \sep markov random fields
% PACS codes here, in the form: \PACS code \sep code
%\end{keyword}



% main text
\section{Introduction}
\label{intro}

For an autonomous mobile robot it is a natural task to create a 3D model from local sensor
data collected as it moves in an unknown environment. This 3D model must contain 
geometric and photometric details as well as knowledge about empty spaces. The final 
representation of this knowledge will reflect the ability of the autonomous system to interact 
with its environment and accomplish its tasks.

Having a mobile robot able to build a 3D map of the environment is particularly appealing as 
it can be used not only for planning robot actions but also for several important applications.
For example, virtual exploration of remote locations, either for safety, automatic rescue and 
inspection of hazardous or inhospitable environments (e.g. the reactor of a nuclear power plant) or 
for efficiency reasons (museums' tours). All these applications depend on the transmission of 
meaningful visual and geometric information.

Acquiring data in order to build 3D models of environments is also an important task on its own. 
To this end, suitable sensors to densely cover the environment are
required. Since all sensors are imperfect, sensor inputs must be
used in a way that enables the robot to interact with its
environment successfully in spite of measurement uncertainty. One
way to cope with the accumulation of uncertainty is through {\em
sensor fusion}, as different types of sensors can have their data
correlated appropriately, strengthening the confidence of the
resulting percepts well beyond that of any individual sensor's
readings. Nowadays, it is common to combine information from many
sensors, even using the same sensors repeatedly, over time, with
the goal of building a model of the environment.

A typical 3D model acquisition pipeline is composed by a
specialized 3D scanner to acquire precise geometry, and a digital
camera to capture appearance information.  Visual sensors are the
preferred alternative for capturing the photometric
characteristics of the scene and they are fast. Conversely, range
sensors capture geometric data directly, but acquiring dense range
maps is a time and energy consuming process, unless costly and/or
sophisticated hardware is used. Thus, when building 3D models or
map representations of large scenes, a desired characteristic is
to simplify the way range sensor data is acquired so that time and
energy consumption can be minimized.  This can be achieved by
acquiring only partial, but reliable, depth information.

Techniques for surface depth recovery are essential in multiple
applications involving robotics and computer vision. In
particular, we investigate the autonomous integration of
incomplete sensory data to build a 3D model of an unknown
large-scale~\footnote{Large-scale space is defined as a physical
space that cannot be entirely perceived from a single vantage
point~\cite{Kuipers78}.}  indoor environment. Thus, the challenge
becomes one of trying to extract, from the sparse sensory data, an
overall concept of shape and size of the structures within the
environment.

We explore and analyze the statistical relationships between
intensity and range data in terms of small image patches. Our goal
is to demonstrate that the surround (context) statistics on both
the intensity and range image patches can provide information to
infer the complete 3D layout of space. It has been shown by Lee
{\em et al.}~\cite{Lee01} that although there are clear
differences between optical and range images, they do have similar
second-order statistics and scaling properties (i.e., they both
have similar structure when viewed as random variables). Our
motivation is to exploit this fact and also that both video
imaging and {\em limited} range sensing are ubiquitous
readily-available technologies while complete volume scanning is
prohibitive on most mobile platforms.

Real scenes are constrained by many regularities in the
environment, such as the natural geometry of objects and their
arrangements in space, natural distributions of light, and
regularities in the observer's position. Statistical studies may
be helpful for understanding these regularities, which are not
obvious from physical models. Statistical methods have enjoyed a
great deal of recent success in their applications to problems in
computer vision. However, few studies have been made into the
statistical relationship between images and range images (depth
images). Those few studies have uncovered meaningful and
exploitable statistical trends in real scenes which may be useful
for designing new algorithms in surface inference, and also for
understanding how humans perceive depth in the real scenes.

Our approach for range estimation is based on the assumption that
the pixels constituting both the range and intensity images
acquired in an environment can be regarded as the results of
pseudo-random processes, but that these random processes exhibit
useful structure. In particular, the assumption that range and
intensity images are correlated, albeit in potentially complicated
ways, is exploited.  A second assumption made is that the
variations of pixels in the range and intensity images are related
to the values elsewhere in the image(s) and that these variations
can be efficiently captured by the neighborhood system of a Markov
Random Field. Both these assumptions have been considered
before~\cite{Geman84,Efros99,Wei00,Efros01,Hertzmann01}, but they
have never been exploited in tandem.


In summary, this research answers three questions. First, how can
the statistical nature of visual (photometric) surround or context
provide information about its geometric properties? In particular,
how can the statistical relationships between intensity and range
data be modeled reliably such that the inference of unknown range
be as accurate as possible? Second, how can the data collected at
different robot poses in a large-scale scene be fused and modeled
such that the models we build are useful for robotic navigation
and localization? Third, based on the previous question, how can
such models be constructed automatically, particularly when very
limited range information is available with respect to intensity
information and no prior information concerning the position of
the robot is known as it collects training data?

\section{Related Work}
\label{sec:priorwork} 

Despite the fact that robot environments are three-dimensional, much of the prior work 
in robot mapping deals only with 2D map reconstruction due to its utility for robot 
navigation, fast visibility computation and  the fact that it is much easier to construct. 
Only more recently many researches have been attracted to the idea of having a 3D model 
of the environment. Most of this research work focuses on the extraction of
geometric relationships and calibration parameters in order to
achieve realistic~\footnote{In this context, realistic means that
the final model contains geometric and photometric details.} and
accurate representations of the world. In most cases it is not
easy to extract the required features, and human intervention is
often required. Moreover, real world environments include a large
number of characteristics and properties due to scene
illumination, sensor geometry, object geometry, and object
reflectance, that have to be taken into account if we want to have
a realistic and robust representation.

The idea of extracting shape or depth information from an image has been studied in the field of computer vision since the late 1960s. These techniques, known as {\em shape-from-X} techniques, extract depth information from intensity images by using cues such as shading, texture, retinal disparity and motion. This group of techniques are {\em passive}, i.e., they obtain image data without emitting energy, and are typically performed by designing mathematical models of image formation and inverting these models. These models are traditionally based on physical principles of light interaction. However, due to the highly underconstrained characteristic of the inverse problem of these principles, many assumptions about the type of surface and albedo need to be made, which may not be all suitable for the complex real scenes. A greater understanding of how real images are formed could lead to substantial insight into how depth information may be inferred from single images. 

Dense stereo vision gained popularity in the early 1990's due to
the large amount of range data that it could
provide~\cite{Little90,Fua93}. In mobile robotics, a common setup
is the use of one or two cameras mounted on the robot to acquire
depth information as the robot moves through the
environment~\cite{Murray00}.  The cameras must be precisely
calibrated for reasonably accurate results. The depth maps
generated by stereo under normal scene conditions (i.e., no
special textures or structured lighting) suffer from problems
inherent in window-based correlation. These problems manifest as
imprecisely localized surfaces in 3D space and as hallucinated
surfaces that in fact do not exist. Other works have attempted to
model 3D objects from image sequences
\cite{Tomasi92,Fitzgibbon98,Pollefeys98}, with the effort of
reducing the amount of calibration and avoiding restriction on the
camera motion. Fitzgibbon and Zisserman~\cite{Fitzgibbon98}
proposed a method that sequentially retrieves the projective
calibration of a complete image sequence based on tracking corner
and/or line features over two or more images, and reconstructs
each feature independently in 3D. Their method solves the feature
correspondence problem using methods based on the fundamental
matrix and tri-focal tensor, which encode precisely the geometric
constraints available from two or more images of the same scene
from different viewpoints. A similar work is that of Pollefeys et.
al.~\cite{Pollefeys98}, in which a 3D model of an object is
obtained from image sequences acquired from a freely moving
camera. The camera motion and its settings are unknown. Their
method is based on a combination of the projective reconstruction,
self calibration and dense depth estimation techniques. In
general, these methods derive the epipolar geometry and the
trifocal tensor from point correspondences. However, they assume
that it is possible to run an interest operator such as a corner
detector to extract from one of the images a sufficiently large
number of points that can then be reliably matched in the other
images. It appears that if one uses information of only one type,
the reconstruction task becomes very difficult and works well only
under narrow constraints.

Depth information from an object or scene can also be acquired directly by using active range sensors. These techniques are known as {\em active sensing} techniques.  This type of range sensors provide directly precise and accurate 3D points. In addition, they are independent from external lighting conditions and do not need any texture to perform well. Range sensors are commonly used to perform scene understanding for indoor mobile robots. However, these sensors tend to be expensive, the data acquisition process slow, and   normally of limited spatial resolution.

There is a vast body of research work using laser rangefinders for
different applications,  particularly, in the 3D reconstruction
problem~\cite{Connolly84,Chien88,Johnson97-1,Reed97,Whaite97,Nyland99,El-Hakim98,Sequeira99,Stamos00}.
In what follows, we will describe the basic concept of laser
rangefinders.

In the 90s, most existing approaches were based on a single
sensor~\cite{Debevec96,Johnson97-1,SoucyG95,Fitzgibbon98}. In
robotics, inter-robot communication~\cite{Balch94,Rekleitis02c} is
being used to overcome the limitations of using single robot
systems (which obtain sensor readings from one position at a
time). Depending on the application and the complexity of the
object or scene, achieving geometric correctness and realism may
require data collection from different sensors (passive and/or
active) as well as the correct fusion of all these observations.
The idea of using more than one sensor to complement the data of
one sensor with that of another is not new \cite{Nelson88}.
However, methods for data fusion are not as well developed as
those for the design and analysis of individual sensors.

Independent of the level of representation used, a variety of
popular mathematical techniques for sensor fusion appear in the
literature: for example, probability and Bayesian inference
\cite{Davies86}, Dempster-Shafer theory of evidence
\cite{Shafer76}. These established techniques are then
incorporated into a framework for transforming data into a common
coordinate system, producing verdicts on the correctness of the
various sources, and allowing stable estimation of the parameters
of a problem.  An important aspect of each technique is the way in
which it models uncertainty in sensor information.

A vast body of research on 3D modeling and virtual reality applications 
has been focused on the fusion of intensity and range data with promising 
results~\cite{Pulli97,El-Hakim98,Sequeira99,Levoy00,Stamos00}. These works 
all consider the complete acquisition of 3D points from the object or scene to be 
modeled, focusing mainly on the registration and integration problems. However,
one of the main goals in building (3D) map representations, is to simplify the 
way sensor data is acquired, to minimize time and energy consumption, yet 
accurately and reliably recover the 3D layout of an scene. Thus, the notable
difference of our research work with those in the literature is that the amount of
the range data acquired is {\em very} small compared to the intensity data. There is 
no prior work (except for these authors previous work 
(see~\cite{torres04:_aaai,torres04:_iros,AbrilThesis05}) that uses partial depth 
measurements  together with the intensity images, for {\em complete} and 
{\em dense} scene recovery.


\section{Our framework}

3D environment modeling typically involves two main goals: 1) the detailed {\em shape} of specific objects or targets within the scene; and 2) the spatial {\em layout} of objects within the scene. The first goal involves the accurate reconstruction of the sensed objects, perhaps for reprojection from novel viewpoints, or for higher level shape analysis for a cultural exposition, for example. The second goal is important for the purpose of navigation and localization.  
 
If the model is required only for navigation purposes, which is one of the most important problems in mobile robotics, then the second goal, i.e., the spatial relationships between objects is important. If the model will be used for virtual walkthroughs, example in a museum, then both goals must be fulfilled. In other words, the geometric and photometric details from specific viewpoints must be captured in the final representation. 

Since we are dealing with incomplete range data, the above mentioned goals involve an additional process: the synthesis of complete and dense range maps from sparse partial distance measurements. Therefore, we divide the 3D environment modeling in the following stages (see Figure~\ref{Fig_Diagram}):
\begin{figure}[*t]
\includegraphics[width=0.9\linewidth]{figures/fig_diagram.eps} %OJO flowchartwithpictures!!!!!!
\caption{Diagram showing the procedures to be followed for the 3D environment modeling problem.}
\label{Fig_Diagram}
\end{figure}
\begin{itemize}
\item data acquisition of the intensity and partial range and their registration to a common reference frame; 
\item range synthesis, which refers to the estimation of dense range maps locally at each robot pose and globally when integrating data from different views; 
\item data integration to a global map; and

\item 3D model representation. % (not covered in this dissertation).  
\end{itemize}

The following sections describe the first three stages of the 3D environment modeling in detail.

\section{Data Acquisition and Registration}
\label{Sec:AcqReg}
This section describes the data acquisition system used to capture the appearance and geometry of {\em large-scale} indoor environments. It consists of a CCD camera and a 2D laser rangefinder mounted on a mobile robot. The main aspect of our data acquisition system relies on {\em how} the data is acquired, which provides two important benefits:  {\it i})~ it allows the robot to rapidly collect sparse range data and intensity images while navigating the environment to be modeled, and {\it ii})~it facilitates the sensor-to-sensor registration. The first benefit is essential when dealing with large environments, where the acquisition of huge amount of range data is a time consuming and impractical task.  The second benefit is related to the complexity of registering different types of sensor data, which have different projections, resolutions and scaling properties. To this end, an image-based technique is presented for registering the range and intensity data that takes advantage of the way data is acquired.

This work focuses on modeling man-made indoor environments. Man-made indoor environments have inherent geometric and photometric characteristics that can be exploited to 
help in the reconstruction. In general, we assume that the environment is static, composed of
smooth planar surfaces (e.g. floors, doors, walls and furniture present large planar surfaces), 
most of the regions contain low texture. Also, the illumination of the environment is assumed to
be constant, that is the brightness or darkness in some places does not dependent on the time 
of day that images are acquired.

In large-scale indoor environments (e.g., offices, labs, museums, etc.), all surfaces will rarely be visible from a single viewpoint, nor will data be acquired at sufficient resolution to encompass the entire layout.  We use the robot to navigate the environment, and together with is sensors, captures the geometry and appearance of the environment in order to build a complete 3D model. The presented work does not consider the problem of planning the robot's trajectory, and we assume that a planning strategy is given (see for example~\cite{Gonzalez02}). 

\subsection{The Data Acquisition System}
The aim of our system configuration is to reduce the data acquisition time and facilitate the registration process. On top of the robot, we have assembled a system consisting of a 2D
{\em laser rangefinder} (laser scanner) and a high-resolution
digital {\em camera}, both mounted on a {\em pan unit} (see 
Figure~\ref{Fig_SensorsPan}). The camera
is attached to the laser rangefinder in such a way that their
center of projections (optical center for the camera and mirror
center for the laser) are aligned to the center of projection of
the pan unit. This alignment facilitates the sensor-to-sensor
registration, that is, the registration between the intensity and
range data (described in the next section), as we only need to 
know their projection types in order to do image mapping.
\begin{figure}[t*]
\begin{center}
\includegraphics[width=0.65\linewidth]{figures/fig_config.eps}
\caption{Arrangement of the sensors on the pan unit on the mobile
robot.} \label{Fig_SensorsPan}
\end{center}
\end{figure}
We assume dense and uniformly sampled intensity images, and sparse
but uniformly sampled range images. Since taking images from the
camera is an effortless task, sampling of intensity images occurs
more often than that of range images. The total sampling area of
both intensity and range images remains fixed all the time,  i.e.,
the area covered by the sampling data is equal at each robot pose.
This area covers approximately a view of $90^o$. However, the
amount of range data may vary depending essentially on the
sampling strategy.

\subsubsection{Acquiring Partial Range Data}

The configuration of the sensors also plays an important role in
the sampling scheme. In our acquisition framework, the spinning
mirror (y-axis) of the laser rangefinder and panning motor
(x-axis) combine to allow the laser to sweep out a
longitude-latitude sphere. Since each step taken by the pan unit
can be programmed (i.e., the stepping angle can be different at
each step) we can have different sampling strategies to acquire
sparse range data.

In obtaining partial range data, we need to consider two key
factors that will reflect in the quality of the whole range map.
These are: the sampling strategy used and the amount of total
range acquired.  The sampling strategy will depend on how far the
objects/walls in the scene are from the robot. Thus, as the robot
gets closer to walls/objects, the subsampling can be sparser since
no much details are lost, compared to when the robot is located
far away. Figure~\ref{Fig_HeuristicSubsampling} shows this simple
heuristic, which is used in collecting our experimental data. It
is essentially based on the relationship between the size of the
objects and the distance from where those objects are seen: the
farther we are from an object, the smaller it looks, so dense
sampling is needed to accurately represent its shape. On the other
hand, the closer we are to an object, the larger it looks, so
sparse subsampling is enough.
\begin{figure}
\begin{center}
\subfigure[]{\includegraphics[width=0.38\linewidth]{figures/fig_subsamp.eps}}\\
\subfigure[]{\includegraphics[width=0.48\linewidth]{figures/fig_subsamp_far.eps}}
\subfigure[]{\includegraphics[width=0.484\linewidth]{figures/fig_subsamp_close.eps}}
\caption{Sampling strategy based on distance of the scene from the
robot. (a) The scene. (b) Robot is far from the objects on the
scene, the subsampling is dense. (c) Robot is close to the
objects, thus the subsampling is sparser.}
\label{Fig_HeuristicSubsampling}
\end{center}
\end{figure}

\subsubsection{Acquiring the Cylindrical Panorama Mosaic}
\label{Sec:CylPan}

A cylindrical panorama is created by projecting images taken from
the same viewpoint, but with different viewing angles onto a
cylindrical surface. Each scene point ${\bf P}=(x,y,z)^T$ is
mapped to cylindrical coordinate system $(\psi,v)$ by
\begin{eqnarray}
\psi = \arctan (\frac{x}{z} ),  \nonumber \\
v = f \frac {y}{\sqrt{x^2 +z^2}}.
\end{eqnarray}
where $\psi$ is the panning angle, $v$ is the scanline, and
$f$ is the camera's focal length. The projected images are
"stitched" and correlated in order to precisely determine the
amount of rotation between two consecutive images, this process is
called {\em image spatial aligment} or {\em image registration}.
In the cylindrical space, a translation becomes a rotation, so we
can easily build the cylindrical image by translating each
component image with respect to the previous one. In practice,
there is also small vertical misalignment between images due to
vertical jitter and optical twist. Therefore, both a horizontal
translation $t_x$ and a vertical translation $t_y$ are estimated
for each input image. To recover the translational motion, we
estimate the incremental translation $\delta{\bf t} = (\delta
t_x,\delta t_y)$ by minimizing the intensity error between two
images,
\begin{equation}
E(\delta\bf t)=\sum_i[I_1(x'_i + \delta t) - I_0(x_i)]^2,
\end{equation}
where ${\bf x}_i=(x_i,y_i)$ and ${\bf x}'_i=(x'_i,y'_i)
=(x_i+t_x,y_i+t_y)$ are corresponding points in the two images,
and $\bf t=(t_x,t_y)$ is the global translational motion field
which is the same for all pixels. After a first order Taylor
series expansion, the above equation becomes
\begin{equation}
E(\delta\bf t) \approx \sum_i[g_i^T \delta t + e_i]^2,
\end{equation}
where $e_i=I_1(x'_i)-I_0(x_i)$ is the current intensity or color
error, and $g_i^T=\nabla I_1(x'_i)$ is the image gradient of $I_1$
at $x'_i$. This minimization problem has a simple least-squares
solution,
\begin{equation}
(\sum_i g_i g_i^T)\delta\bf t = -  (\sum_i[e_i g_i]).
\end{equation}
The complexity of the registration lies on the amount of overlap
between the images to be aligned. In our experimental apparatus,
we will typically have about $20$ to $30$~\% of overlap
between adjacent images.  However, as the panning angles at which
images are taken is known, the overlap can be as small as $10$~\% 
and still be able to align the images.

When aligning the images, corresponding points often have
different intensity values for various reasons, including change
in view angle of the camera, vignetting (intensity decreases
towards the edge of the image), parallax effects due to unwanted
motion of the optical centre, and change in scene lighting. To
reduce discontinuities in intensity between images, we weight each
pixel in every image proportionally to their distance to the edge
of the image (i.e., it varies linearly from $1$ at the centre of
the image to $0$ at the edge), so that intensities in the overlap
area show a smooth transition between intensities in one image to
intensities of the other image. A natural weighting function is
the {\em hat function},
\begin{equation} {\bf w}(x,y) = \|
\frac{h/2 - x} {h/2} \| - \| \frac{w/2- y}{w/2} \|
\end{equation}
where $h$ and $w$ are the height and the width of the image.
Intuitively, this function gives less weight to the pixels along
the image edges.

\subsection{Camera-Laser Data Registration: Panorama
with depth}
\label{Sec:DataRegistration}

To effectively use the panoramic image mosaic and the incomplete
spherical range data for range synthesis, model building and
rendering, both sensor inputs need to be registered with respect
to a common reference frame. Note that the problem is eliminated
when the data is acquired using the same sensor, as in the case of
range from stereo, structured light or range and radiance images
acquired by a  laser rangefinder, but it becomes complex when data
come from two or more different types of sensors. Their
projections, resolutions and scaling properties are different due
to the fact that they capture different types of data representing
different physical aspects of the environment (i.e., appearance
and distance). Therefore, before any meaningful high-level
interpretation of the acquired images can be made, the projective
model for both sensing technologies must be understood. The laser
scanner and the CCD camera work with different coordinate systems
and they must be adjusted one to each other. The software of most
laser scanner delivers spherical coordinates, whereas the camera
puts out data in a typical image projection. Further, the limited
range information do not permit the use of typical methods for
registration, such as feature extraction.

Determining the relationship between the projective models of the
camera and laser rangefinder can be done through calibration, and
is the basis of many geometry-based techniques. It requires that
both sensors, together with the pan unit, be fixed on the mobile
robot once the relationship is calibrated. In our method, even
though we also have a fixed sensor configuration, there is no
calibration involved. Instead, an image-based technique, similar
to that presented in~\cite{Cobzas03}, is used that recovers the
projective model transformation by computing a direct mapping
between the points in the data sets. The accuracy of this method
depends on the characteristics of the physical system and the
assumptions made (e.g. affine camera or planar scene), but in
general their performance is good for locally recovering a mapping
between data sets.

The approach we take assumes that the optical center of the camera
and the mirror center of the laser scanner are vertically aligned
and that the orientation of both rotation axes coincide. Thus, we
only need to transform the panoramic camera data into the laser
coordinate system. The following section describes the geometric
relationship between the range and intensity data acquired from
the sensors, in other words, the relation between laser points and
image pixels with respect to their coordinate systems.

As both data are in different independent coordinate systems, the
first step in the registration process is to map both type of data
to a cylindrical coordinate system. We already described how the
acquired intensity images are stitched together to form a
cylindrical panorama. To convert the spherical range image to a
cylindrical representation similar to that of the panoramic image
mosaic, the radius of the cylindrical range image must be equal to
the camera's focal length. This mapping is given by
\begin{equation}
{\bf P}(r, \theta ,\phi)\mapsto {\bf P}(r,\phi,\frac{f}{\tan
\theta})\mapsto{\bf P}(r, \phi,h)
\end{equation}

\noindent  where $r$ represents the distance from the center of
the cylinder to the point, $h$ is the height of the point
projected on the cylinder, $\phi$ is the azimuth angle and $f$ the
focal length of the camera (see Figure~\ref{FigCylProj}). Again,
this data is sampled on a cylindrical grid $(\phi,h)$ and
represented as a cylindrical image. The same procedure is applied
to the amplitude data to obtain the cylindrical amplitude image.
\begin{figure}[t]
\begin{center}
\includegraphics[width=0.78\linewidth]{figures/fig_cylproj.eps}
\caption{Projection of the 3D point $P$ onto cylindrical coordinates: $(\phi,h)$ 
for the range data and $(u,v)$ for the panoramic mosaic.}
\label{FigCylProj}
\end{center}
\end{figure}

Once having the intensity and range data in similar cylindrical
image representations, a {global mapping} between them is
computed. The physical configuration of the sensors is
approximated, assuming only a vertical translation $\Delta Y$ and
a pan rotation between the two reference coordinate systems LCS
(laser coordinate system) and CCS (camera coordinate system). For
a point $x_l(\phi,h)$ in the cylindrical laser image, its
corresponding point in the panoramic mosaic $x_c(u, v)$ is
\begin{eqnarray}
\nonumber u =  a \phi + \alpha, \,\,\,\,\,\,\,& & \\
v  = f   \frac {Y - \Delta Y}{r}  & = & f \frac{Y} {r} - f \frac
{\Delta Y}{r}   \,\,\, =  \,\,\, bh - f \frac{\Delta Y}{r}
\end{eqnarray}
where $a$ and $b$ are two warp parameters that will account for
difference in resolution between the two images, $\alpha$ aligns
the pan rotation, and $\,Y = r h/\sqrt{f^2+h^2}\,\,$ is the height
of the 3D point $X(r, \phi, h)$. Since $f$, $\Delta Y$, and the
$r$ remain fixed through the experimental setup, the term $f
\frac{\Delta Y}{r}$ can be approximated to a constant $\beta$.
Thus, the general warp equations are:
\begin{eqnarray}
\label{eqglobalmapping} u = a\phi + \alpha, & v = b h + \beta
\end{eqnarray}

The warp parameters $(a, b, \alpha, \beta)$ are computed by
minimizing the sum of the squared error of two or more
corresponding points~\footnote{Since the computation of the warp
parameters is done only once, the actual number of corresponding
points must be a large number, ideally $20$, to obtain a good
linear fit.} in the two images. In the case of a good (i.e., what
we hope to be normal) initial estimate, a reasonable convergence
to a nearly correct~\footnote{The term "correct" simply means that
the final solution "looks good." There is not recorded ground
truth for this purpose.} solution is obtained. The initial
estimate places the panorama mosaic nearly aligned with the range
data, with a moderate translation or misalignment typically of
about $5$ to $7$~pixels. To correct this, a {\em local alignment}
is performed using the set of corresponding control points. Since
these control points are typically obtained by matching the 2D
edges on the intensity image to the 3D edges on the range image,
after calculating the misalignment with the warp parameters, we
can locally "stretch" the range data to fit the intensity data by
using cubic interpolation and check for additional matchings
between $2D$ and $3D$ edges. After that, each range data point
acquired by the laser rangefinder has a corresponding pixel value
in the panoramic image. Since the resolution of the depth data is
sparser than the resolution of the image data, the inverse mapping
cannot be computed because the function is not one-to-one.

\section{Range synthesis}

After registering the intensity and partial range information at
every robot pose, we apply our range synthesis method. A crucial
aspect is the sampling strategy used when the robot is at
particular pose. The denser the sampling the easier the range map
estimation. However, since our goal is to minimize time and energy
consumption in the data acquisition process, we want to be
strategic in sampling the 3D data. We use a heuristic that is
based on the distance the robot is from the objects in the scene.
Basically, the closer the robot is to the objects, the denser the
sampling is, and the further the robot is from the objects, the
sparser the sampling of 3D data. The advantages and limitations of
this heuristic are analyzed.

The following sections detail our statistical learning method for
depth recovery. Specifically, we estimate dense or high resolution
range maps of indoor environments using only intensity images and
sparse partial depth information. Markov Random Field (MRF) models
are proposed as a viable stochastic model for the spatial
distribution of intensity and range data. This model is trained
using the (local) relationships between the observed range data
and the variations in the intensity images and then used to
compute unknown depth values.  Two techniques for the MAP-MRF
estimation are described, the first one based on a non-parametric
sampling strategy and the second one by using the belief
propagation (BP) algorithm. Their advantages as well as their
limitations are highlighted.

\subsection{The MRF Model}
The range estimation problem can be posed as a labeling problem. A
labeling is specified in terms of a set of {\em sites} and a set
of {\em labels}. In our case, sites represent the pixel
intensities in the matrix $I$ and the labels represent the depth
values in $R$. Let ${\mathcal S}$ index a discrete set of $M$
sites ${\mathcal S}=\{s_1,s_2,...,s_M\}$,  and ${\mathcal L}$ be
the set of corresponding labels ${\mathrm L}=\{l_1,l_2,...,l_M
\}$, where each $l_i$ takes a depth value. The inter-relationship
between sites and labels define the {\em neighborhood system}
$\mathcal{N}=\{N_s\ \mid \forall s \in {\mathcal S} \}$, where
$N_s$ is the set of {\em neighbors} of $s$ (i.e., the neighborhood
of $s$), such that (1) $s \not\!\in N_s$, and  (2) $s \in N_r \iff
r \in N_s$ (see Figure~\ref{Fig_Neighborhood}).  Each site $s_i$
is associated with a random variable $F_i$. Formally, let
${\mathrm F} = \{F_1, ..., F_M\}$ be a random field defined on
${\mathcal S}$, in which a random variable $F_i$ takes a value
$f_i$ in ${\mathcal L}$. A realization ${\it f} ={f_1,...,f_M}$,
is called a {\em configuration} of $\mathrm F$, corresponding to a
realization of  the field. The random variables $\mathrm{F}$
defined on $\mathcal{S}$ are related to one another via the
neighborhood system ${\mathcal N}$.

\begin{figure}[t*]
\begin{center}
\includegraphics[width=0.5\linewidth]{figures/fig_neighSites.eps}
\caption{A neighborhood system definition. }
\label{Fig_Neighborhood} \end{center}
\end{figure}

$\mathrm F$ is said to be an MRF on ${\mathcal S}$ with respect to
${\mathcal N}$ if and only if the following two conditions are
satisfied~\cite{Hammersley71}:

\vspace{0.35cm}
$P({\it f}) >0$  (positivity), and \\
\indent $P(f_i \mid f_{{\mathcal S}-\{i\}})= P(f_i \mid f_{{\it
N}_i})$ \, (Markovianity). \vspace{0.35cm}

\noindent where ${\mathcal S}-\{i\}$ is the set difference,
$f_{{\mathcal S}-\{i\}}$ denotes the set of labels at the sites in
${\mathcal S}-\{i\}$ and $f_{\it N_i}=\{f_i' \mid i' \in {\it
N_i}\}$ stands for the set of labels at the sites neighboring $i$.
The Markovianity condition describes the local characteristics of
${\mathrm F}$. The depth value (label) at a site is dependent only
on the augmented voxels (containing intensity and/or range) at the
neighboring sites. In other words, only neighboring augmented
voxels have direct interactions on each other.

The choice of $\,\it{N}\,$ together with the conditional
probability distribution of $P(f_i\mid f_{{\mathcal S}-\{i\}})$,
provides a powerful mechanism for modeling spatial continuity and
other  scene features. On one hand, we choose to model a
neighborhood ${\it N}_i$ as a square mask of size $n \times n$
centered at pixel location $i$, where only those augmented voxels
with already assigned intensity and range values are considered in
the synthesis process. Thus, the neighborhood is in fact, of an
arbitrary shape depending on the current available information on
each of its augmented voxels.  On the other hand, calculating the conditional
probabilities in an explicit form to infer the exact maximum {\em
a posteriori} (MAP) in MRF models is intractable. We cannot
efficiently represent or determine all the possible combinations
between pixels with its associated neighborhoods. Various
techniques exist for approximating the MAP estimate, such as
Markov Chain Monte Carlo (MCMC), iterated conditional modes (ICM),
maximizer of posterior marginals (MPM), etc. 

In our research, we avoid the computational expense of sampling
from a probability distribution and take two different approaches
for the MAP-MRF estimation. The first approach is based on a
non-parametric sampling strategy that is easy to implement,
generates good results and is fast to execute. The second
approach, which is an extension of the first, uses the belief
propagation (BP) algorithm to compute marginal probabilities. This
approach improves the synthesized results of those of the first
approach, at the expense of additional computation.

\subsection{Non-parametric Sampling and MAP-MRF estimation}
\label{Sec:NPS}

To compute the MAP estimate for a depth value $R_i$ of the
augmented voxel $V_i$, one first needs to construct an
approximation to the conditional probability distribution $P(f_i
\mid f_{N_i})$ and then sample from it. We could use the
parameters themselves, estimated from the given input data or
samples  (neighborhoods from input intensity and range data), as
an approximation to maximum likelihood sampling, however this is
hindered by the need to compute the partition function $Z$ which
is usually computationally intractable. Instead, for each new
depth value $R_i \in {\mathrm R}$ to estimate, the samples, which
correspond to the neighborhood system for the voxel location $i$,
$\mathcal N_i$, are queried and the distribution of $R_i$ is
constructed as a histogram of all possible values that occurred in
the samples. $\mathcal N_i$ is a subset of the real infinite set
of augmented voxels, denoted by $\mathcal N_{real}$.

Based on our MRF model, we assume that the depth value $R_i$
depends only  of its immediate neighbors in intensity and range,
i.e. of ${N}_i$. If we define a set
\begin{equation}
\Gamma(R_i) = \{ N^\star \subset \mathcal N_{real}: \; \parallel
N_i -N^\star\parallel = 0 \}
\end{equation}
containing all occurrences of $N_i$ in $\mathcal N_{real}$, then
the conditional probability distribution of $R_i$ can be estimated
with a histogram based on the depth values of voxels representing
each $N^\star$ in $\Gamma(R_i)$.

Unfortunately, we are only given $\mathbf V$, i.e., a finite
sample from $\mathcal N_{real}$. Thus, there might not be any
neigbhorhood containing exactly the same characteristics in
intensity and range as $\it{N}_i$ in $\mathbf V$. Thus, we must
use a heuristic which let us find a plausible $\Gamma'(R_i)
\approx \Gamma(R_i)$ to sample from.

Let $\mathcal{A}_p$ be a {\em local} neighborhood system for the
augmented voxel $p$, that comprises nearby neighborhoods within a
radius $r$,
\begin{equation}
\mathcal A_p = \{A_q \in \mathcal N \mid distance(p,q) \leq r \}
\end{equation}  %such that $distance(p,q)\leq d$.
where $R_q$ is a known depth value. In other words, the local
neighborhood system contains all the neighborhoods which center
voxels have already assigned labels (depth values), located at a
maximum distance $r$ from the location of voxel $p$. This set will
conform the training/sample data for that particular voxel $p$.

In the non-parametric approach, a depth value $R_p$ from the
augmented voxel $V_p$ with neighborhood $N_p$, is synthesized by
first selecting the most similar neighborhood ($N_{best}$) to
$N_p$, i.e., the closest match to the region being filled in,
\begin{equation}
N_{best} = \arg \min_{A_q  \in \mathcal{A}_p} \parallel N_p - A_q \parallel.
\end{equation}

Second, all the neighborhoods $\it {A}_q$ in $\mathcal{A}_p$ that
are similar (within a threshold $\epsilon$) to this $N_{best}$ are
included in $\Gamma'(R_p)$,  as follows
\begin{equation}
\parallel N_p - A_q \parallel < (1+\epsilon) \parallel \it{N}_p - \it{N}_{best} \parallel
\end{equation}
The similarity measure $\parallel . \parallel$ between two generic
neighborhoods $N_a$ and $N_b$ is described over the partial data
in the two neighborhoods  and is calculated as follows,
\begin{equation}
\parallel N_a - N_b \parallel = \sum_{\vec{v} \in N_a,N_b} G(\sigma,\vec{v}-\vec{v}_0)
    \sqrt{(I^a_{\vec{v}}-I^b_{\vec{v}})^2 + (R^a_{\vec{v}}-R^b_{\vec{v}})^2},
\end{equation}
where $\vec{v}_0$ represents the augmented voxel located at the
center of the neighborhoods $N_a$ and $N_b$, $\vec{v}$ is a
neighboring voxel of $\vec{v}_0$. $I^a$ and $R^a$ are the
intensity and range values of the neighboring augmented voxels of
the depth value ${R_p} \in \vec{v}_0$ to synthesize, and $I^b$ and
$R^b$ are the intensity and range values to be compared with and
in which, the center voxel $\vec{v}_0$ has already assigned a
depth value. $G$ is a 2-D Gaussian kernel applied to each
neighborhood, such that those voxels near the center are given
more weight than those at the edge of the window.

We can now construct a histogram from the depth values $R_p$ in
the center of each neighborhood in $\Gamma'(R_p)$, and randomly
sample from it. $R_q$ is then used to specify $R_p$. For each
successive augmented voxel this approximates the maximum {\em a
posteriori} estimate.

\subsection{MAP-MRF using Belief Propagation (BP)} \label{Sec:BP}

In order to propagate evidence, we use a pairwise Markov network.
In our case, observation nodes represent image patches where some
of the voxels may contain both intensity and range information, or
only intensity information;  the hidden nodes are the depth values
to be estimated of the center voxels of the image patches of the
observable nodes. BP efficiently estimates Bayesian beliefs in the
MRF network by iteratively passing messages between neighboring
nodes. We illustrate this by using an example. Figure~\ref{Fig_RI}
shows \begin{figure}[t*]
\centering
\subfigure[Input intensity]{\includegraphics[width=0.26\linewidth]{figures/fig_teddy_I.eps}}
\subfigure[Input range]{\includegraphics[width=0.26\linewidth]{figures/fig_teddy_Rs.eps}}
\caption{\label{Fig_RI} Input data used to illustrate the MAP-MRF using BP.}
\end{figure} the input intensity and range images, where white areas represent
unknown range values. The pairwise Markov network for the range
estimation problem is depicted in
Figure~\ref{Fig_MarkovNetworkRI}(b),
\begin{figure}[h*]
\centering
\includegraphics[width=0.95\linewidth]{figures/fig_markovnet_range.eps} \\
\caption{\label{Fig_MarkovNetworkRI} Pairwise Markov network for the range estimation problem. }
\end{figure}
where the observation node $y_i$ is a neighborhood in intensity centered at voxel location $i$ (see (a)), and the hidden nodes $x_i$ represent the depth values to be estimated (white areas in (c)), but also hidden nodes contain the already available range data (as image patches), whose beliefs remain fixed at all times; a subset of these available range is used to locally train the network and compute compatibilities between observation and hidden nodes  and the rest is used for propagating the beliefs {\em in} and {\em out} globally.

\subsubsection{Learning the Compatibility Functions}

The training pairs of intensity image patches with its
corresponding range image patches are used to learn the
compatibility functions. However, not all available (observed)
pairs of intensity and range image patches are used as training
pairs. We choose only a {\em local} set of image pairs that are
located up to a distance $d$ from the voxel location of the depth
value to be estimated. This reflects our heuristics about how the
intensity values locally provide knowledge about the type of
surface that intensity value belongs to.
\begin{figure}[b*]
  \centering
  \fbox{\includegraphics[width=0.36\linewidth]{figures/fig_overlap.eps}} \\
  \caption{\label{Fig_OverlapCompatibility}
           The compatibility between range image patch candidates~\cite{Freeman00}.} %OJO}
\end{figure}

As in the method described elegantly in~\cite{Freeman00}, we use
the overlapping information from the intensity image patches
themselves, to estimate the compatibilities $\Psi(x_j,x_k)$
between neighbors. Let $k$ and $j$ be two neighboring intensity
image patches. Let $d_{jk}^l$ be a vector of pixels of the $l$th
possible candidate for image patch $x_k$ which lie in the overlap
region with patch $j$. Likewise, let $d_{kj}^m$ be the values of
the pixels (in correspondence with those of $d_{jk}^l$) of $m$th
candidate for patch $x_j$ which overlap patch $k$ (see
Figure~\ref{Fig_OverlapCompatibility}). We say that image
candidates $x_k^l$ (candidate $l$ at node $k$) and $x_j^m$  are
compatible with each other if the pixels in their region of
overlap agree. We assume that the image and training samples
differ from the ''ideal'' training samples by Gaussian noise of
covariance $\sigma_i$ and $\sigma_s$, respectively.  Those
covariance values are parameters of the algorithm. Then, the
compatibility matrix between range nodes $k$ and $j$ are defined
as follows:
\begin{equation}
\Psi(x_k^l, x_j^m) = {\rm exp}^{- \mid d_{jk}^l - d_{kl}^m \mid ^2
/ 2 \sigma_s^2}. \label{Eq:PsiXX}
\end{equation}
The rows and columns of the compatibility matrix $\Phi(x_k^l,
x_j^m)$ are indexed by $l$ and  $m$, the range image candidates at
each node, at nodes $j$ and $k$.

We say that a range image patch candidate $x_k^l$ is compatible
with an observed intensity image patch $y_0$ if the intensity
image patch $y_k^l$, associated with the range image patch
candidate $x_k^l$ in the training database matches $y_0$. Since it
will not exactly match, we must again assume ''noisy'' training
data and define the compatibility
\begin{equation}
\Phi(x_k^l,y_k) = {\rm exp}^{- \mid y_k^l - y_o \mid ^ 2 / 2
\sigma_s^2}. \label{Eq:PhiXY}
\end{equation}

\subsubsection{The MAP estimate}

The maximum a posteriori (MAP) range image patch for node $i$ is:
\begin{equation}
x_{iMAP}=  \arg \max_{x_i} \; \Phi(x_i,y_i) \displaystyle  \prod_{j \in {\mathrm N}(i)} M_{ji}(x_i).
\end{equation}

\noindent where $N(i)$ are all node neighbors of node $i$, and
$M_{ji}$ is the message from node $j$ to node $i$ and is computed
as follows:
\begin{equation}
M_{ij}(x_j) = Z \displaystyle \sum_{x_i}
\Psi(x_i,x_j)\Phi(x_i,y_i) \displaystyle \prod_{k \in {\mathrm
N}(i) \setminus \{j\}} M_{ki}(x_i)
\end{equation}
where $Z$ is the normalization constant.

Resuming, the algorithm for the range estimation problem using the
MRF-BP framework is listed in Figure~\ref{Fig_algorithm}.

\begin{figure}[h*]
\begin{center}
{\small
\framebox[5.5in] { %[5.7in] {
\begin{minipage}[t]{5.3in}
\begin{center}
\begin{enumerate}
\item[] i) Divide the input registered images, i.e., the intensity and the (incomplete) range images, into small patches, which form the sets of $x_i$'s and $y_i$'s. The areas of unknown depth values, i.e., $\mathbf\Omega$, is divided in such a way that each voxel with missing depth has its own associated intensity image patch and available range patch. 
\item[]  ii) For each intensity image patch $y_i$ in $\mathbf \Omega$, find the $k$ closest $y_{x_i}$'s from the local training set located up to distance $d$ from that voxel location. The corresponding $x_i$'s are the candidates for that patch. 
\item[] iii) Calculate the compatibility function $\Phi(x_i,y_i)$ according to Eq.~\ref{Eq:PhiXY}. 
\item[] iv) For each pair of neighboring input patches, calculate the $k \times k$ compatibility function $\Psi(x_i,x_j)$ according to
Eq.~\ref{Eq:PsiXX}. 
 \item[] v)  Estimate the MRF-MAP solution using BP.
 \item[] vi) Assign the depth value of the center pixel of each estimated maximum probability patch $x_{iMAP}$ to the corresponding pixel in the range image patch.
\end{enumerate}
\end{center}
\end{minipage}
} % end framebox
} % end small
\end{center}
\vspace{-0.5cm}
\caption{The MRF-BP algorithm for range synthesis. }
\label{Fig_algorithm}
\end{figure}

Currently, the main drawback of using the BP-based method is the
computational time. The BP-based algorithm runs in $O(nk^2T)$
time, where $n$ is the number of pixels to be synthesized, $k$ is
the number of possible labels for each pixel and $T$ is the number
of iterations. It takes $O(k^2)$ time to compute each message and
there are $O(n)$ messages per iteration. The NP sampling method
runs in $O(Mpn)$ time, where $M$ is the number of neighborhoods to
be compared with, $p$ is the number of pixels that conform the
neighborhood of each pixel (e.g., $5\times 5= 25$ pixels). It
usually takes around $30$ seconds for an image of size $128\times
128$ in a 3 GHz Pentium 4 with 1 Gigabyte of RAM.

A critical issue on the range estimation process is the order in
which the depth values are synthesized.  The reconstruction sequence depends entirely on the priority values that are assigned to each voxel on the boundary of the region to be synthesized. The priority computation is biased toward those voxels that are surrounded by high-confidence voxels, and whose neighborhood does not represent an intensity discontinuity, in other words,  whose neighborhood does not have any edges on it. Furthermore, edge information is used to defer the synthesis of those voxels that are on an edge to the very end. When using the BP-based method, we essentially use the following message-passing rule:

\vspace{0.5cm}
{\bf Edge-constraint rule:} {\em A node can send a message to a neighboring node once it has received messages from all its other neighbors except those edge-labeled nodes.}
\vspace{0.5cm}


We start by giving some basic notations to explain how the methods we present reconstruct the unknown depth values (notation similar to that used in the inpainting literature~\cite{Criminisi03}). 

\begin{figure}[*h]
  \centering                            
  \includegraphics[width=0.5\linewidth]{figures/fig_notation.eps} \\ 
  \caption{\label{Fig_Notation} 
           The notation diagram. }
\end{figure}  

Figure~\ref{Fig_Notation} shows the notation diagram. The region to be synthesized, i.e., the {\em target} region is indicated by $\mathbf \Omega = \{\omega_i \mid i \in \mathcal{A}\}$, where $\omega_i=R(x_i,y_i)$ is the unknown depth value at location $(x_i,y_i)$ and $\mathcal{A}\subset Z_m$ is the set of subscripts for the unknown range data. The contour of $\Omega$ is denoted by $\delta\Omega$. The input intensity ($I$) and the known range values ($R_k$) together form the {\em source} region, and is indicated by $\mathbf \zeta$. This region $\mathbf \zeta$ is used to calculate the statistics between the intensity and the input range for reconstruction, as it was described above. Let $V_p$ be an augmented voxel with unknown range located at the boundary $\delta\Omega$ and $\it{N}_p$ be its neighborhood, which is a $n \times n$ square window centered at $V_p$. 

For all voxels $V_p \in \delta\Omega$, their priority value is computed (which is going to determine the order in which they are filled) as follows:
\begin{equation}
P(V_p)=C(V_p) D(V_p) + 1/(1+E).
\end{equation}
\noindent where $E$ is the number of edges found in the neighborhood or image patch $N_p$; $C(V_p)$ is the {\em confidence} term, and $D(V_p)$ the {\em data} term. The confidence term is defined as follows:
\begin{equation}
C(V_p)= \frac {\sum_{i\in N_p \cap \zeta} C(V_p)}{ | N_p |}, 
\label{Eq:Confidence}
\end{equation}
where $| \mathcal{N}_p |$ is the total number of voxels (augmented or not) in $\mathcal{N}_p$. At the beginning, the confidence of each voxel is assigned $1$ if its intensity and range values are filled and $0$ if the range value is unknown.  This confidence term $C(V_p)$ may be thought of as a measurement of the amount of reliable information surrounding the voxel $V_p$. Thus, as we reconstruct, those voxels whose neighborhood has more of their voxels already filled, are synthesized first, with additional preference given to voxels that were synthesized early on.  

\begin{figure}[t*]
  \centering                            
  \includegraphics[width=0.36\linewidth]{figures/fig_Method2.eps} \\ 
  \caption{\label{FigMethod2} 
           The diagram shows how priority values are computed for each voxel $V_p$ on $\delta\Omega$.
            Given the neighboorhood of $V_p$, $n_p$ is the normal to the contour $\delta\Omega$ of the target region $\Omega$ and $\nabla_{I_p}^\perp$ is the isophote (direction and range) at voxel location $p$.}
\end{figure}  

The data term $D(V_p)$ is computed using the available range data in the neighborhood $\mathcal{N}_p$, as follows (see Figure~\ref{FigMethod2}):
\begin{equation}
D(V_p)= \frac {\alpha} {| \nabla_{I_p}^\perp  \cdot n_p |}.   
\end{equation}
where $\alpha$ is a normalization factor (e.g. $\alpha=255$, in a typical gray-level image), $n_p$ is a unit vector orthogonal to the boundary $\delta\Omega$ at voxel $V_p$.  This term reduces the priority of a voxel in whose neighborhood an isophote ''flows'' into, thus altering the sequencing of the extrapolation process. This term plays an important role in the algorithm because it prevents the synthesis of voxels lying near a depth discontinuity.  Note, however, that it does not explicitly alter the probability distribution associated the voxel (except by deferring its evaluation), and thus
has only limited risk for the theoretical correctness of the algorithm.

Once all priority values of each voxel on $\delta\Omega$ have been computed, we find the voxel with the highest priority. We then use our MRF model to synthesize its depth value. After a voxel has been augmented (i.e. it has intensity and range data), the confidence of the $C(V_p)=C(V_q)$, i.e. it is assigned the confidence of the augmented voxel which most resemble the neighborhood of $V_p$ (see Eq.~\ref{Eq:Confidence}).


\section{Data Integration} \label{Sec:Integration}

Our approach for integrating the resulting {\em dense} panoramas
with depth from multiple viewpoints is based on a hybrid method,
similar to that proposed in~\cite{Bendels04}. This hybrid method
combines two techniques, one is for matching intensity features on
the panorama mosaic and the other matching two 3D range scans.
Since dense panoramas with depth are given as an input, their 
integration to a common reference frame is easier
than having only intensity or range data separately. To this end a
simple method  that combines the two previous techniques is
presented.

\subsection{Matching of Two Range Data Sets}

The common approach to register two range data sets from different
viewpoints is to derive an initial transformation by aligning a
small set of corresponding feature points in the range images.
State-of-the-art systems often require manual specification of
initial pose estimates or rely on external pose measurement
systems~\footnote{For instance, the relative viewpoint position
might be known, e.g., from tracking the scanner position. Although
direct and convenient, this is not always feasible due to the
characteristics of the environment and inherent errors in the
robot's odometry.}, making the pre-registration a tedious and
time-consuming task.  Recently, methods for automatically
registrating multiple range images have been proposed. One of the
most popular registration methods in the literature, is the {\em
Iterative Closest Point} (ICP) algorithm. The ICP algorithm,
introduced by Besl and McKay~\cite{Besl92}, iteratively searches
for closest point pairs in two surface patches and optimizes the
transformation to minimize the distances between these points.
Although the role of the two point pairs to be matched is
exchangeable, we speak of data points which are to be registered
with a model or model points.

\subsection{Feature Detection and Matching}

Extraction of geometric features from real-life range images is a
difficult task as only parts of the objects' surface are visible
due to occlusion and limited field-of-view. In the particular
(common) situation, when the surfaces of neighboring objects are
geometrically similar, distinguishing them as surfaces of
independent objects is practically impossible.

On the other hand, feature detection and matching in 2D images is
a well-researched topic, and algorithms robustly detecting
features that are insensitive to brightness changes, scaling or
local occlusions exist. In this research, we select to use the
Scale Invariant Feature Transform (SIFT), developed by
Lowe~\cite{Lowe99} (see also ~\cite{Lowe04}) based on earlier work
by Lindeberg~\cite{Lin93}). This algorithm was found to perform
best in a recent comparison study among several local feature
descriptors~\cite{Mikolajczyk03}. The study compared their
robustness with respect to noise, lighting and viewpoint changes
up to $60$ degrees.

The SIFT algorithm detects features with a scale parameter that
reflects the spatial extension of its defining image neighborhood.
This scale property is of vital importance for our method since it
allows to robustly estimate a 3D position for each detected image
feature.

We can directly derive a 3D feature position from a 2D feature
using the one-to-one correspondence between pixels in the
intensity image and the depth values in the range image. However,
as pointed out in ~\cite{Bendels04}, this is not advisable, as the
resulting 3D point is sensitive to noise and small feature
deviations. Therefore, instead of using a single 3D point (the
direct corresponding point to the 2D feature point) as feature, we
use a set of 2D-image features with intrinsic scale information
for finding corresponding points on the 3D views. These derived
features are called {\em feature surface
elements}~\cite{Bendels04} to accent that they are indeed a
surface realization of the scale-equipped feature
points~\footnote{The definition of these feature surface elements
is taken from the notion of {\em surfels}, i.e., surface points
equipped with normals. Surfels  implicitly store a local
first-order approximation of the neighboring surface. Analogously,
feature surface elements represent a sampling of the neighborhood.
Unlike surfels though, the feature surface elements represent a
region on the surface with a well-defined size known from the 2D
image features~\cite{Bendels04}.}.  Thus, a {\em feature point}
can be defined as the center of gravity of the respective feature
surface element. The set of corresponding feature points is
denoted by $\mathcal C_{lk}$ for any pair $(l,k)$ of range images
(see Figure~\ref{Fig_FeaturePoint}).
\begin{figure}[h]
\begin{center}
\includegraphics[width=0.85\linewidth]{figures/fig_featurepoints.eps}
\caption{Two range images with matching feature point and scale.
For the given corresponding feature point, only inside the
scale-induced feature surface element (the dotted circle) the two
range images can robustly be expected to contain corresponding
parts of the scene.} \label{Fig_FeaturePoint}
\end{center}
\end{figure}

The SIFT algorithm provides good matching results, however false
positive matches are possible. Since the subsequent registration
steps are sensitive to such false correspondences, an additional
filtering to the matches based on the RANSAC
method~\cite{Fischler81} is applied. First, the 3D positions of
the features are determined by checking their conformity with
respect to rigid transformations. Then, the set of matching
features in pair of images are validated.  Since it is
computationally expensive to actually compute the largest
conformal set of matching features (maximum clique), the RANSAC
method selects a set of three features pairs randomly and computes
its support, i.e. the set of all features pairs conforming to the
implied transformation. A support set is rejected if it is below a
certain size $\alpha$. This allows us to remove unreliable
correspondences, since large sets of false, yet conforming matches
are extremely improbable.

At this point, slight deviations in the 3D positions may occur
because the sampling of a feature surface element in different
images is usually not consistent.  While such deviated features
can be filtered out using the RANSAC approach to improve the
registration accuracy, we tolerate these deviations to a certain
extent to increase the number of conformal matches.

\subsection{Hybrid 2D and 3D Feature Matching Approach}

Our integration algorithm is incremental in the sense that
additional 3D panoramas (i.e., panoramas with depth) can be
incorporated into a set of already integrated 3D panoramas very
efficiently. The feature detection is performed unilaterally
(constant time), whereas the feature matching has to be done with
respect to each of the intensity information in the panoramas in
the given set (linear). Results from previous 3D panorama
integration can nonetheless be exploited.

\subsection{Pairwise Registration}
\label{Sec:PairwiseReg}

Let $P^l=\{{\mathbf p}^l_i \mid i=1,...,n_l\}$ be a set of
scale-equipped feature points of the range image $l$. For any pair
$(l,k)$ of range images we have a (possible empty) set of
correspondences

 \begin{equation}
 C_{lk}=\{(i,j) \mid {\mathbf p}_i^l \in P^l \, \, {\rm and} \, \, {\mathbf p}^k_j \in P^k \,\, {\rm corresponding}\, \}.
 \end{equation}

In the following sections, we describe the two-stage registration
procedure for a pair $(l,k)$ with non-empty correspondence set
$C_{lk}$.% (see Figure~\ref{Fig_FineCoarseReg}).

\subsection{Coarse registration}

The coarse registration step consists of aligning the point sets
$P^l$ and $P^k$ in a least square sense, i.e., among the set of
all rigid transformations we are looking for the solution to the
local minimization problem
\begin{equation}
T_{lk} = \arg \min_T \; {\mathbf \varepsilon} (T \cdot  l,k),
\end{equation}

\noindent where the registration error $\mathbf \varepsilon$ is
defined as

\begin{equation}
{\mathbf \varepsilon}(l,k) = \displaystyle\sum_{(i,j)\in\mathcal
C_{lk}} d^2({\mathbf p}^l_i, {\mathbf p}^k_j).
\end{equation}

This is a non-iterative procedure since correspondences are known
and fixed. The result is a fast and efficient initial registration
for $l$ and $k$. However, the alignment based solely on the
feature points accounts only for a fraction of the information
available in the range images~\footnote{Typically, the number of
feature points is in the order of dozens compared to the several
hundred thousands of data points.}. To compensate for the errors
induced in the feature point computation as described in the
previous section a second registration step is performed.

\subsection{Fine registration}

After the coarse registration, the pre-aligned pair of range
images can be registered by applying one of the many variants of
the ICP algorithm. Although, they have proven to lead to excellent
registration results for good starting positions, they are,
unfortunately, computationally non-trivial and susceptible to
false correspondences, which might lead to slow convergence and,
more importantly, to run into a local minima. Therefore, we solve
these problems by restricting the domain for the correspondence
computation to regions of the object that are known to correspond
(from the feature detection in the 2D images, we know that the
feature surface elements constitute corresponding parts of the
surface).

Thus,  for all pairs $(i,j) \in {\mathcal C}_{lk}$, we find new
correspondences at closest point pairs in the according sets of 3D
points. These enhanced correspondence sets are then aligned using
standard ICP techniques.

It is important to note, however, that the 2D feature matching
procedure does not take into account the distribution of the
feature points over the range images. Thus, substantial
registration errors may occur in regions far from the feature
surface elements for cases where the bounding box of the feature
surface elements is very small compared to the bounding box of the
range image itself.  One solution to resolve the remaining
inconsistency could be to perform a final ICP stage on the full
data.  However, in our experiments, the two-step registration
process by feature surface elements alignment proved to be
sufficient.


\section{Experimental Results}
\label{Sec:ExpResults} In order to validate our approach for 3D
environment modeling, we carried out experiments into two
environments. The environments are different in size and the type
of objects they contain. The first environment is a medium-size
room (approximately 9.5$\times$6$\times$3m, which
corresponds to our Mobile Robotics laboratory. It contains the
usual objects in offices and labs (e.g., chairs, tables,
computers, tools, windows, etc.) The second environment is larger
(approx. 2$\times$20$\times$3m)  and corresponds to
the corridors of the CIM floor~\footnote{The Center for
Intelligent Machines (CIM) floor is the $4^{th}$ floor of the
McConnell Engineering Building at McGill University.}. This
environment is mostly composed of walls, doors, windows.

In the following sections, we will give details about the hardware
setup, implementation and show the experimental results for the
environments described above for each of the stages involved in
the 3D environment modeling.

\subsection{Acquiring Intensity and Range Data}

 \begin{figure}[t*]
\begin{center}
\includegraphics[width=0.32\linewidth]{figures/fig_scout_titles.eps} \caption{Our mobile robot. Nomad Super Scout II with the 2D laser range finder and the camera mounted on it.}
\label{FigRefAvarice}
\end{center}
\end{figure} 
The mobile robot used in our experiments is a Nomad Super Scout
II, manufactured by Nomadics, Inc., retrofitted and customized for
this work. It has a mobile base equipped with two driven by servo
motors. Mounted on the robot are the 2D laser range finder and the
CCD camera (Figure~\ref{FigRefAvarice}). At each robot pose, the intensity and range information is acquired and then the robot
moves approximately 1m from the previous pose (this rough odometry
is consider to integrate the multiple views).

Intensity images are acquired by using a Dragonfly camera from
Point Grey Research, with a resolution of $1024\times 768$ at 15
frames per second. The camera, attached to the laser, is mounted
on the pan unit, allowing for panoramic image acquisition. In
Figure~\ref{Fig_Exp1y2_Intensity}, two intensity images acquired
from the two environments are depicted.
Figure~\ref{Fig_Exp1y2_Intensity}(a) is an image from our lab and
(b) an image from the corridors of the CIM floor.
\begin{figure}[hb*]
\begin{center}
\subfigure[An image from our lab.]{\includegraphics[width=0.35\linewidth]{figures/fig_Exp_IntensityImage1.eps}}
\subfigure[Image from CIM floor.]{\includegraphics[width=0.35\linewidth]{figures/fig_Exp_IntensityImage2.eps}}
 \caption{Intensity images from the environments to model.}
\label{Fig_Exp1y2_Intensity}
\end{center}
\end{figure}

To acquire the range images, we use an infrared 8mW AccuRange
4000-LIR, from Accuity Research, Inc. The scanning range of the
laser allows distance measurements between 0 to 15 meters at up to
50,000 range readings per second, with precision better than 1cm.
A mirror, $45^o$ off-axis, rotates about a shaft with a
2000-position encoder to sweep out a plane. The mirror center of
projection of the laser rangefinder is attached to the center of
rotation of the pan unit. The pan unit was built in our lab, and
has a very clean interface, making programming very simple. The
panning angle (the horizontal angle) covers an area of $180^o$
with one step of $0.36^o$. Since we are looking at having quality
range values, each (2D) slice is an average of 10 samples, taking
around 0.5 sec. per sample with a slower sampling resolution.  The
acquisition time of complete range scans covering a $180^o$ area
($500$ slices) is about $20$ minutes.
Figure~\ref{Fig_Exp1y2_RangeData} shows two examples of the
complete range scans acquired from the two environments (black
areas represent missing range values).
 \begin{figure}[h*]
\begin{center}
\subfigure[A range scan from our lab.]{\includegraphics[width=0.8\linewidth]{figures/fig_Exp_RangeScanLab.eps}}
\subfigure[A range scan from the CIM floor.]{\includegraphics[width=0.8\linewidth]{figures/fig_Exp_RangeScanHall.eps}}
\caption{Complete range scans from the environments to be modeled.}
\label{Fig_Exp1y2_RangeData}
\end{center}
\end{figure}  

\noindent {\em Acquiring Partial Range}. In practice, we will
acquire partial range data by sampling a small amount of range
data (approximately 30 to 50\% of total range) at each robot pose,
thus reducing significantly the acquisition time. However, in
order to be able to estimate the performance of our method for
estimating dense range maps, complete range scans were acquired
through all the experiments presented here.

\noindent {\em Acquiring the Panorama}. As the pan unit rotates,
at every $18$ degrees our CCD camera takes a picture. All the
images are then projected onto a cylindrical representation to
obtain a cylindrical panorama mosaic. Figure~\ref{FigCylMosaic}
presents two $180^o$ cylindrical panoramas (from the respective
environments) constructed using the technique described in
Section~\ref{Sec:CylPan}.

\begin{figure}[h*]
\begin{center}
\subfigure[From images taken in our lab]{\includegraphics[width=0.95\linewidth]{figures/fig_cylmos1.eps}}
\subfigure[From images taken in the hall]{\includegraphics[width=0.95\linewidth]{figures/fig_cylmosHall.eps}}
\caption{Two cylindrical panoramas.}
\label{FigCylMosaic}
\end{center}
\end{figure} 


\subsection{Camera-Laser Data Registration}

As described in Section~\ref{Sec:DataRegistration}, both the range and intensity data
must be in similar cylindrical representations. For the
arrangement used in these experiments, $f=300$~pixels, $\Delta
Y=5$~cm and the range of the points is $r =5-8$~meters, and
$\beta$ is between $6$ to $10$ pixel units.
Figure~\ref{FigRegRangeMosaic1} shows  samples of the registered
panorama mosaic (top) and range image (bottom).

It is important to note, that the registration was computed using
only the partial range data as an input. Since we are using a
mapping between intensity and range image coordinates, the quality
of the registration in the coarse registration step, does not
depend on the amount of range data given as an input. In the fine
registration step, i.e., the local alignment of 2D and 3D edges,
the features captured by the input range data are crucial.

\begin{figure}[]
\begin{center}
\subfigure[]{\includegraphics[width=0.8\linewidth]{figures/fig_registeredOneLab.eps}}
\subfigure[]{\includegraphics[width=0.8\linewidth]{figures/fig_registeredOneHall.eps}}
%\vspace{-0.25cm}
\caption{Samples of the registered intensity (top) and range data (bottom) collected (a) from our lab and (b) from the CIM floor.}
\label{FigRegRangeMosaic1}
\end{center}
\end{figure} 


\subsection{Range synthesis}

Figure~\ref{Fig_InputEM} shows an example of data collected in our
lab. (a) is the input intensity, (b) the intensity edges and (c)
the input partial range data, where 50\% of the total range is
unknown. The resulted synthesized range image is shown in (d), and
the ground truth range image in (e), for comparison purposes. The
MAR error for this example is $7.85$ centimeters.
\begin{figure}[]
\begin{center}
\subfigure[Input intensity data]{\includegraphics[width=0.49\linewidth]{figures/fig_EM_v1_I.eps}}
\subfigure[Intensity edges]{\includegraphics[width=0.49\linewidth]{figures/fig_EM_v1_e.eps}} \\
\subfigure[Input range (50\% of total range is uknown)]{\includegraphics[width=0.49\linewidth]{figures/fig_EM_v1_iR.eps}}
\subfigure[Synthesized range image]{\includegraphics[width=0.49\linewidth]{figures/fig_EM_v1_RSBP.eps}} \\
\subfigure[Ground truth range]{\includegraphics[width=0.49\linewidth]{figures/fig_EM_v1_GT.eps}}
\caption{Results on dense range map estimation. (a)-(c) Input data to our range synthesis algorithm. (b) The synthesized range image and (e) the ground truth range.}
\label{Fig_InputEM}
\end{center}
\end{figure} 

A second example, from the data acquired on the CIM floor, is
shown in Figure~\ref{Fig_Hall_Input} (a)-(c) is the input data:
the intensity image, intensity edges and the partial range,
respectively. The synthesized range image after running our
algorithm is shown in (d), and (e) shows the ground truth range
for comparison purposes. The MAR
error is $5.76$ centimeters. % and a plot of the histogram is shown in Figure~\ref{Hist_Exp_Hall}.
\begin{figure}[]
\begin{center}
\subfigure[Intensity data]{\includegraphics[width=0.49\linewidth]{figures/fig_Exp_Hall_v1_i.eps}}
\subfigure[Intensity edges]{\includegraphics[width=0.49\linewidth]{figures/fig_Exp_Hall_v1_e.eps}} \\
\subfigure[Input range (50\% of total range is unknown)]{\includegraphics[width=0.49\linewidth]{figures/fig_Exp_Hall_v1_k10-10.eps}}
\subfigure[Synthesized range image]{\includegraphics[width=0.49\linewidth]{figures/fig_Exp_Hall_v1_SRbp.eps}} \\
\subfigure[Ground truth range]{\includegraphics[width=0.49\linewidth]{figures/fig_Exp_Hall_v1_GT.eps}}
\caption{Results on dense range map estimation. (a)-(c) Input data to our range synthesis algorithm. (b) The synthesized range image and (e) the ground truth range.}
\label{Fig_Hall_Input}
\end{center}
\end{figure} (a)-(c) is the input data: the intensity image, intensity edges and the partial range, respectively.   The synthesized range image after running our algorithm is shown in (d), and (e) shows the ground truth range for comparison purposes. The MAR error is $5.76$ centimeters. % and a plot of the histogram is shown in Figure~\ref{Hist_Exp_Hall}.


\subsection{Integration of Multiple Panoramas with Depth}
\label{Sec:Exp_Integration}

In this stage, we integrate all the panoramas with depth taken at
different robot poses. We first show  results for the environment
that corresponds to our lab. The first step is to find the
corresponding 2D feature points among the intensity panoramas.
Figure~\ref{Fig_SIFT_Lab} shows the SIFT features and their
association from two panoramas.

\begin{figure}[h*]
\begin{center}
\includegraphics[width=0.76\linewidth]{figures/fig_SIFT_LabM.eps}
\caption{The SIFT features and their associations from two panoramic mosaics acquired in our lab.}
\label{Fig_SIFT_Lab}
\end{center}
\end{figure} 

The second step is to establish a set of 3D points from these 2D
feature points to define the surface feature elements, and apply
the registration steps described in Section~\ref{Sec:PairwiseReg}.
We set a window of $5 \times 5 $ pixels to the corresponding 3D
points in each view. It is important however, to mention at this
point, that using only the partial range maps to perform a global
alignment may not generate good results, especially for highly
unstructured environments and the lack of distinctive features,
like the one we are dealing with here. The reason for this is that
there may not be enough surface feature elements to perform the
alignment if the 2D feature points correspond to areas with
missing range. To demonstrate this, we show in \begin{figure}[t]
\begin{center}
\subfigure[Partial range data]{\includegraphics[width=0.6\linewidth]{figures/topviews_partial.eps}}
\subfigure[Synthesized (dense) range data]{\includegraphics[width=0.6\linewidth]{figures/topviews_dense.eps}} \\
\caption{Top views of the two 3D range scans to be registered. (a) The input (partial) range scans and (b) the synthesized (dense) range scans.}
\label{Fig_TopViews}
\end{center}
\end{figure} 
Figure~\ref{Fig_Alignment}, the alignment of the two range scans
(displayed in Figure~\ref{Fig_TopViews}) using only the incomplete
range scans (a) and when using the synthesized dense range scans
(b). 
\begin{figure}[h*]
\begin{center}
\subfigure[]{\includegraphics[width=0.34\linewidth]{figures/topview_int_partial.eps}}
\hspace{0.2cm}
\subfigure[]{\includegraphics[width=0.38\linewidth]{figures/topview_int_dense_g.eps}}
\caption{Top views of the alignments of the range scans of Fig.~\ref{Fig_TopViews},  (a) using only the partial range data and (b) using the synthesized dense range.}
\label{Fig_Alignment}
\end{center}
\end{figure} 

From the images, we observe that the alignment using only partial
range data is not accurate. The total length obtained is
approximately 1 meter bigger than the actual size of the room,
with an averaged missalignment of individual readings of about 12
centimeters. On the other hand, when the synthesized dense range
data is used together with the 2D feature points there is an error
of 12 centimeters in the actual length and an average
missalignment of 7 centimeters.

We now show results for the environment corresponding to the data
acquired in the corridors of CIM. As in the previous environment,
we first find the SIFT features. We show the SIFT features and
their associations for two pairs of panoramas in
Figure~\ref{Fig_SIFT_CIM}.
\begin{figure}[]
\begin{center}
\subfigure[]{\includegraphics[width=0.9\linewidth]{figures/out_16-1_1-2M.eps}} \\
\subfigure[]{\includegraphics[width=0.95\linewidth]{figures/out_p3_16-2M.eps}}
\caption{The SIFT features and their associations from two pairs of panoramic mosaics acquired in the CIM floor.}
\label{Fig_SIFT_CIM}
\end{center}
\end{figure} 

After defining the surface feature elements, we register each of
the synthesized range scans. In this experiment, we set a window
of $9 \times 9$ pixels to the corresponding 3D points in each
view. Figure~\ref{Fig_TopViewsCIM} shows four synthesized dense
range scans (some of them also show parts of the ceiling).

\begin{figure}[]
\begin{center}
\subfigure[]{\includegraphics[width=0.35\linewidth]{figures/reg_topview_p1.eps}}
\hspace{0.25cm}
\subfigure[]{\includegraphics[width=0.37\linewidth]{figures/reg_topview_p161.eps}} \\
\subfigure[]{\includegraphics[width=0.41\linewidth]{figures/reg_topview_p3.eps}}
\subfigure[]{\includegraphics[width=0.41\linewidth]{figures/reg_topview_p164.eps}} \\
\caption{Top views of the four synthesized (dense) range scans taken in the CIM floor.}
\label{Fig_TopViewsCIM}
\end{center}
\end{figure} 

We then perform the global alignment of all the synthesized range
scans incrementally, i.e., every time we integrate two range
scans, we run our range synthesis method to fill in the missing
range data, and continue with the next range scan. We also update
real readings when available and discard the synthesized ones if
they differ in more than 3 centimeters. A top view of the final
aligment is displayed in Figure~\ref{Fig_Alignment_CIM}.

\begin{figure}[t*]
\begin{center}
\includegraphics[width=0.56\linewidth]{figures/regist_hall.eps}
\caption{Top view of the global alignment of the synthesized range scans.}
\label{Fig_Alignment_CIM}
\end{center}
\end{figure} 


As we can see, the use of intensity information dramatically
improves the integration of range scans. In general, the accuracy
of the results depends on having a good intensity feature
detector.

\section{Discussion and Future Work}
In this paper we have presented a complete framework for the
mobile robot environment modeling problem using incomplete
sensor data. The ability to reconstruct a 3D model of an object or scene
greatly depends on the type, quality and amount of information
available. The data acquisition framework described was designed 
to speed up the acquisition of range data by
obtaining a relatively small amount of range information from the
scene to be modeled. By doing so, we compromise the accuracy of
our final representation. However, since we are dealing with
man-made environments, we can take into account the coherence of
surfaces and their causal inter-relationships with the photometric
information, to estimate complete range maps from the partial
range data.  The experimental results shown in this paper for
two different indoor scenes demonstrate the feasibility of our
method.

While our results are highly satisfying, there remain several
interesting issues to be resolved. Foremost among these is the
need for a formal characterization of performance for such an
approach.

Another issue, is that related to the spatial sampling of range
data of the environment, which at the moment is done by using a
simple strategy depending on the distance to objects in the
scene. For more complicated environments, it may be desirable
to apply an active sensing technique so that a more dense sampling
is done in sites having more objects with different depth profiles
and a sparse sampling in sites with none (e.g. walls) or few objects.
In ongoing work, we are exploring this issue more fully. Interesting
features from images taken at different views should be selected
across different scales. This selection may be done according to
the geometric structure, texture and shading characteristics observed
in the images. In the same vain, the exploration of the environment 
by the robot will depend on the selection of interesting locations 
visually available from its previous position.


\section*{Acknowledgments}
This work was funded by the National Council for Science and Technology
of Mexico (CONACyT)  and Natural Sciences and Engineering Research 
Council of Canada.


% The Appendices part is started with the command \appendix;
% appendix sections are then done as normal sections
% \appendix


\bibstyle{apalike} %plain}
\bibliography{abril}


% Bibliographic references with the natbib package:
% Parenthetical: \citep{Bai92} produces (Bailyn 1992).
% Textual: \citet{Bai95} produces Bailyn et al. (1995).
% An affix and part of a reference:
%   \citep[e.g.][Ch. 2]{Bar76}
%   produces (e.g. Barnes et al. 1976, Ch. 2).

%\begin{thebibliography}{}
% \bibitem[Names(Year)]{label} or \bibitem[Names(Year)Long names]{label}.
% (\harvarditem{Name}{Year}{label} is also supported.)
% Text of bibliographic item
%\bibitem[]{}
%\end{thebibliography}

\end{document}


\newpage
Abstract\\

This article presents a complete framework for building a 3D model of a large-scale
indoor environment by fusing incomplete sensor data. The incomplete
sensor data is obtained by a mobile robot navigating the environment.
The geometry of large-scale environments is usually extracted by acquiring
a huge amount of range data and registering it.  For many real systems, this data 
acquisition process is  time and energy consuming and also physically demanding.
The framework presented here overcomes this problem by allowing a robot to rapidly 
collect a set of intensity images and a very small amount of range information. 
The data registration of the intensity and range data is achieved by using a image-based 
registration algorithm and then the
missing geometric structures are inferred from the intensity images using a 
statistical learning method. This method basically integrates and analyzes the statistical 
relationships between the visual data and the available depth on terms of small patches. 
Finally, the integration of multiple panoramas with depth at different robot poses is
achieved by using a hybrid feature matching approach from the literature. Experiments on 
real-world data are conducted under different configurations to demonstrate the 
feasibility of the method. 


