Continuous trajectory estimation for 3D SLAM from actuated lidar

We extend the Iterative Closest Point (ICP) algorithm to obtain a method for continuous-time trajectory estimation (CICP) suitable for SLAM from actuated lidar. Traditional solutions to SLAM from actuated lidar rely heavily on the accuracy of an auxiliary pose sensor to form rigid frames. These frames are then used with ICP to obtain accurate pose estimates. However, since lidar records a single range sample at time, any error in inter-sample sensor motion must be accounted for. This is not possible if the frame is treated as a rigid point cloud. In this work, instead of ICP we estimate a continuous-time trajectory that takes into account inter-sample pose errors. The trajectory is represented as a linear combination of basis functions and formulated as a solution to a (sparse) linear system without restrictive assumptions on sensor motion. We evaluate the algorithm on synthetic and real data and show improved accuracy in open-loop SLAM in comparison to state-of-the-art rigid registration methods.

[1]  Marc Levoy,et al.  Efficient variants of the ICP algorithm , 2001, Proceedings Third International Conference on 3-D Digital Imaging and Modeling.

[2]  Michael Felsberg,et al.  Rolling shutter bundle adjustment , 2012, 2012 IEEE Conference on Computer Vision and Pattern Recognition.

[3]  Tim D. Barfoot,et al.  Towards relative continuous-time SLAM , 2013, 2013 IEEE International Conference on Robotics and Automation.

[4]  David G. Lowe,et al.  Fast Approximate Nearest Neighbors with Automatic Algorithm Configuration , 2009, VISAPP.

[5]  Roland Siegwart,et al.  Rolling Shutter Camera Calibration , 2013, 2013 IEEE Conference on Computer Vision and Pattern Recognition.

[6]  Paul Timothy Furgale,et al.  Continuous-time batch estimation using temporal basis functions , 2012, 2012 IEEE International Conference on Robotics and Automation.

[7]  Gabe Sibley,et al.  Spline Fusion: A continuous-time representation for visual-inertial fusion with application to rolling shutter cameras , 2013, BMVC.

[8]  Paul J. Besl,et al.  A Method for Registration of 3-D Shapes , 1992, IEEE Trans. Pattern Anal. Mach. Intell..

[9]  Brett Browning,et al.  Evaluating Pose Estimation Methods for Stereo Visual Odometry on Robots , 2010 .

[10]  J. Junkins,et al.  Optimal Estimation of Dynamic Systems , 2004 .

[11]  Gary K. L. Tam,et al.  Registration of 3D Point Clouds and Meshes: A Survey from Rigid to Nonrigid , 2013, IEEE Transactions on Visualization and Computer Graphics.

[12]  Nikos G. Tsagarakis,et al.  Proceedings of Robotics: Science and Systems IX , 2013 .

[13]  Stephen P. Boyd,et al.  An Interior-Point Method for Large-Scale l1-Regularized Logistic Regression , 2007, J. Mach. Learn. Res..

[14]  Paul Newman,et al.  Using laser range data for 3D SLAM in outdoor environments , 2006, Proceedings 2006 IEEE International Conference on Robotics and Automation, 2006. ICRA 2006..

[15]  Andrea Tagliasacchi,et al.  Sparse Iterative Closest Point , 2013, Comput. Graph. Forum.

[16]  Julien Mairal,et al.  Optimization with Sparsity-Inducing Penalties , 2011, Found. Trends Mach. Learn..

[17]  Michael Unser,et al.  Splines: a perfect fit for signal and image processing , 1999, IEEE Signal Process. Mag..

[18]  Brett Browning,et al.  Automatic Calibration of a Range Sensor and Camera System , 2012, 2012 Second International Conference on 3D Imaging, Modeling, Processing, Visualization & Transmission.

[19]  Paul Timothy Furgale,et al.  Gaussian Process Gauss–Newton for non-parametric simultaneous localization and mapping , 2013, Int. J. Robotics Res..

[20]  Paul Newman,et al.  Continuous vehicle localisation using sparse 3D sensing, kernelised rényi distance and fast Gauss transforms , 2013, 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[21]  C. R. Deboor,et al.  A practical guide to splines , 1978 .

[22]  Michael Bosse,et al.  Continuous 3D scan-matching with a spinning 2D laser , 2009, 2009 IEEE International Conference on Robotics and Automation.

[23]  Marc Levoy,et al.  Zippered polygon meshes from range images , 1994, SIGGRAPH.

[24]  Michael Bosse,et al.  Zebedee: Design of a Spring-Mounted 3-D Range Sensor with Application to Mobile Mapping , 2012, IEEE Transactions on Robotics.

[25]  Carl de Boor,et al.  A Practical Guide to Splines , 1978, Applied Mathematical Sciences.

[26]  Robert B. Fisher,et al.  Estimating 3-D rigid body transformations: a comparison of four major algorithms , 1997, Machine Vision and Applications.