Nonlinear State Estimation for Robotics and Computer Vision Applications: An Overview Arun Das 05/09/2017 Arun Das Waterloo Autonomous Vehicles Lab
Introduction What s in a name? Arun Das Waterloo Autonomous Vehicles Lab
Introduction Nonlinear State Estimation y = Cx Linear measurement model y = h(x) nonlinear measurement model Arun Das Waterloo Autonomous Vehicles Lab
Introduction for Robotics Arun Das Waterloo Autonomous Vehicles Lab
Introduction and Computer Vision Applications. Arun Das Waterloo Autonomous Vehicles Lab
Introduction Motivation Calibration SLAM Localization Build a least-squares estimation framework for many applications. Arun Das Waterloo Autonomous Vehicles Lab
Introduction Outline Syllabus Arun Das Waterloo Autonomous Vehicles Lab
Estimation Backend Vs Front End Front End Sensors Measurement Extraction State Estimate Measurements Back End Optimizer
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
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
Introduction Calibration SLAM Localization
Introduction SLAM Calibration Localization Lie Group Theory Feature Tracking IMU Preintegration NL Least Squares Factor Graphs
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
Estimation Rotations Exponential Map Lie Group SO(3) State Vector Lie Algebra so(3) Parameter Estimate Exp. Map Log Map
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
Estimation Maximum Likelihood Estimation independent measurements parameters Likelihood function: Log Likelihood function:
Estimation Maximum Likelihood Estimation Find the parameters that maximize the average log likelihood function:
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.
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:
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
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
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
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.
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:
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.
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?
Estimation Box Operators
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!
Estimation SLAM on Manifolds Solve the linear system Update the system state using box-plus
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
Estimation Batch vs Sliding Window Batch: Optimize over all poses Odometry GPS Optimization Window
Estimation Batch vs Sliding Window Sliding Window: Optimize over only poses in the window Odometry GPS Prior
Estimation Batch vs Sliding Window Sliding Window: Optimize over only poses in the window Odometry GPS Prior
Estimation Batch vs Sliding Window Sliding Window: Optimize over only poses in the window Odometry GPS Prior
Estimation Batch vs Sliding Window Sliding Window: Optimize over only poses in the window Odometry GPS Prior
Batch Estimation Batch vs Sliding Window Sliding Window: Optimize over only poses in the window Odometry GPS Prior
Estimation Batch vs Sliding Window Sliding Window: Optimize over only poses in the window Odometry GPS Prior Marginalization
Estimation Batch vs Sliding Window Sliding Window: Optimize over only poses in the window Odometry GPS Prior
Estimation Batch vs Sliding Window Sliding Window: Optimize over only poses in the window Odometry GPS Prior
Estimation Batch vs Sliding Window Sliding Window: Optimize over only poses in the window Odometry GPS Prior
Estimation Batch vs Sliding Window Sliding Window: Optimize over only poses in the window Odometry GPS Prior
Estimation Batch vs Sliding Window Sliding Window: Optimize over only poses in the window Odometry GPS Prior
Estimation Batch vs Sliding Window Sliding Window: Optimize over only poses in the window Odometry GPS Prior
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
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.
Sensor Measurements LIDAR Find the relative transformation between scans, in order to place them in a single co-ordinate system
Batch Estimation Scan Registration Find the relative transformation between scans, in order to place them in a single co-ordinate system
Sensor Measurements Scan Registration Find the relative pose from one time-step to the next using Scan Registration Factor graph
Sensor Measurements Cameras Feature based Track features between images using keypoints and descriptors (SIFT, SURF, FAST, ORB, etc).
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.
Sensor Measurements Cameras Photometric error Error is defined per-pixel, using photometric error. Facilitates dense map building
Sensor Measurements Odometry Find the relative pose from one time-step to the next using local measurements of velocity Factor graph
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
Applications Course Overview By the end of this course, you will know how to Arun Das Waterloo Autonomous Vehicles Lab
Application Examples Lens calibration
Application Examples Car Calibration
Application Examples Car Calibration
Application Examples Laser SLAM
Application Examples Visual Inertial Odometry
Application Examples Laser Localization
Application Examples SVO
Application Examples DSO