程序代写 Perception for Autonomous Systems 31392:

Perception for Autonomous Systems 31392:
Visual SLAM
Simultaneous Localization & Mapping
Lecturer: —PhD

4 Apr. 2022 DTU Electrical Engineering 2

• Sum up Localization from last time • Some terminology
• Pose-Landmark Graph Slam
• Example of Linear 1D SLAM
• Non-Linear Optimization approaches • Bundle Adjustment
• Visual Slam System architecture
4 Apr. 2022 DTU Electrical Engineering 3

Visual Odometry is great
• Lets Sum Up What we did last time • We performed 3D-to-2D
4 Apr. 2022 DTU Electrical Engineering 4

Visual Odometry is great
• Lets Sum Up What we did last time
• We performed 3D-to-2D
• Any better approach? How could we – How about some more features?
• Let’s keep the features from multiple frames
– How could do we know that those are new features?
4 Apr. 2022 DTU Electrical Engineering 5

Visual Odometry is great
• Anything more we mentioned?
– Windowed Bundle Adjustment (BA)
4 Apr. 2022 DTU Electrical Engineering 6

Windowed Bundle Adjustment (BA)
• Similar to pose-optimization but it also optimizes 3D points
• In order to not get stuck in local minima, the initialization should be close the minimum • Levenberg-Marquadt can be used
4 Apr. 2022 DTU Electrical Engineering 7

Formal definitions
• Visual Odometry
• Structure from Motion (SFM) • Bundle Adjustment
• Visual SLAM
SfM VSLAM VO
4 Apr. 2022 DTU Electrical Engineering 8

Pose-Landmark Graph-Slam
• SLAM problem depicted as Bayes network graph
• At each location xt
• Observes a nearby feature
in the map m = {m1; m2; m3}
• Movement ut
• An arrow defines causal relationship
4 Apr. 2022 DTU Electrical Engineering 10

Graph-Based SLAM
Definition
• Use a graph to represent the problem
• Nodes represent:
– poses or
– locations
• Edges Represent:
– Landmark observations
– Odometry Measurements
• The minimization optimizes
the landmark locations and robot poses
Graph-Based SLAM: Build the graph and find a node configuration that minimize the error introduced by the constraints
4 Apr. 2022 DTU Electrical Engineering 11

Graph-Based SLAM (intuition of optimization)
• Observing previously seen areas generates constraints between non-successive poses • Treat constraints (motion and measurement) as “soft” elastic springs
• Want to minimize the total energy in the springs
We can define the error as follows
• Expected observation (2D sensor):
– With the error:
4 Apr. 2022 DTU Electrical Engineering 12

1D Linear SLAM
• In the linear case we can solve as follows:
– First construct all constrains • Absolute Constrain:
X(0) = Q – starting position
• Movement Constrains: X(t) = X(t-1) + Dx(t)
• Measurement constrains:
L(k) = X(t) + N
• Then, solve linear equations
4 Apr. 2022 DTU Electrical Engineering 13

1D Linear SLAM – case 1
• Case 1 – Exact solution exists:
4 Apr. 2022 DTU Electrical Engineering 14

1D Linear SLAM – case 2
• Case 2 – Overdefined problem: – X0 sees L0 at distance 10
– X1 sees L0 at distance 5
– X2 sees L0 at distance 2
We infer a consistent landmark position
4 Apr. 2022 DTU Electrical Engineering 15

1D Linear SLAM – case 3
• Case 3 – Inconsistent Measurements : – X0 sees L0 at distance 10
– X1 sees L0 at distance 5
– X2 sees L0 at distance 1 (Wrong)
We handled inaccurate measurements
4 Apr. 2022 DTU Electrical Engineering 16

1D Linear SLAM – case 4
Case 4 – Inconsistent Measurements with Confidence Matrix:
• Linear Least Squares allows us
to include a weighting of each linear constraint.
• We can include weights in the computation
• We weight each constraint by
a diagonal matrix where the weights are 1/variance for each constraint.
• Let’s say 2 variance is 5
Why did the estimation just become worse??
4 Apr. 2022 DTU Electrical Engineering 17

What about non-Linear Least Squares?
4 Apr. 2022 DTU Electrical Engineering 18

Gauss Newton Solution
4 Apr. 2022 DTU Electrical Engineering 19

Gauss Newton
1D Gauss-Newton (Newton) iteration.
4 Apr. 2022 DTU Electrical Engineering 20

Gauss Newton
1D Gauss-Newton (Newton) iteration (failure)
4 Apr. 2022 DTU Electrical Engineering 21

Gradient Descent
4 Apr. 2022 DTU Electrical Engineering 22

Levenberg-Marquadt
4 Apr. 2022 DTU Electrical Engineering 23

What about non-Linear Least Squares?
• Lets See some examples 1:
Gauss-Newton Gradient descent Levenberg
4 Apr. 2022 DTU Electrical Engineering 24

What about non-Linear Least Squares?
• Lets See some examples 2:
Gauss-Newton Gradient descent Levenberg
4 Apr. 2022 DTU Electrical Engineering 25

What about non-Linear Least Squares?
• Lets See some examples 3:
Gauss-Newton Gradient descent Levenberg
• It is obvious that displays robustness
4 Apr. 2022 DTU Electrical Engineering 26

Bundle Adjustment
• Bundle Adjustment is the employment of nonlinear optimization in the problem of the minimization of the re-projection error, by finding the optimal Poses (extrinsics) of the cameras and the locations of the 3D points.
4 Apr. 2022 DTU Electrical Engineering 27

Bundle Adjustment – Linearization
4 Apr. 2022 DTU Electrical Engineering 28

Bundle Adjustment – Linearization
• The Linearization of the minimization happens by calculating the Jacobian of the projection matrix
• Assuming the following projection matrix:
• We first define the orientation as the rotation matrix
associated with the axis angle wx,wy,wz using the Rodriques equation
• The Jacobian FUNCTION can be calculated as:
4 Apr. 2022 DTU Electrical Engineering 29

Bundle Adjustment – Comments
• Bundle adjustment (and graph optimization) is the backbone of all SLAM algorithms • Keep in mind that:
– We need to provide the Jacobian of the projection
– We usually provide a covariance matrix (See the linear case for uncertainty)
– It is solved using Levenberg-Marquadt
– There are a lot of computational issues which we can overcome by exploiting the sparsity of the function AX=b (see least squares)
Look at the following A and b Matrices
This solution is called Sparse Bundle Adjustment!
4 Apr. 2022 DTU Electrical Engineering 30

Bundle Adjustment – Covariance
➢ The uncertainty of the camera pose 𝐶𝑘 is a combination of the uncertainty at 𝐶𝑘−1 (black-solid ellipse) and the uncertainty of the
transformation 𝑇 𝑘
➢𝐶=𝑓(𝐶 ,𝑇) 𝑘 𝑘−1𝑘
(gray dashed ellipse)
➢ The combined covariance ∑𝑘is
➢ The camera-pose uncertainty is always increasing when concatenating transformations. Thus, it is important to keep the uncertainties of the individual transformations small
4 Apr. 2022 DTU Electrical Engineering

Recent Visual Slam Solutions – Intro
• That was too much info, let’s see now some recent solutions to the Slam Problem:
• Most recent visual SLAM methods are split in two parts:
• The Frontend: where the raw data are converted into pose graphs and Loop constrains and
the Backend where, given a graph with constrains, the new pose of the robot is calculated as well as the surrounding map points.
4 Apr. 2022 DTU Electrical Engineering 32

Recent Visual Slam Solutions – Common Architecture
• Front End
– Data Association
• Frame to Frame
• Multi-frame
• Loop Closure Detection
– Geometric Initialization
• Pose Estimation
• Landmark Triangulation
– System Formation
• Observation Matrix
• Covariance Matrix
• Graph Generation and Update
• Back End
– Filter-Based State Estimation
• Extended Kalman Filter
• Particle Filters
– Least squares optimization
• Bundle Adjustment • Graph Optimization
– Key Frame
4 Apr. 2022 DTU Electrical Engineering 33

Recent Visual Slam Solutions – recent advances
Towards Realtime operation?:
• The computational cost of bundle adjustment has lead to the idea of keyframing: i.e.: identifying and describing some of the frames to be used for graph optimization.
• Bags of words for robust loop closure. – What can you tell me about that?
• Co-visibility Graph
4 Apr. 2022 DTU Electrical Engineering 34

• The ORBSLAM algorithm is one of the most well performing opensource implementations of visual slam. • Three parallel threads:
– tracking,
• localizing the camera with every frame and deciding when to insert a new keyframe
– local mapping
• Processes new keyframes and performs local BA to achieve an optimal
• reconstruction in the surroundings of the camera pose – loop closing
• The loop closing searches for loops with every new keyframe
• Essential Graph
4 Apr. 2022 DTU Electrical Engineering 35

ORB SLAM on Kitti
4 Apr. 2022 DTU Electrical Engineering 36

Perception for Autonomous Systems 31392:
Visual SLAM
Simultaneous Localization & Mapping
Lecturer: —PhD
4 Apr. 2022 DTU Electrical Engineering 38

程序代写 CS代考加微信: powcoder QQ: 1823890830 Email: powcoder@163.com

Related Posts