Real-time SLAM with Piecewise-planar Surface Models and Sparse 3D Point Clouds

Transcription

1 Real-tme SLAM wth Pecewse-planar Surface Models and Sparse 3D Pont Clouds Paul Ozog and Ryan M. Eustce Abstract Ths paper reports on the use of planar patches as features n a real-tme smultaneous localzaton and mappng (SLAM) system to model smooth surfaces as pecewse-planar. Ths approach wors well for usng observed pont clouds to correct odometry error, even when the pont cloud s sparse. Such sparse pont clouds are easly derved by Doppler velocty log sensors for underwater navgaton. Each planar patch contaned n ths pont cloud can be constraned n a factorgraph-based approach to SLAM so that neghborng patches are suffcently coplanar so as to constran the robot trajectory, but not so much so that the curvature of the surface s lost n the representaton. To valdate our approach, we smulated a vrtual 6-degree of freedom robot performng a spral-le survey of a sphere, and provde real-world expermental results for an autonomous underwater vehcle used for automated shp hull nspecton. We demonstrate that usng the sparse 3D pont cloud greatly mproves the self-consstency of the map. Furthermore, the use of our pecewse-planar framewor provdes an addtonal constrant to mult-sesson underwater SLAM, mprovng performance over monocular camera measurements alone. I. INTRODUCTION The classc approach to the smultaneous localzaton and mappng (SLAM) problem uses landmar measurements, ether bearng-only or coupled wth range, to estmate both the locatons of envronment features and the trajectory of a moble robot. These features are often ncluded wth the vehcle pose n a state vector that s estmated usng an extended Kalman flter (EKF) [1], partcle flter [], or nonlnear least-squares solver [3] [5]. As robot sensors become more data-rch, le hghresoluton lght detecton and rangng (LIDAR) scanners or depth cameras, ncludng every pont observed by these sensors nto the state vector can qucly become burdensome. Instead, researchers have used feature-cloud matchng so that landmar detectons at two robot poses are used to compute a sngle relatve transformaton between the two poses. For D or 3D scanners, algorthms le teratve closest pont (ICP) [6] or correlatve scan matchng [7] have been shown to wor well for many wdespread applcatons. Feature-cloud matchng also preserves the sparsty of the nformaton matrx n graph-based vsual SLAM [8], whch has nspred a number of successful wors for cameraequpped autonomous underwater robots [9] [14]. Ths wor was supported by the Offce of Naval Research under award N P. Ozog s wth the Department of Electrcal Engneerng & Computer Scence, Unversty of Mchgan, Ann Arbor, MI 4819, USA paulozog@umch.edu R. Eustce s wth the Department of Naval Archtecture & Marne Engneerng, Unversty of Mchgan, Ann Arbor, MI 4819, USA eustce@umch.edu (b) HAUV (a) SS Curtss (c) Pecewse-planar SLAM Fg. 1. Treatng large shp hulls as a collecton of pecewse-planar surface features augments our vsual SLAM system to produce more self-consstent maps, even wth a very sparse 3D DVL pont cloud (twenty 3D ponts per second). In (a), a 16 m vessel, the SS Curtss, s surveyed by the HAUV [1] shown n (b), whch s equpped wth an actuated DVL sensor. A reconstructed underwater vew of the HAUV s trajectory (sold lne) and the shp hull (gray surface) s shown n (c). The Doppler velocty log (DVL) sensor s common for precse navgaton n underwater vehcles, and t can provde an accurate, though very sparse, 3D pont cloud. For underwater robots, actve range scanners le the DVL are smply not as data-rch as other sensors, le multbeam sonar or LIDAR scanners. For comparson, a RD Instruments Worhorse DVL [15] senses about twenty 3D ponts per second, whereas a hgh-end Velodyne HDL-64E scanner for ground vehcles [16] returns over a mllon ponts per second. Due to the sparsty of these pont clouds, the DVL has smply not been able to tae advantage of the aforementoned feature-cloud matchng that s so wdespread n modern moble robotcs. In ths wor, we leverage the sparse DVL pont cloud by fttng local planar patches to a movng tme wndow of ponts, and show how ths can sgnfcantly constran the trajectory of a Hoverng Autonomous Underwater Vehcle (HAUV) performng automated shp hull nspecton, as shown n Fg. 1. A. Related Wor Despte the nablty of underwater robots to perform feature-cloud-based SLAM usng a DVL, there are stll a wde varety of vable technques for underwater SLAM. Barby et al. [17] perform SLAM on the seabed usng a

2 partcle flter that weghts partcles based on sonar beam measurement agreement wth prevous observatons. By fttng a Gaussan process to the past sonar beam observatons, they are able to detect loop closures even wth lttle overlap between the current sonar swath and past swaths. Cameras have also proven to be a vable sensor for performng SLAM n underwater envronments. In [13], Km and Eustce use a monocular camera on a HAUV to effcently bound odometry error by only consderng vsually salent eyframes. Ths wors to great effect for good underwater vsblty condtons and suffcently feature-rch shp hulls, however these condtons are not always avalable. In that wor, the DVL was used only for measurng veloctyderved odometry the resultng 3D pont cloud was not used. Ths wor wll show that ncorporatng ths nformaton nto a SLAM bacend s very useful for correctng odometry error, partcularly when good magery s not avalable. Our algorthm reles on the use of planes as features for performng SLAM, rather than the standard Cartesan pontfeatures or feature-clouds. Planar features have been used n real-tme SLAM systems for a number of years [18] []. In [18], Wengarten and Segwart use a vehcle equpped wth multple SICK LIDAR scanners to detect planar feature patches n an offce buldng usng an EKF framewor. They use a rotatng SICK scanner to acqure a dense 3D scan, from whch planar patches are detected and fused. These fused planes are used as a sngle feature measurement, eepng the number of features small n the resultng maps. Trevor et al. [19] mplemented planar features as factors n a factorgraph-based SLAM framewor. Effectvely, these efforts have re-ntroduced feature-based maps for LIDAR-equpped robots rather than usng feature-cloud scan-matchng for laser odometry. In both of these wors, a robot traverses an offce buldng-le envronment, and observes planar walls and tables wth hgh-resoluton 3D and D scanners. In unstructured envronments usng low-resoluton scanners, t s unclear f these approaches would wor. In [1], Ruhne et al. propose a system that avods treatng feature-clouds as rgd bodes and nstead performs offlne optmzaton of both the pose and envronment surfaces n a jont optmzaton framewor. Besdes a laser range scanner, ther sensor sute ncludes a RGB-D camera and they are able to construct accurate, hgh-resoluton models. Smlar to [1], we use planar patches to approxmate a curved surface n a pecewse-planar fashon, however our approach s desgned for real-tme applcatons on much sparser 3D pont clouds. B. Outlne In II, we cover the theory behnd the use of planar features n our SLAM system. We frame the problem usng factor-graphs, whch can be ntegrated nto the ncremental smoothng and mappng (SAM) framewor [4], and we detal the weght matrces used n the sparse optmzaton. In III, we present a smulaton framewor n whch a robot n 3D surveys a sphercal object, showng that our approach s capable of successfully modelng surfaces that are curved everywhere. In IV, we apply ths framewor to a real-world autonomous shp hull nspecton experment, and present an mprovement n the map consstency. Moreover, we provde ntal results n a mult-sesson SLAM system n whch the relablty s greatly mproved by the addton of anchor node support [] n our planar feature framewor. Fnally, n V, we offer some concludng remars and address areas for mprovement and future wor. II. APPROACH A. Parameterzaton of Planes n 6-DOF SLAM Let x = [x, y, z, φ, θ, ψ ] be the 6-degree of freedom (DOF) relatve-pose of the vehcle n the global frame at tme, where x, y, z are the Cartesan translaton components, and φ, θ, and ψ denote the roll (x-axs), ptch (y-axs), and yaw (z-axs) Euler angles, respectvely. Let π = [a, e, d ] be the plane ndexed by, expressed n the frame of x. Ths plane s represented by the azmuth and elevaton of the drecton of the surface normal, and the orthogonal dstance, or standoff, of the vehcle to the plane (.e., a, e, and d, respectvely). For convenence, let n be the Cartesancoordnate unt-norm vector correspondng to the azmuth and elevaton of the surface normal of π. Ths normal vector s computed as n = n x n y n z = h ([ a e ]) cos(e ) cos(a ) = cos(e ) sn(a ). (1) sn(e ) The nverse of (1) s gven by [ ] a n [ e = h 1 x atan(n n y, n x ) y = ( n atan n z, z ) n x + n y () Parameterzng a plane usng a 3-DOF vector, π, avods problems assocated wth over-parameterzed representatons of varable nodes n SAM. The plane, π, s determned as the least-squares ft of a sldng-wndow of N staced 3D ponts n pose s frame, p R3N. By reshapng p nto a 3 N data matrx and applyng prncpal component analyss (PCA), the unt-norm can be determned, up to sgn, as the left sngular vector correspondng to the smallest sngular value [3]. The standoff, d, s computed by dottng the unt normal wth the centrod of p. Fnally, the azmuth and elevaton are computed drectly from (). The process s llustrated n Fg. (a). We model the plane s uncertanty as a frst-order approxmaton, descrbed n II-D. B. Constranng Planes and Poses wth Factors The wdely-used factor-graph representaton of the SLAM problem uses varable nodes and factors n a bpartte graph representaton [3]. We ntroduce a planar node, parameterzed by the three values descrbed n II-A: azmuth, elevaton and dstance. To ntalze a planar node, we ft a least-squares plane as descrbed n II-A. Ths gves us a measurement, z π, of the plane, ndexed by, n the frame of a pose ndexed by. ].

3 z Nose corrupted Grouth Truth 3 y (a) Least-squares plane ft x 4 Elevaton Monte Carlo Frst Order Azmuth (b) Frst-order covarance (a) Separate 6-DOF pose and 3-DOF plane nodes Fg.. Smple example of smulated least-squares ft of 3D ponts to a plane. In (a), we ft a plane to the sparse DVL pont cloud by tang the prncpal component that captures the least varaton n the data. Ths leastsquares ft s shown by the blue arrow. In (b), the propagaton of the pont cloud covarance, Σ p, s well-approxmated by numercally lnearzng the least-squares ft descrbed n II-A, so long as the pont cloud uncertanty s small. Then, a unary factor s added to the ntalzed plane node wth the followng potental: Ψ(π ; z π, Σ π ) = z π π Σ π, (3) where Σ π s the measurement covarance matrx, to be dscussed n II-D.1. Once two plane nodes are observed ether from the same pose or two dfferent poses and deemed suffcently coplanar, a quaternary factor s added that constrans the two planes and the two poses from whch they were observed. The potental for ths factor, computed as n [19] by the dfference between the predcted plane expressed n pose s frame and the plane l expressed n pose j s frame, s gven by: Ω(x, π, x j, π j l ; Σ π l ) = π j πj l Σ πl, (4) where Σ πl s a weght matrx, to be dscussed n II-D.. The plane predcton, π j, s computed as follows: n j = Rj n d j = t j n + d, where R j s the rotaton matrx that rotates vectors n s frame to vectors n j s frame, and t j s the translaton from to j expressed n s frame. It s mportant to note that f ths factor s left out of the graph, planar nodes have no mpact on the optmzaton of the robot poses and j. A smple example of the graph layout for our approach s shown n Fg. 3(a). A detaled descrpton of the weght matrces used n (3) and (4) s provded n II-D. C. 9-DOF Vehcle State Nodes For the smulaton and expermental results n III and IV, the planes are ft from a very sparse 3D scanner so we assume for convenence that there s only one observed planar patch per robot pose. We therefore combne the vehcle pose and planar patch nto a sngle node n our factorgraph usng the 9-DOF vehcle state nodes of the form [ ] x x =. π (b) Combned 9-DOF state pose and plane nodes Fg. 3. Small example of factor-graphs usng our pecewse-planar approach (best vewed n color), wth blac representng unary pror factors and bnary odometry factors. In (a), planes π1 1, π1, and π 3 are represented as ther own nodes n the factor-graph, along wth pose nodes x 1 and x. In ths example, pose 1 observes planes 1 and, whle pose observes plane 3 (blue factors). The addton of the quaternary yellow factor constrans the trajectory so that planes and 3 are suffcently coplanar. In (b), we convenently assume that each pose observes one plane, so we lump the 6- DOF pose node together wth the 3-DOF plane node nto a 9-DOF vehcle state node. In ths case, we can use bnary factors (yellow) to constran the robot trajectory. The planar components of ths vehcle state node are ntalzed n the same manner as (3). Then, the 9-DOF co-planar factor potental, Ω, s computed by evaluatng (4) wth = and l = j: Ω (x, x j; Σ πj ) = Ω(x, π, x j, π j j ; Σ π j ) (5) D. Weght matrces 1) Least-Squares Plane Ft: The weght matrx Σ π used n (3) can be nterpreted as the covarance of a leastsquares ftted plane. To compute ths covarance matrx, we assume the sldng wndow of 3D ponts for the th plane as expressed n pose s frame s corrupted by Gaussan nose. The propagaton of ths uncertanty s modeled by lnearzng the functon f( ) that fts π from p : π = f(p ) where p N (µ p, Σ p ). Ths functon s the PCA-based method descrbed n II-A, whch s numercally dfferentable when care s taen to guarantee that the unt normal consstently faces toward the robot, snce typcal sngular value decomposton (SVD) lbrares may return sngular vectors wth flpped sgns when the nput matrx s slghtly perturbed. Then, upon ntalzaton, the plane s covarance s gven by Σ π FΣ p F, (6) where F s the Jacoban of f( ) evaluated at the observed value of p. If Σ p s small, ths frst-order approxmaton performs well as shown n Fg. (b). For our expermental underwater robot, the uncertanty of ths pont cloud was estmated to be. m by computng the sample varance when the sensor s ponted towards a nown surface (for

4 nstance, the very bottom of a shp hull where the depth error s small). ) Co-planar Factor Potental : The norm π πj l based on the potental from (4) wll be small f the two planes and l are perfectly coplanar. Of course, these planes may not be perfectly coplanar for two reasons: () propagaton of sensor nose dscussed n the prevous secton, and () the surface from whch the two planes are measured s not exactly planar. Both of these, f not accounted for, wll cause the bacend optmzer to over-compensate for the factor s potental. For the remander of ths secton, we use 9-DOF representaton of poses and planes to eep the notaton smpler;.e., = and l = j. To account for nonplanar surfaces, we defne an addtonal weght matrx to account for the surface curvature. Ths weght matrx s modeled by user-provded characterstc rad for both the azmuth and elevaton dmensons. We compute the expected dfference n azmuth, elevaton, and dstance gven the crcular curvature model (whch consttutes a dagonal set of weghts), and a forward-propagaton of the covarance of the 9-DOF nodes and j as follows: W πj = dag( a, e, d ) + C [ ] Σ Σ j C, Σ j Σ jj (7) where C s the Jacoban of the functon c( ) that s a smple curvature model of the surface beng observed. It predcts the dfference of the normal drecton and standoff between two poses: a e = c(x, x j; r a, r e ), d [ ] [ ] ( a a where = h 1 e e ( ([ a d = h + t j y /r a e + t j z /r e R j h ([ a + t j y /r a e + t j z /r e ]) h ([ a e ]) ), ]) ) t j. Here, r a and r e are the user-defned characterstc rad n the azmuth and elevaton axes, respectfully, t j s the translaton vector and h( ) s defned n (1). Fg. 4 llustrates ths smple curvature model n D. The matrx used n the factor potental from (4) s the sum of the covarance of the j th measured plane from (6) and the weght matrx from (7): Σ πj = Σ π j + W πj. j Importantly, ths matrx should be nterpreted as a weght matrx, and not as a covarance matrx. The am of ths weght matrx s to avod over-penalzaton of errors from (4) that are due to surface curvature, tang nto account the uncertanty of the pose and planar measurements. Ths determnaton of weghts clearly depends on the state of the vehcle and the uncertanty of the relevant robot poses at the tme the factor s added to the graph. Thus, the weght matrx tends to provde conservatve posteror covarances. Furthermore, the nonlnear least-squares problem we are Fg. 4. An overvew of how the weght matrx used n (4) s computed between two poses and two planes. Treatng the translaton from to j as an approxmate arc-length, the change n azmuth, a, between the two poses can be computed gven the characterstc radus, r a. The uncertanty of the poses, shown by the blue ellpses, also plays a role nto the computaton of a, so ths s also ncluded n the weght matrx. provdng to SAM s clearly not the maxmum lelhood estmate (MLE) of the robot trajectory and surface, because the measurement covarance matrx depends on the state, as formulated. However, so long as the trajectory does not drastcally change durng the optmzaton, we acheve good results. In partcular, we have found that assumng depth, ptch, and roll uncertanty as bounded mproves ths approach, even wth large translatonal and yaw uncertanty. Ths wll be shown n our smulaton envronment n III. E. Data Assocaton Unle the wor from [18] and [19], we focus on experments where the dfferences n plane postons and surface normals vary gradually on a smooth surface, rather than surface normals whch belong to, say, walls n a buldng. To solve the data assocaton problem, [18] checs the Mahalanobs dstance of the canddate plane observed from the current pose to a plane that s already ncluded n the EKF state vector. If t s below a threshold, the current planar measurement s assocated wth the fltered planar feature. Fndng the best assocaton of planar patches on a smooth surface s sgnfcantly more challengng because () there s a large number of possble node pars from whch to add coplanar factors, and () most neghborng planar features are all consstent n terms of Mahalanobs dstance. Even so, we use the same basc method as [18] when t comes to addng a planar factor from (4) to the graph: for each nearest neghborng planar patch to the current pose, we chec that the χ -error from (4) s below a threshold correspondng to a confdence regon of 99%. We have found that ths smple method performs well even when the odometry s qute nosy as n III. III. SIMULATED TRIALS To evaluate the proposed technque for modelng curved surfaces, we smulated a robot wth a sparse 3D scanner surveyng a sphere wth radus r = 8 m at a standoff of 1 m. Clearly, ths experment motvates the use of a pecewseplanar model because the surface beng observed s curved everywhere. {

5 15 DR Poston SLAM Poston DR Orentaton SLAM Orentaton..15 tr(σxyz) 1.1 tr(σφθψ) 5.5 (a) SLAM vs. DR (b) SLAM vs. ground-truth Tme (c) Covarance over tme Fg. 5. Results of the SLAM smulaton usng a sphercal surface and a 3D robot. In (a), the left plot shows a vsualzaton of the coplanar factors from (4) as yellow lnes. The cyan regon denotes the 99% postonal confdence regon. Ths confdence regon grows at the begnnng and end of the smulaton, when the surface normals are not capable of correctng odometry error. Ths explans why the center of the reconstructed sphere n (b) does not algn wth ground-truth. In (c), the postonal and orentatonal uncertantes are shown for each pose. For the frst and last 1 poses, the surface normals are approxmately algned wth gravty, and ths explans why uncertanty grows at the start and end of the smulaton, but s bounded durng the nterm. In ths smulaton, the robot traverses the surface of the sphere n a spral-le pattern n such a way that the robot never observes the sphere from the same pose more than once. We corrupt the ground-truth to provde odometry measurements, but have the z, ptch, and roll measurement uncertantes bounded (as would be the case from a pressure depth sensor and gravty-derved roll/ptch measurements n our applcaton). The robot observes one planar patch for each pose, so we use the 9-DOF graph representaton that s descrbed n II-C. The results of ths experment are shown n Fg. 5. Usng our approach, the robot s able to reasonably reconstruct an estmate of ts trajectory and surface of the sphere. Interestngly, the robot accumulates most of the pose uncertanty at the top and bottom of the sphere, as shown n Fg. 5(c). At these portons of the sphere, the surface normal measurements are unable to correct for lateral and headng (x, y, yaw) error because the surface normals algn wth gravty, whch bounds the z, ptch, and roll uncertantes. However, once the robot maps a sphere, the robot s well-localzed relatve to the sphere, and uncertanty n a global sense s bounded. Ths suggests the best approach for robot mappng would be to mnmze tme spent accumulatng error n regons where the surface normals are not provdng any way to correct odometry error. Because ntal odometry error n the smulaton cannot be corrected wth planar measurements, we avod evaluatng one-to-one error between ground-truth poses and SLAM poses. For ths smulaton, we nstead evaluate the closeness of the SLAM map to the true sphere by fttng a least-squares sphere for the estmated robot trajectory. We compute the best-ft sphere center c and radus, ρ, by solvng argmn r,c N ( P c ρ). (8) =1 For ths smulaton, P R 3 N conssts of the postons for each of the N estmated poses. We evaluate ths cost functon usng the true radus, whch ndcates the closeness of the y (m) Ground-truth x (m) Fg. 6. The results of our smulaton suggest that our approach produces good SLAM estmates even f the characterstc curvature s not the groundtruth value. We chose a varety of rad for our smulaton, and we shaded the correspondng crcular regons above accordng to how well the groundtruth sphere from Fg. 5(b) fts the estmated trajectory. All user-defned rad between 3 and 19 m produce good SLAM estmates compared to the true value, 8 m, whch s mared wth trangles. postons to ground-truth. These values are shown n Fg. 6, where we also analyze the senstvty of our approach to the characterstc radus of the sphere. IV. EXPERIMENTAL TRIALS A. Improvements to Vsual SLAM The HAUV, shown n Fg. 1, contnuously observes the underwater surface of a shp hull. The vehcle s DVL sensor s constantly ponted nadr to the hull so as to mantan a constant standoff and orentaton. Ths provdes velocty-derved hull-relatve dead-reconng, however n ths experment we also apply our pecewse-planar model to the sparse 3D pont cloud produced by the sensor. The planar factors, as descrbed n II-B wor concurrently wth 5-DOF bearng-only camera measurements taen from the vehcle s monocular camera. These camera-derved measurements consttute the crux of the vsual SLAM system used on the HAUV [1], [13]. The characterstc azmuth and elevaton rad were userdetermned from the physcal lengths of the shp. We estmate these values by fttng least-squares crcles to small sde-tosde and top-to-bottom portons of DVL pont clouds. For the Resdual of least-squares ft

6 (a) Planar factors only (b) Camera factors only (c) Planar and camera factors Fg. 7. The results of the HAUV dataset on a 5 m 15 m 1 m secton of the hull are shown n (a), (b), and (c). Each plot shows the qualtatve effects of the dfferent factor types avalable to our SLAM system. Yellow lnes denote the factors that enforce the pecewse-planar constrant from (4), and red lnes represent monocular camera measurements. The pecewse-planar constrant provdes notceable translatonal correcton along the x-axs and rotatonal correcton about the z-axs. Furthermore, the use of pecewse-planar constrants offers far more vsual loop closures, to be shown n Fg. 8. det(σ) 1/6 (m rad) 1.4 x Camera factors Plane and camera factors Plane factors Dead reconng. Fg. 8. The use of planar factors produces a more geometrcallyconsstent map, because we successfully capture more nformatve camera measurements. Ths hstogram shows the tme dfference between nodes that are constraned by camera measurements. Larger tmes ndcate larger loop closures. Usng our pecewse-planar approach, we can add more loop closures to the graph because the reconstructon s more self-consstent. On the other hand, by dsablng the planar factors the model becomes nconsstent and we can only mae successful camera measurements between nodes that are closely separated n tme Tme Fg. 9. Margnal covarance over tme for the experment shown n Fg. 7. The green, blue, and red curves denote the uncertantes of Fg. 7(a), Fg. 7(b), and Fg. 7(c), respectvely. These uncertantes are computed based on the method from [4], whch taes the sxth root of the 6-DOF pose covarance, and has unts m rad. The addton of planar factors to the vsual SLAM graph (yellow lnes from Fg. 7(c)) lowers the uncertanty of the estmate substantally. shp used n ths experment, the azmuth (sde-to-sde) and elevaton (top-to-bottom) characterstc rad were 3 m and 7 m, respectvely. In practce, we found that we can double or half these parameters before notcng any sgnfcant change n the performance, confrmng that our approach s not partcularly senstve to the characterstc rad. As shown n Fg. 7, ncorporatng our method nto the SLAM system produces more consstent maps. We can quanttatvely measure consstency smply by countng the number of large vsual loop closures, as shown n Fg. 8. More precsely, we count the number of camera measurements between two nodes that are separated n tme by more than 3 s. Usng our planar factors, we successfully add 41 such loop closures to the graph. Wthout usng our approach, we only add. Ths suggests that our approach produces more geometrcally consstent maps, whch can be understood by consderng that the ntal guess of the twovew camera measurements are derved usng the current estmate of the vehcle poses. So, an nconsstent map leads to nconsstent ntalzaton, whch n turn leads to unsuccessful camera measurements. Moreover, the margnal pose covarances are also sgnfcantly mproved usng our planar factors. Ths s shown n Fg. 9, where covarances from Fg. 7(b) and Fg. 7(c) are plotted over tme. Fnally, as a santy chec, we can defne a roughness measure for the hull surface reconstructon by computng PCA over small neghborhoods of the DVL pont cloud. The roughness measure around a pont s defned as the smallest sngular value of a data matrx consstng of the nearest neghbors. Smaller values denote smoother surfaces. These values are overlad onto a meshed pont cloud n Fg. 1 for a vsual comparson. B. Mult-Sesson SLAM The use of planar and camera factors extends naturally to the mult-sesson SLAM problem wth the support of anchor nodes []. In short, we can co-regster two sessons, A and B, wth the addton of two nodes to the factor-graph, each representng an unnown 6-DOF transformaton. These unnown transformatons smply relate the global frame to A s frame, and the global frame to B s frame. We then

7 z (m) x (m) 1 5 y (m) Rougness (m) z (m) x (m) y (m) Rougness (m) (a) Wth planes (b) Wthout planes Fg. 1. Roughness measures of the DVL pont cloud after SLAM optmzaton, wth smaller values encodng smoother surfaces (best vewed n color). (a) The addton of our planar factors to the graph maes the DVL pont cloud roughness measures very small. (b) Dsablng our planar factors, the roughness measure ncreases drastcally because the planar patches are not beng explctly optmzed n the SLAM bacend. ncorporate the anchor node to the planar factor from (4) by applyng a root-shft to each pose before evaluatng the potental. For the HAUV, each sesson s separated n tme and the robot s tased wth algnng the current survey to a prevously-completed survey. Anchor nodes requre that the current SLAM sesson observes a full 6-DOF transformaton to another sesson before ran-defcent measurements le the planar factors can constran the anchor nodes. For the HAUV, ths full transformaton s approxmated wth a bearng-only monocular camera measurement by assgnng a rough estmate of scale. The covarance of the camera measurement s properly nflated along the ran-defcent egenvector to ensure that the matrx s full-ran. The addton of anchor node support to planar factors provdes mproved results for mult-sesson SLAM wth the HAUV. We globally algned two surveys that occur on the same day, wth each survey havng ts own global frame. The reset of the global frame happens each tme the vehcle completes a survey. We ntalze the global algnment wth a camera measurement, and add planar factors between the two surveys to refne the algnment, whch typcally leads to the addton of more camera measurements. These results are shown n Fg. 11. V. CONCLUSION We ntroduced a technque usng planar patches as a way to approxmate smooth, curved surfaces for real-tme factorgraph-based SLAM. We presented and verfed a smple model to ncorporate a noton of a surface s characterstc curvature when usng our planar factors to constran planar patches and 3D robot poses. Ths model was verfed n smulaton and showed that a robot wth hgh odometry uncertanty can reconstruct a sphere usng only planar surface measurements. The commonly-used DVL sensor for underwater vehcle navgaton produces a useful by-product: a sparse 3D pont cloud. We have shown that ths nformaton can be qute useful for SLAM applcatons nvolvng contnuous observatons of mostly-smooth surfaces, le shp hulls. Usng data from the HAUV, we have shown an ncrease n map consstency and a decrease n pose uncertanty usng our approach. Fnally, the use of anchor nodes for mult-sesson SLAM can be easly ncorporated nto our planar factors, and we have shown a notceable mprovement n mult-sesson SLAM usng the HAUV. Our approach models the characterstc curvature of the surface beng observed by the robot. The two user-provded parameters, the characterstc rad n the azmuth and elevaton dmensons, are held as fxed for the smulaton and expermental trals n ths paper. Though ths acheves good results for our experments, future wor could nvolve the onlne segmentaton of the observed planar patches nto separate characterstc rad. Ths would be necessary f the observed surface curvature s not well-approxmated by a fxed parameter. REFERENCES [1] R. Smth, M. Self, and P. Cheeseman, Estmatng uncertan spatal relatonshps n robotcs, n Autonomous Robot Vehcles, I. Cox and G. Wlfong, Eds. Sprnger-Verlag, 199, pp [] S. Thrun, M. Montemerlo, D. Koller, B. Wegbret, J. Neto, and E. Nebot, FastSLAM: An effcent soluton to the smultaneous localzaton and mappng problem wth unnown data assocaton, J. Machne Learnng Res., vol. 4, no. 3, pp , 4. [3] F. Dellaert and M. Kaess, Square root SAM: Smultaneous localzaton and mappng va square root nformaton smoothng, Int. J. Robot. Res., vol. 5, no. 1, pp , 6. [4] M. Kaess, A. Ranganathan, and F. Dellaert, SAM: Incremental smoothng and mappng, IEEE Trans. Robot., vol. 4, no. 6, pp , Dec. 8. [5] G. Grsett, D. Lod Rzzn, C. Stachnss, E. Olson, and W. Burgard, Onlne constrant networ optmzaton for effcent maxmum lelhood map learnng, n Proc. IEEE Int. Conf. Robot. and Automaton, Pasadena, CA, May 8, pp [6] Z. Zhang, Iteratve pont matchng for regstraton of free-form curves and surfaces, Int. J. Comput. Vs., vol. 13, no., pp , Oct [7] E. Olson, Real-tme correlatve scan matchng, n Proc. IEEE Int. Conf. Robot. and Automaton, Kobe, Japan, June 9, pp [8] R. M. Eustce, H. Sngh, and J. Leonard, Exactly sparse delayed-state flters, n Proc. IEEE Int. Conf. Robot. and Automaton, Barcelona, Span, Apr. 5, pp

8 (a) Intal camera regstraton (b) Algnment wthout planar factors (c) Algnment wth planar factors Fg. 11. Overvew of our mult-sesson SLAM system, where a 13 m surface survey s globally algned wth an underwater survey. In (a), two vews from each survey are used to compute a relatve-pose from whch to ntalze the global algnment. Ths algnment s refned wth the addton of more camera measurements and planar factors. In (b) and (c), we attempt ths experment wth and wthout the ad of planar factors. Wthout planar factors, the robot s only ntally algned to the prevous survey, wth the red hghlghted regon denotng areas wth frequent camera measurements between the two dves. After submergng and surfacng, however, uncorrected odometry error causes the faled detecton of loop closures at subsequent surfacngs, shown n the dotted crcled regon. In (c), the addton of planar factors enforces addtonal constrants on the global algnment, whch leads to a more geometrcally consstent map. In ths settng, the robot regularly adds camera measurements between the two surveys n the regons hghlghted n red. [9] R. M. Eustce, H. Sngh, J. J. Leonard, and M. R. Walter, Vsually mappng the RMS Ttanc: Conservatve covarance estmates for SLAM nformaton flters, Int. J. Robot. Res., vol. 5, no. 1, pp , 6. [1] I. Mahon, S. B. Wllams, O. Pzarro, and M. Johnson-Roberson, Effcent vew-based SLAM usng vsual loop closures, IEEE Trans. Robot., vol. 4, no. 5, pp , Oct. 8. [11] P. Rdao, M. Carreras, D. Rbas, and R. Garca, Vsual nspecton of hydroelectrc dams usng an autonomous underwater vehcle, J. Feld Robot., vol. 7, no. 6, pp , Nov. 1. [1] F. S. Hover, R. M. Eustce, A. Km, B. Englot, H. Johannsson, M. Kaess, and J. J. Leonard, Advanced percepton, navgaton and plannng for autonomous n-water shp hull nspecton, Int. J. Robot. Res., vol. 31, no. 1, pp , October 1. [13] A. Km and R. M. Eustce, Real-tme vsual SLAM for autonomous underwater hull nspecton usng vsual salency, IEEE Trans. Robot., vol. 9, no. 3, pp , June 13. [14] C. Kunz and H. Sngh, Map buldng fusng acoustc and vsual nformaton usng autonomous underwater vehcles, J. Feld Robot., 13, In Prnt. [15] Teledyne RD Instruments. (6) RD Instruments Worhorse Navgator DVL. Specfcaton sheet and documentatons Avalable at [16] Velodyne, Inc. (7, October) Velodyne HDL-64E: A hgh defnton LIDAR sensor for 3D applcatons. Specfcaton sheet and documentatons Avalable at [17] S. Barby, S. B. Wllams, O. Pzarro, and M. V. Jauba, Bathymetrc SLAM wth no map overlap usng gaussan processes, n Proc. IEEE/RSJ Int. Conf. Intell. Robots and Syst., 11, pp [18] J. Wengarten and R. Segwart, 3D SLAM usng planar segments, n Proc. IEEE/RSJ Int. Conf. Intell. Robots and Syst., 6, pp [19] A. J. B. Trevor, J. G. Rogers, and H. I. Chrstensen, Planar surface SLAM wth 3D and D sensors, n Proc. IEEE Int. Conf. Robot. and Automaton, 1, pp [] T. Petzsch, Planar features for vsual SLAM, Advances n Artfcal Intellgence, pp , 8. [1] M. Ruhne, R. Kummerle, G. Grsett, and W. Burgard, Hghly accurate 3D surface models by sparse surface adjustment, n Proc. IEEE Int. Conf. Robot. and Automaton, 1, pp [] B. Km, M. Kaess, L. Fletcher, J. J. Leonard, A. Bachrach, N. Roy, and S. Teller, Multple relatve pose graphs for robust cooperatve mappng, n Proc. IEEE Int. Conf. Robot. and Automaton, Anchorage, Alasa, May 1, pp [3] N. J. Mtra, A. Nguyen, and L. Gubas, Estmatng surface normals n nosy pont cloud data, Int. J. of Comput. Geom. and Applcat., vol. 14, pp , 4. [4] H. Carrllo, I. Red, and J. A. Castellanos, On the comparson of uncertanty crtera for actve SLAM, n Proc. IEEE Int. Conf. Robot. and Automaton, 1, pp