Landmark Mapping from Unbiased Observations Jason S. Ku, Stephen Ho, and Sanjay Sarma Department of Mechanical Engineering Massachusetts Institute of Technology Cambridge, Massachusetts 02139 Email: jasonku@mit.edu, ssh1@mit.edu, sesarma@mit.edu Abstract—The starting point of any Smart City approach is knowing what is in the city and the location of city assets. We propose a general, automated approach to inventorying and monitoring outdoor city infrastructure using common sensors: namely a GPS, IMU, and camera. The presented mapping algorithm operates in the mobile sensing paradigm, using obser- vations from a moving vehicle to construct a map of landmark location estimates whose uncertainty decreases linearly with the number of observations, robust to both translational and angular error to first order. The algorithm is adaptable to many applications given an appropriate image classifier. We apply our algorithm to automatically locate and inventory city streetlights and demonstrate its performance using both numerical simulation and field experiments. I. I NTRODUCTION Simultaneous localization and mapping, or SLAM refers to machines mapping their environment and positioning them- selves accurately within that environment [1] [2] [3]. SLAM has been studied extensively in robotics and motion planning, typically implimented using range finders [4] and cameras combined with modeling of vehicle kinematics to inform an estimate. This paper ignores observer localization in favor of a limited but powerful model of landmark mapping with two main features: an environment containing a finite set of fixed landmarks; and an observer equipped with noisy estimates of its own position, and estimates of the direction in which landmarks are observed. The motivation for the above formulation is twofold. First, we limit the environment to a sparse set of landmarks to simplify the typical computational complexity of trying to map an entire environment. Second, the modern ubiquity of smartphones provides a convenient platform to collect data consistent with this model: small computers equipped with a camera, IMU, and GPS. The IMU and GPS can give (possibly highly inaccurate) estimates of position and orientation, and with a sufficient image classifier for some class of landmarks, the camera may provide an estimate of the direction of land- marks in front of the camera. Using unbiased estimators like an IMU with magnetometer (compass) and GPS greatly decreases our reliance on sensitive odometry to estimate position that is prone to drifting over time. Much of the existing work using a single camera without depth information for landmark mapping (called MonoSLAM) relies on continuous motion of a camera for feature mapping [5], or addresses the combinatorial problem of mapping and location given perfect observation measurements [6] [7]. Our estimation approach is robust to noise even for observations given in random order. Existing work in triangulation at- tempts to reconstruct the location of objects given only a few measurements [8] [9], where as our approach exploits multiple measurements to decrease the covariance matrix of our estimation as the number of observations increases. While addressing this general formulation, we keep in mind the specific application of automatically locating and inventorying landmarks in cities. The starting point of any Smart City approach is knowing what is in a city, as well as the location of city assets. The observation system described in this paper could easily be mounted on existing municipal fleet vehicles and gather data as the vehicle moves about the city. The proposed estimation algorithm could be used to locate and inventory many types of landmarks, given an appropriate image classifier. In this paper, we demonstrate the proposed framework by mapping the location of streetlights. This information is useful to cities to inventory and maintain their infrastructure. Streetlights comprise a significant part of most municipal budgets, and many municipalities still rely on self reporting to maintain them. If a city knows where its infrastructure is, the process of monitoring for maintenance and repair can be automated saving time and money. Section II develops the theoretical framework for the estimation algorithm. Section III discusses the numerical simulation used to verify the theory. Section IV describes experimental setup used to conduct field experiments, with Section V analyzing the collected data. Final thoughts are concluded in Section VI. II. THEORY We would like to identify the location of certain landmarks in space. For now, we restrict our analysis to locating a single landmark. For practical applications, we will mostly be interested in landmarks in the plane or in three dimensions, but the theory developed extends naturally to higher dimensions. A landmark exists at an unknown position p in R d . Unless otherwise stated, we assume vectors are column vectors so that p · p = p T p. Observations of the landmark are taken at different positions x i ∈ R d for i ∈{1,...,n}, observing the landmark in direction u i =(p − x)/‖p − x‖ where ‖·‖ denotes the Euclidean norm. Observations are of the form (˜ x i , ˜ u i ) where ˜ x i and ˜ u are measurement estimates, functions only of observation position x i and direction u i respectively. We will use X to denote collectively the set of position observations, and similarly U for the set of direction observations. A. Perfect Information First assume perfect data, i.e. that ˜ x i = x i and ˜ u i = u i . Then certainly any two observations (x 1 ,u 1 ) and (x 2 ,u 2 ) that