Maximum Likelihood Point Cloud. Acquisition from a ... Algorithm Overview. 3. Robot moves .... Point clouds acquired were compared to raw odometry-based ...
Maximum Likelihood Point Cloud Acquisition from a Mobile Platform Todor Stoyanov and Achim J. Lilienthal AASS Learning Systems Lab, Örebro University International Conference on Autonomous Robots (ICAR) LS Lab Meeting, 28 Jan 2009
2009-06-24
1 Problem Definition
ICAR, 24 June 2009
1 Problem Definition
ICAR, 24 June 2009
1 Problem Definition
ICAR, 24 June 2009
1 Problem Definition
ICAR, 24 June 2009
1 Problem Definition
ICAR, 24 June 2009
1 Problem Definition
ICAR, 24 June 2009
Outline 1 Problem Definition 2 Related Work 3 Approach ●
Sensors, Robot, Calibration Routines
●
Estimation Algorithm
4 Performance Evaluation
ICAR, 24 June 2009
2 Related Approaches Harrison and Newman “High Quality 3D Laser Ranging Under General Vehicle Motion” ICRA 08 Rapidly tilting laser Extract planes from scans Assume almost vertical objects are vertical Compute correction between successive scans and interpolate
ICAR, 24 June 2009
2 Related Approaches Bosse and Zlot “Continuous 3d ScanMatching with a Spinning 2D Laser” ICRA 09 Group points in a spin Extract and match voxels Correct in-scan trajectory through interpolation
ICAR, 24 June 2009
3 Sensors and Platform Custom robot Front laser for obstacle avoidance 3D range data acquisition: SICK LMS200 Amtec rotational actuator
ICAR, 24 June 2009
3 Sensors and Platform Custom robot Front laser for obstacle avoidance 3D range data acquisition: SICK LMS200 Amtec rotational actuator
ICAR, 24 June 2009
3 Sensors and Platform Custom robot Front laser for obstacle avoidance 3D range data acquisition: SICK LMS200 Amtec rotational actuator
ICAR, 24 June 2009
3 Sensors and Platform Custom robot Front laser for obstacle avoidance 3D range data acquisition: SICK LMS200 Amtec rotational actuator
ICAR, 24 June 2009
3 Sensor Calibration To acquire accurate range information, estimate spatial parameters Approach : Measure an empty room by hand Acquire range data as well Fit planes using RANSAC Plane-to-plane distances using least squares
Estimate dx to fit measurements ICAR, 24 June 2009
3 Algorithm Overview Robot moves through environment Rotate laser and observe points Use odometry for point coordinate calculation
When points are reobserved perform registration Distribute error along the path segment ICAR, 24 June 2009
3 Algorithm Overview Robot moves through environment Rotate laser and observe points Use odometry for point coordinate calculation
When points are reobserved perform registration Distribute error along the path segment ICAR, 24 June 2009
3 Algorithm Overview Robot moves through environment Rotate laser and observe points Use odometry for point coordinate calculation
When points are reobserved perform registration Distribute error along the path segment ICAR, 24 June 2009
3 Algorithm Overview
ICAR, 24 June 2009
3 State Covariance Estimation Use odometry sensor of the robot Short travel distance (up to 5-7 m) Offers three state variables – x and y position, orientation Discrete measurement sequence Standard method for covariance estimation through propagation
ICAR, 24 June 2009
3 Algorithm Overview
ICAR, 24 June 2009
3 Estimating 3D Points When new sensor data arrives we use: Odometry position and orientation Actuator orientation Laser scanner bearing and range
Estimate 3D coordinates of observed point
ICAR, 24 June 2009
3 Algorithm Overview
ICAR, 24 June 2009
3 Uncertainty Propagation Idea: asses the uncertainty in estimated 3D range points Points are affected by: x, y position of the robot Robot orientation Actuator orientation Laser range measurement
Use error propagation on these variables 3x3 Covariance matrix Geometrical interpretation is an ellipsoid ICAR, 24 June 2009
3 Algorithm Overview
ICAR, 24 June 2009
3 Nearest Neighbor Search Estimated 3D points are inserted in a spatial indexing data structure Oct Tree
Allows for fast neighbor search When new points are inserted Find closest neighbor Check if inside uncertainty ellipsoid If yes, mark as a candidate match point
ICAR, 24 June 2009
3 Algorithm Overview
ICAR, 24 June 2009
3 Point Set Registration Two sets of 3D points observed Model points – low error at start of motion Data points – with estimated error ellipsoids
Use Iterative Closest Point (ICP) for registration Initial corresponding pairs from Nearest Neighbor (NN) computations
ICAR, 24 June 2009
3 Handling Occlusions
ICAR, 24 June 2009
3 Handling Occlusions
ICAR, 24 June 2009
3 Handling Occlusions
ICAR, 24 June 2009
3 Handling Occlusions
ICAR, 24 June 2009
3 Handling Occlusions
ICAR, 24 June 2009
3 Algorithm Overview
ICAR, 24 June 2009
3 Multi-Level Relaxation Graph-based approach for error minimization Each robot state is a node Connect consecutive measurements with edges Encode motion as transformation vector Uncertainty as covariance matrix
Add an edge from ICP registration
ICAR, 24 June 2009
3 Multi-Level Relaxation Graph-based approach for error minimization Each robot state is a node Connect consecutive measurements with edges Encode motion as transformation vector Uncertainty as covariance matrix
Add an edge from ICP registration
ICAR, 24 June 2009
3 Multi-Level Relaxation Graph-based approach for error minimization Each robot state is a node Connect consecutive measurements with edges Encode motion as transformation vector Uncertainty as covariance matrix
Add an edge from ICP registration
ICAR, 24 June 2009
3 Multi-Level Relaxation Graph-based approach for error minimization Each robot state is a node Connect consecutive measurements with edges Encode motion as transformation vector Uncertainty as covariance matrix
Add an edge from ICP registration
ICAR, 24 June 2009
3 Multi-Level Relaxation Graph-based approach for error minimization Each robot state is a node Connect consecutive measurements with edges Encode motion as transformation vector Uncertainty as covariance matrix
Add an edge from ICP registration
ICAR, 24 June 2009
3 Multi-Level Relaxation Graph-based approach for error minimization Each robot state is a node Connect consecutive measurements with edges Encode motion as transformation vector Uncertainty as covariance matrix
Add an edge from ICP registration
ICAR, 24 June 2009
4 Results and Evaluation Evaluation was conducted on recorded data sets Point clouds acquired were compared to raw odometry-based point clouds Partially observable indoor environment Few features
ICAR, 24 June 2009
4 Results and Evaluation
ICAR, 24 June 2009
4 Results and Evaluation
ICAR, 24 June 2009
4 Results and Evaluation
ICAR, 24 June 2009
4 Results and Evaluation
ICAR, 24 June 2009
4 Results and Evaluation
ICAR, 24 June 2009
4 Results and Evaluation
ICAR, 24 June 2009
4 Limitations Sampling rate Maximum rotational speed
Large viewpoint changes Maximum translational speed
Feature-poor environments 6DoF motion ICAR, 24 June 2009
4 Conclusion and Outlook Presented an algorithm that: Uses a probabilistic framework Produces locally consistent point clouds
Future work will focus on: Addressing limitations Quantitative evaluation
ICAR, 24 June 2009
Thank you! (Questions)
ICAR, 24 June 2009
Short Bibliography [1] A. Harrison and P. Newman, “High Quality 3D Laser Ranging Under General Vehicle Motion,” Robotics and Automation, 2008. ICRA 2008. IEEE International Conference on, pp. 7–12, May 2008. [2] M. Bosse and R. Zlot “Continuous 3d Scan-Matching with a Spinning 2D Laser” Robotics and Automation, 2009. ICRA 2009. IEEE International Conference on, pp. 12-17, May 2009. [3] U. Frese, P. Larsson, and T. Duckett, “A Multilevel Relaxation Algorithm for Simultaneous Localization and Mapping,” Robotics, IEEE Transactions on, vol. 21, no. 2, pp. 196–207, April 2005. [4] U. Frese, “A Discussion of Simultaneous Localization and Mapping,” Autonomous Robots, vol. 20, no. 1, pp. 25–42, 2006. [5] P. J. Besl and N. D. McKay, “A method for registration of 3-D shapes,” IEEE Trans. on Pattern Analysis and Machine Intelligence, vol. 14, no. 2, pp. 239–256, Feb 1992. [6] O. Bengtsson and A. Baerveldt, “Localization in changing environments - estimation of a covariance matrix for the IDC algorithm,” in Intelligent Robots and Systems, 2001. Proceedings. 2001 IEEE/RSJ International Conference on, vol. 4, 2001, pp. 1931–1937 vol.4. ICAR, 24 June 2009
3 State Covariance Estimation - detail Accumulate oriented uncertainty Each step uncertainty depends on distance and angle traveled
ICAR, 24 June 2009
3 Estimating 3D Points - detail Originally points estimated through:
To propagate uncertainty compute Jacobian:
ICAR, 24 June 2009
3 Estimating 3D Points – detail (2) Finally use to perform error propagation through:
ICAR, 24 June 2009
