Nonlinear State Estimation for Robotics and Computer Vision Applications: An Overview

Transcription

1 Nonlinear State Estimation for Robotics and Computer Vision Applications: An Overview Arun Das 05/09/2017 Arun Das Waterloo Autonomous Vehicles Lab

2 Introduction What s in a name? Arun Das Waterloo Autonomous Vehicles Lab

3 Introduction Nonlinear State Estimation y = Cx Linear measurement model y = h(x) nonlinear measurement model Arun Das Waterloo Autonomous Vehicles Lab

4 Introduction for Robotics Arun Das Waterloo Autonomous Vehicles Lab

5 Introduction and Computer Vision Applications. Arun Das Waterloo Autonomous Vehicles Lab

6 Introduction Motivation Calibration SLAM Localization Build a least-squares estimation framework for many applications. Arun Das Waterloo Autonomous Vehicles Lab

7 Introduction Outline Syllabus Arun Das Waterloo Autonomous Vehicles Lab

8 Estimation Backend Vs Front End Front End Sensors Measurement Extraction State Estimate Measurements Back End Optimizer

9 Introduction Backend Vs Front End Scan-Matching IMU integration Feature Tracking Image Alignment Etc Front End Sensors Measurement Extraction State Estimate Measurements Back End Optimizer

10 Introduction Backend Vs Front End Scan-Matching IMU integration Feature Tracking Image Alignment Etc MAP Estimation Residual Building Lie Group Theory Factor Graphs Sliding Window Estimation Front End Sensors Measurement Extraction State Estimate Measurements Back End Optimizer

11 Introduction Calibration SLAM Localization

12 Introduction SLAM Calibration Localization Lie Group Theory Feature Tracking IMU Preintegration NL Least Squares Factor Graphs

13 Estimation Lie Group Theory Why do we need to study this? We deal with pose (orientation and position) as estimation states How do we optimize a Rotation or Transformation matrix? Study different parameterizations and representations Arun Das Waterloo Autonomous Vehicles Lab

14 Estimation Rotations Exponential Map Lie Group SO(3) State Vector Lie Algebra so(3) Parameter Estimate Exp. Map Log Map

15 Estimation Maximum Likelihood Estimation Most of the problems we will be dealing with can be formulated as a nonlinear least squares optimization Starts with maximum likelihood estimation Back End Nonlinear Least Squares

16 Estimation Maximum Likelihood Estimation independent measurements parameters Likelihood function: Log Likelihood function:

17 Estimation Maximum Likelihood Estimation Find the parameters that maximize the average log likelihood function:

18 Estimation Maximum Likelihood Estimation From estimation perspective, we can map the following concepts: Independent samples Sensor measurements Likelihood functions Measurement and motion models Parameters vehicle / camera poses, calibration quantities, etc.

19 Estimation Factor Graphs Factor Graphs: Graphical Representation for MLE/MAP x1,x2,x3 are estimation parameters (nodes) o1,o2 are odometry measurements Prior on x1 Likelihood of x2 and x3, given odometry o2 Likelihood of x1 and x2, given odometry o1 * Find x1,x2,x3 to maximize total likelihood:

20 Estimation Factor Graphs Factor Graphs: Graphical Representation for MLE/MAP Factors between two nodes are called Binary Factors Can also have unary, ternary,, n-ary factors Sometimes called residuals (ceres) constraints (g2o) Likelihood of x3, given measurement z3

21 Estimation Solving Factor Graphs Factor Graphs are solved using nonlinear optimization Most of the time, cost is formulated as a least squares problem Minimize the sum of squared residuals over all the factors

22 Estimation Solving Factor Graphs Because they are nonlinear, we need to linearize about a Linearization point using Taylor series expansion Where J is the Jacobian of the residual term

23 Estimation Solving Factor Graphs Substitute back into the original cost function: Where J is the Jacobian of the residual term To find the minimum, we just need to take the derivative wrt the parameter update and set it equal to zero.

24 Estimation Solving Factor Graphs To find the minimum, we just need to take the derivative wrt the parameter update and set it equal to zero. We finally get the linearized system we need to solve:

25 Estimation Solving Factor Graphs We solve the linear system using any method we wish. Note the sparsity pattern of H Once solved, the initial linearization point it updated: The process of linearization and solving is repeated until convergence.

26 Estimation Issue Issue: Our estimation tools are developed for vector spaces Many times, our state space lives on a manifold How can we use the vector space tools to update manifold quantities?

27 Estimation Box Operators

28 Estimation SLAM on Manifolds Approach: Perform same operations as before, but using the box operator where appropriate Not exactly same Jacobian from vector case! Need to take box-plus implementation into account when calculating Jacobian!

29 Estimation SLAM on Manifolds Solve the linear system Update the system state using box-plus

30 Estimation SLAM on Manifolds General Summary: Get an initial guess for the estimation parameters Until convergence: Linearize the nonlinear residual terms Calculate the Jacobian, Hessian, and gradient Solve the linear system Update the linearization point using the solved value Batch approach allows for re-linearization over all trajectory Better performance with nonlinear models Less error absorbed into prior

31 Estimation Batch vs Sliding Window Batch: Optimize over all poses Odometry GPS Optimization Window

32 Estimation Batch vs Sliding Window Sliding Window: Optimize over only poses in the window Odometry GPS Prior

33 Estimation Batch vs Sliding Window Sliding Window: Optimize over only poses in the window Odometry GPS Prior

34 Estimation Batch vs Sliding Window Sliding Window: Optimize over only poses in the window Odometry GPS Prior

35 Estimation Batch vs Sliding Window Sliding Window: Optimize over only poses in the window Odometry GPS Prior

36 Batch Estimation Batch vs Sliding Window Sliding Window: Optimize over only poses in the window Odometry GPS Prior

37 Estimation Batch vs Sliding Window Sliding Window: Optimize over only poses in the window Odometry GPS Prior Marginalization

38 Estimation Batch vs Sliding Window Sliding Window: Optimize over only poses in the window Odometry GPS Prior

39 Estimation Batch vs Sliding Window Sliding Window: Optimize over only poses in the window Odometry GPS Prior

40 Estimation Batch vs Sliding Window Sliding Window: Optimize over only poses in the window Odometry GPS Prior

41 Estimation Batch vs Sliding Window Sliding Window: Optimize over only poses in the window Odometry GPS Prior

42 Estimation Batch vs Sliding Window Sliding Window: Optimize over only poses in the window Odometry GPS Prior

43 Estimation Batch vs Sliding Window Sliding Window: Optimize over only poses in the window Odometry GPS Prior

44 Sensor Measurements Motivation We need to generate measurements from our raw sensor data Define measurement equations which predict the measurements given the state Scan-Matching IMU integration Feature Tracking Image Alignment Etc Front End y = Cx Sensors Linear measurement model y = h(x) Measurement Extraction nonlinear measurement model

45 Sensor Measurements Generating Residual Terms Residuals: The terms we wish to minimize (cost function contributions). Camera re-projection / photometric error. IMU Pre-integration. Laser Scan Registration. Kinematics models.

46 Sensor Measurements LIDAR Find the relative transformation between scans, in order to place them in a single co-ordinate system

47 Batch Estimation Scan Registration Find the relative transformation between scans, in order to place them in a single co-ordinate system

48 Sensor Measurements Scan Registration Find the relative pose from one time-step to the next using Scan Registration Factor graph

49 Sensor Measurements Cameras Feature based Track features between images using keypoints and descriptors (SIFT, SURF, FAST, ORB, etc).

50 Sensor Measurements Cameras Feature based re-projection error predicted meas. actual meas. l n Factor graph Triangulate the landmark location by minimizing re-projection error.

51 Sensor Measurements Cameras Photometric error Error is defined per-pixel, using photometric error. Facilitates dense map building

52 Sensor Measurements Odometry Find the relative pose from one time-step to the next using local measurements of velocity Factor graph

53 Sensor Measurements IMU Find the relative pose from one time-step to the next using local measurements of acceleration (need to account for biases!) Factor graph

54 Applications Course Overview By the end of this course, you will know how to Arun Das Waterloo Autonomous Vehicles Lab

55 Application Examples Lens calibration

56 Application Examples Car Calibration

57 Application Examples Car Calibration

58 Application Examples Laser SLAM

59 Application Examples Visual Inertial Odometry

60 Application Examples Laser Localization

61 Application Examples SVO

62 Application Examples DSO