L15. POSE-GRAPH SLAM. NA568 Mobile Robotics: Methods & Algorithms

Transcription

1 L15. POSE-GRAPH SLAM NA568 Mobile Robotics: Methods & Algorithms

2 Today s Topic Nonlinear Least Squares Pose-Graph SLAM Incremental Smoothing and Mapping

3 Feature-Based SLAM Filtering Problem: Motion Prediction Causes Fill-in Conceptual Abstraction: Full-SLAM Marginalize out the Poses -1

4 Feature-Based SLAM In feature-based SLAM, the information matrix fills in unless we enforce approximations to make it sparse This is because we are continually marginalizing out the robot trajectory from the state representation What if we were to marginalize out the landmarks instead? Feature-Based SLAM Pose-Base SLAM

5 Pose-Graph SLAM Feature-based SLAM requires approximations to enforce sparsity. Furthermore, this approximation is non-trivial Conceptual Abstraction: Full-SLAM Marginalize out the Landmarks Key idea: marginalizing out the landmarks preserves locality

6 Three Main SLAM Paradigms Kalman filter Particle filter Graphbased least squares approach to SLAM Courtesy: C. Stachniss

7 Least Squares in General Approach for computing a solution for an overdetermined system More equations than unknowns Minimizes the sum of the squared errors in the equations Standard approach to a large set of problems Today: Application to SLAM Courtesy: C. Stachniss

8 Graph-Based SLAM Constraints connect the poses of the robot while it is moving Constraints are inherently uncertain Robot pose Constraint Courtesy: C. Stachniss

9 Graph-Based SLAM Observing previously seen areas generates constraints between non-successive poses Robot pose Constraint Courtesy: C. Stachniss

10 Idea of Graph-Based SLAM Use a graph to represent the problem Every node in the graph corresponds to a pose of the robot during mapping Every edge between two nodes corresponds to a spatial constraint between them Graph-Based SLAM: Build the graph and find a node configuration that minimizes the error introduced by the constraints Courtesy: C. Stachniss

11 Graph-Based SLAM in a Nutshell Every node in the graph corresponds to a robot position and a laser measurement An edge between two nodes represents a spatial constraint between the nodes Courtesy: C. Stachniss KUKA Halle 22, courtesy of P. Pfaff

12 Graph-Based SLAM in a Nutshell Every node in the graph corresponds to a robot position and a laser measurement An edge between two nodes represents a spatial constraint between the nodes Courtesy: C. Stachniss KUKA Halle 22, courtesy of P. Pfaff

13 Graph-Based SLAM in a Nutshell Once we have the graph, we determine the most likely map by correcting the nodes Courtesy: C. Stachniss

14 Graph-Based SLAM in a Nutshell Once we have the graph, we determine the most likely map by correcting the nodes like this Courtesy: C. Stachniss

15 Graph-Based SLAM in a Nutshell Once we have the graph, we determine the most likely map by correcting the nodes like this Then, we can render a map based on the known poses Courtesy: C. Stachniss

16 The Overall SLAM System Interplay of front-end and back-end Map helps to determine constraints by reducing the search space Topic today: optimization node positions raw data Graph Construction (Front-End) graph (nodes & edges) Graph Optimization (Back-End) Courtesy: C. Stachniss today

17 The Graph It consists of n nodes Each is a 2D or 3D transformation (the pose of the robot at time ) A constraint/edge exists between the nodes and if Courtesy: C. Stachniss

18 Create an Edge If (1) the robot moves from to Edge corresponds to odometry The edge represents the odometry measurement Courtesy: C. Stachniss

19 Create an Edge If (2) the robot observes the same part of the environment from and from Measurement from Measurement from Courtesy: C. Stachniss

20 Create an Edge If (2) the robot observes the same part of the environment from and from Construct a virtual measurement about the position of seen from Edge represents the position of from based on the observation seen Courtesy: C. Stachniss

21 Transformations Transformations can be expressed using homogenous coordinates Odometry-Based edge Observation-Based edge How node i sees node j Courtesy: C. Stachniss

22

23 Visual SLAM: Why Filter?

24 Visual SLAM

25 The SLAM Problem (t=0) Landmark measurement Robot Onboard sensors: Wheel odometry Inertial measurement unit (gyro, accelerometer) Sonar Laser range finder Camera RGB-D sensors Landmark

26 The SLAM Problem (t=1) Odometry measurement Robot Landmark measurement Landmark 1 Landmark 2

27 The SLAM Problem (t=n-1) Odometry measurement Robot Landmark measurement Landmark 1 Landmark 2

28 The SLAM Problem (t=n) Odometry measurement Landmark measurement

29 Factor Graph Representation Odometry measurement Robot pose Landmark measurement Landmark position Bipartite graph with variable nodes and factor nodes

30 Factor Graph Representation: Pose Graph Odometry measurement Loop closing constraint Robot pose Landmark measurement Landmark position Bipartite graph with variable nodes and factor nodes

31 Factor Graph Representation: Bundle Adjust. Pose Point measurement Point Bipartite graph with variable nodes and factor nodes

32 Nonlinear Least-Squares [Dellaert and Kaess, IJRR 06] poses points Gaussian noise Repeatedly solve linearized system (GN) A b

33 Solving the Linear Least-Squares System Solve: A Normal equations A T A Information matrix Measurement Jacobian

34 Solving the Linear Least-Squares System Can we simply invert A T A to solve for x? Normal equations A T A Yes, but we shouldn t The inverse of A T A is dense! O(n 3 ) Can do much better by taking advantage of sparsity! Information matrix

35 Solving the Linear Least-Squares System Solve: [Dellaert and Kaess, IJRR 06] A Normal equations Matrix factorization A T A Information matrix R Measurement Jacobian Square root information matrix

36 Matrix Square Root Factorization QR on A: Numerically More Stable A QR R Cholesky on A T A: Faster A T A Cholesky R

37 Retaining Sparsity: Variable Ordering Fill-in depends on elimination order: A T A R [Dellaert and Kaess, IJRR 06] factor Default ordering (poses, landmarks) permute factor Ordering based on COLAMD heuristic [Davis04] (best order: NP hard)

38 Solving by Backsubstitution After factorization: R T R x = A T b Forward substitution R T y = A T b, solve for y R T Backsubstition R x = y, solve for x R

39 Full Bundle Adjustment From Strasdat et al, 2011 IVC Visual SLAM: Why filter? Graph grows with time: Have to solve a sequence of increasingly larger BA problems Will become too expensive even for sparse Cholesky F. Dellaert and M. Kaess, Square Root SAM: Simultaneous localization and mapping via square root information smoothing, IJRR 2006

40 Keyframe Bundle Adjustment Drop subset of poses to reduce density/complexity Only retain keyframes necessary for good map Complexity still grows with time, just slower

41 Filter Keyframe idea not applicable: map would fall apart Instead, marginalize out previous poses Extended Kalman Filter (EKF) Problems when used for Visual SLAM: All points become fully connected! expensive Relinearization not possible! inconsistent

42 Incremental Solver Back to full BA and keyframes: New information is added to the graph Older information does not change Can be exploited to obtain an efficient solution!

43 Incremental Smoothing and Mapping (isam)

44 isam Solving a growing system: Exact/batch (quickly gets expensive) Approximations Incremental Smoothing and Mapping (isam) Key idea: New measurements -> Append to existing matrix factorization Repair using Givens rotations Periodic batch steps for Relinearization Variable reordering (to keep sparsity) [Kaess et al., TRO 08] R R

45 Factor Updates with Givens Rotations Zeroing entries with Givens Rotations: Old R factor New rows New R factor: Triangulated using Givens Rotations Numerically stable! Constant time!

46 Variable Reordering Constrained COLAMD Greedy approach Constrained Ordering Arbitrary placement of newest variable Newest variables forced to the end Number of affected variables: low high Much cheaper!

47 Variable Reordering Fill-in Incremental ordering still yields good overall ordering Only slightly more fill-in than batch COLAMD ordering Constrained ordering is worse than naïve/greedy: Suboptimal ordering because of partial constraint, but cheaper to update!

48 isam [Kaess et al., TRO 08] Example from real sequence: Square root inf. matrix Side length: variables Dense: 1.7GB, sparse: 1MB R non-zeros 0.1% density 11/column

49 Next Lecture FastSLAM