The role of Planes and Edges in Lidar-Inertial Integration Teresa Vidal-Calleja Montreal, May 2019 cas.uts.edu.au
LIDAR - INERTIAL 3D- Lidar IMU (300kHz points) Lidar → Depth (10Hz) ● IMU → Acceleration, velocity (100Hz) ● Sparse point cloud (low vertical resolution) ● Motion during lidar sweep → Motion distortion ● Raw lidar - sparse Motion distortion cas.uts.edu.au |
APPROACH A probabilistic framework for localisation, mapping, and extrinsic calibration based on a 3D-lidar and a 6-DoF-IMU Key ideas Point-to-map plane minimisation from frame to target for simultaneous • localisation and calibration Point-to-edge/plane minimisation from frame to frame for simultaneous • localisation, mapping and autocalibration Upsampled Preintegrated Measurements for point transformation • > Motion distortion correction without explicit motion model Probabilistic full batch on-manifold optimisation • cas.uts.edu.au
APPROACH OVERVIEW cas.uts.edu.au cas.uts.edu.au
ROLE OF PLANE AND EDGE FEATURES Dense Lidar data: • ICP (and variations) • Surfels • 3D NDT, etc Sparse Lidar data [1] : • Keypoints that belong to edges and lines • Channel by channel extraction • Point to plane and line formulation for estimation [1] Ji Zhang and Sanjiv Singh, “LOAM: Lidar Odometry and Mapping in Real-time” Robotics: Science and Systems Conference, 2014. cas.uts.edu.au
ROLE OF PLANE AND EDGE FEATURES FEATURE EXTRACTION and MATCHING Features • Points with high score of being planar or corner features • Channel per channel Matching • Point reprojection to beginning of lidar frame • Dependent on current state and IMU interpolation to correct for motion distortion • Nearest Neighbour search Planar points Corners cas.uts.edu.au
ROLE OF PLANE AND EDGE FEATURES FORMULATION Point to explicit plane calibration Point to implicit plane Point to implicit line SLAM cas.uts.edu.au
ROLE OF PLANE AND EDGE FEATURES FORMULATION Maximum Likelihood Estimation with as the Lidar Factor point-to-map plane/ plane / line
APPROACH OVERVIEW cas.uts.edu.au
IMU PRE-INTEGRATION Preintegrated measurements [2] Independent of the initial conditions and processed before optimisation • Initial conditions Example on velocities Initial conditions + gyroscope integration ( f = accelerometer readings) Integration Preintegrated measurements (only depends on IMU readings) Preintegration Initial conditions [2] T. Lupton and S. Sukkarieh, “Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions” IEEE Transactions on Robotics, vol. 28, no. 1, pp. 61–76, 2012 cas.uts.edu.au
UPSAMPLED PREINTEGRATED IMU IMU IMU interpolation Measurements IMU raw measurements IMU inferred measurements Gaussian Process Regression ● Continuous measurement ● representation Preintegration over inferred ● measurements IMU Time No explicit motion model timestamps ● Lidar timestamps cas.uts.edu.au
R c , LIDAR-IMU CALIBRATION p c 3D- Lidar ? IMU Estimation of IMU-Lidar 3D transformation • Simple calibration target (set of planes) • Point-to-map plane minimisation from frame to target for simultaneous • localisation and calibration ICRA’18 cas.uts.edu.au
R c , LIDAR-IMU CALIBRATION p c 3D- Lidar ? IMU First static scan → Map creation (set of plane, ● e.g. a room corner) ICRA’18 cas.uts.edu.au
CALIBRATION OVERVIEW -t 0 Calibration target 3+ planes (E.g. room corner) Map creation First sweep static -t 1 Plane equations System motion Point to map-plane correspondence Simultaneous calibration and localisation -t 2 Lidar points as independent measurements ICRA’18 cas.uts.edu.au
SIMULTANEOUS CALIBRATION AND LOCALISATION Nodes (state to estimate) IMU poses Lidar scan timestamps Calibration parameters I 0 I 1 I m I 2 I M t 0 t 1 t m t 2 t M Factors (constraints) IMU factors Scan 1 Scan 0 Lidar factors ICRA’18 cas.uts.edu.au |
Lidar IMU CALIBRATION RESULTS Comparison with chained calibration with camera RGB-D camera Chained lidar – IMU/IMU - camera reprojection Lidar - camera reprojection ICRA’18 cas.uts.edu.au
IN2LAMA INERTIAL - LIDAR LOCALISATION AND MAPPING Point-to-plane and point-to-line • On-manifold full batch optimisation • Lidar/bias/time-shift factors • ICRA’19 cas.uts.edu.au |
IN2LAMA INERTIAL - LIDAR LOCALISATION AND MAPPING MLE Cost Function ICRA’19 cas.uts.edu.au |
IN2LAAMA - INERTIAL - LIDAR LOCALISATION AUTOCALIBRATION AND MAPPING Point-to-plane and point-to-line distance minimisation • On-manifold full batch optimisation • Lidar/bias/time-shift / IMU factors • Autocalibration • archive 1905.09517 cas.uts.edu.au
IN2LAAMA - INERTIAL - LIDAR LOCALISATION AUTOCALIBRATION AND MAPPING MLE Cost Function archive 1905.09517 cas.uts.edu.au
FRONT END • Planar and edge features • Frame-to-frame matching • Tight front end/back end interaction • Scan greater than 360 deg • Back and forth data association cas.uts.edu.au
RESULTS LOAM IN2LAMA IN2LAAMA cas.uts.edu.au
ACKNOWLEDGMENTS Cedric Le Gentil video link cas.uts.edu.au
Recommend
More recommend