6 DOF EKF SLAM in Underwater Environments Markus Solbach Final - - PowerPoint PPT Presentation

▶
6 dof ekf slam in underwater environments
SMART_READER_LITE
LIVE PREVIEW

6 DOF EKF SLAM in Underwater Environments Markus Solbach Final - - PowerPoint PPT Presentation

Nov 08, 2023 204 likes •889 views

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion 6 DOF EKF SLAM in Underwater Environments Markus Solbach Final Masters Project Universitat de les Illes Balears September 24, 2014 1 / 66 Introduction

slide-1
SLIDE 1

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

6 DOF EKF SLAM in Underwater Environments

Markus Solbach

Final Masters Project Universitat de les Illes Balears

September 24, 2014

1 / 66

slide-2
SLIDE 2

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Introduction Problem Statement Related Work 3D Transformation Composition ⊕ Inversion ⊖ Jacobian Matrices Image Registration Visual EKF-SLAM Prediction Stage Augmentation Stage Update Stage Results Conclusion

2 / 66

slide-3
SLIDE 3

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Problem Statement

3 / 66

slide-4
SLIDE 4

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Problem Statement

  • Accessibility of the sub-aquatic world is important for research

and industry

  • AUV1 promising advantages compared to ROV2
  • Untethered, independent, self-powered, ...
  • Question: How to perform the localization of AUVs
  • Accurate localization is important for the mission success
  • Maintenance, Rescue Operations, Sampling, Inspections, ...

1Autonomous Underwater Vehicle 2Remotely Operated Vehicle 4 / 66

slide-5
SLIDE 5

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Vehicle Localization

  • pose = Position and Orientation
  • 6 Degrees of Freedom
  • 3 Translation
  • 3 Rotation
  • Vehicle State X = pose (in this work)
  • collection of poses = State Vector ֒

→ Trajectory

5 / 66

slide-6
SLIDE 6

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Vehicle Localization

  • Several possibilities
  • Using:
  • IMU (velocity, orientation, and gravitational forces)
  • Odometry (Acoustic Sensors or Cameras)
  • Sensor Fusion
  • Prone to Drift
  • Visual Odometry, because Cameras

+ Spatial and Temporal Resolution + More Environmental Data − Dependent on light and visibility

6 / 66

slide-7
SLIDE 7

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

SLAM

  • Visual Odometry
  • Displacement of two consecutive Images
  • Estimation of the Absolute Motion (Prone to drift)
  • SLAM (Simultaneous Localization And Mapping)
  • Most successful approach
  • Computes pose
  • Refines pose of landmarks of environment
  • Extended Kalman Filtering (EKF)

= Visual EKF SLAM

7 / 66

slide-8
SLIDE 8

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

EKF (In a Nutshell)

  • Three Stages
  • 1. Prediction Stage
  • Predicting vehicle’s localization (visual odometry)
  • Prone to drift
  • Uncertainty is modelled with covariance matrix
  • 2. State Augmentation Stage
  • Prediction is added to the end of X
  • Uncertainty accumulates over time
  • 3. Update Stage
  • Detection of Loop Closings
  • Provide the system with more reliable Data
  • Update X

8 / 66

slide-9
SLIDE 9

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Related Work

9 / 66

slide-10
SLIDE 10

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Related Work

  • Literature is scarce, but deals mainly with:
  • Correcting the odometry with the result of the Image

Registration

  • Adding Landmarks to X

+ Continous Correction of pose and landmarks + Whole X is corrected − Increasing complexity over time (X gets big) − On-line usage no longer possible

10 / 66

slide-11
SLIDE 11

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Related Work

  • [Schattschneider et al., 2011]
  • Underwater SLAM
  • Stereo Camera System used for ship hull inspection
  • 3D Landmarks used to detect Loop Closings
  • State = [poses , landmarks]
  • [Eustice et al., 2008]
  • Underwater SLAM
  • State = [linear velocity, acceleration and angular rate]
  • Landmarks not saved in X
  • But: Image Registration used at every Iteration

11 / 66

slide-12
SLIDE 12

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Related Work

  • [This study]
  • Underwater SLAM
  • Stereo Camera System
  • Obtains 3D Environmental Information
  • AUV is moving in 3D
  • X = [poses]
  • Orientation is represented as a quaternion
  • Full 6 DOF Transformation
  • Different to [Burguera et al., 2014] (depth estimated by

pressure sensor)

  • Jacobian Matrices of 3D Transformation
  • Application of EKF to correct the localisation

12 / 66

slide-13
SLIDE 13

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

3D Transformation

13 / 66

slide-14
SLIDE 14

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

3D Transformation

  • Classical Transformation for 6 DOF
  • composition ⊕
  • inversion ⊖
  • Jacobian Matrices J⊕ and J⊖
  • Robot Transformation is non-linear
  • Direct Covariance computation in not possible
  • Approximation: Linearisation of transformation

functions

14 / 66

slide-15
SLIDE 15

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Composition ⊕

15 / 66

slide-16
SLIDE 16

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Composition ⊕

  • Adds a relative Transformation h to an absolute State X x
  • Result: New absolute pose X+ =

X t

+

X r

+

  • 16 / 66
slide-17
SLIDE 17

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Composition ⊕

  • Composition: X+ =

X t

+

X r

+

  • Quaternions (Orientation)
  • q =
  • qw

q1 q2 q3

  • faster computation
  • no trigonometric functions
  • no gimbal lock
  • Attention
  • Accumulation of Orientation = Multiplication of Quaternions
  • X r

+ = qT · qP

  • Quaternion to rotation Matrix A

17 / 66

slide-18
SLIDE 18

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Composition ⊕

  • Composition: X+ =

X t

+

X r

+

  • X t

+ =

    xP yP zP 1     + AP ·     xT yT zT 1    

18 / 66

slide-19
SLIDE 19

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Inversion ⊖

19 / 66

slide-20
SLIDE 20

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Inversion ⊖

  • Inverts a Transformation h
  • With ⊕ used to get relative Transformations from absolutes

20 / 66

slide-21
SLIDE 21

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Inversion ⊖

  • Task: Invert T =

x, y, z

t

qw, q1, q2, q3

  • A
  • n
  • a
  • p

A t 1

  • A

t 1 −1 =     − n ◦ p AT −

  • ◦

p − a ◦ p 1    

  • q−1 =

qw −q1 −q2 −q3

  • Result is

⊖X =     − n ◦ p −

  • ◦

p − a ◦ p q−1T    

21 / 66

slide-22
SLIDE 22

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Jacobian Matrices J1⊕, J2⊕ and J⊖

  • Necessary to compute the uncertainty
  • Apply: Taylor Series of first order

= Covariance: Uncertainty with zero mean random Gaussian noise

  • Jacobian for each Transformation ⊕ and ⊖
  • Jacobian Matrix in general
  • ∇f = ∂f

∂x |ˆ x

22 / 66

slide-23
SLIDE 23

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Jacobian Matrices J1⊕, J2⊕ and J⊖

  • J1⊕ and J2⊕
  • Composition ⊕ has two parameters (T and P)
  • Each: Jacobian Matrix of X+ ֒

→ J1⊕ and J2⊕

  • Covariance of Composition ⊕:

C+ = J1⊕ · C 1 · JT

1⊕ + J2⊕ · C 2 · JT 2⊕

23 / 66

slide-24
SLIDE 24

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Jacobian Matrices J1⊕, J2⊕ and J⊖

  • J⊖
  • Composition ⊖ has one parameter
  • Derivation will give us J⊖
  • Covariance of Inversion ⊖:

C− = J⊖ · C · JT

⊖

24 / 66

slide-25
SLIDE 25

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Image Registration

25 / 66

slide-26
SLIDE 26

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Image Registration

  • Result: 3D camera Transformation zk between two images
  • Images have to be overlapped
  • Detects Loop Closings: Update Stage (EKF)
  • Without: Trajectory cannot be updated

26 / 66

slide-27
SLIDE 27

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Pseudocode

27 / 66

slide-28
SLIDE 28

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

findFeature(Ii)

  • Important function
  • Reliable Feature are very important
  • SIFT Features
  • David G. Lowe (1999)
  • Scale invariant
  • Reliable
  • High reproducibility
  • Feature: 128 dimensional descriptor

28 / 66

slide-29
SLIDE 29

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

stereoMatching(Sl, Sr)

  • First: findFeature(Ii) with Sl, Sr
  • Comparing the squared differences of each descriptor
  • Differences reaches a certain treshold: Matched
  • Additional: Usage of RANSAC

29 / 66

slide-30
SLIDE 30

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Pseudocode

30 / 66

slide-31
SLIDE 31

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

calc3DPoints(Fl, Fr)

  • Result: 3D Points
  • Missing depth-value z can be calculated
  • Feature coordinates (x, y)
  • Reprojection Matrix Q

Q =     1 −Cx 1 −Cy fx − 1

Tx (Cx−Cx′) Tx

   

  • Cx and Cy optical center
  • fx focal length
  • Tx = baseline ·fx
  • Primed from left Camera, unprimed from right Camera

31 / 66

slide-32
SLIDE 32

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

calc3DPoints(Fl, Fr)

  • From 2D to 3D
  • Applied for each stereo Matching

    X Y Z W     = Q · 1     xl yl d 1    

  • d = xl − xr (disparity)

32 / 66

slide-33
SLIDE 33

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

solvePnPRansac(Ft, P3D)

  • Solves the Perspective N-Point Problem (PnP)
  • Estimates a pose transformation
  • Minimizes the Reprojection Error between
  • 3D Feature
  • corresponding 2D Feature
  • Result: 3D transformation [R, t]

33 / 66

slide-34
SLIDE 34

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

solvePnPRansac(Ft, P3D)

  • With respect to the Pseudocode
  • Sl is transformed into Ii (if overlap big enough)
  • Transformation is done in 3D

Figure: Left: Sl; middle: loop closing image Ii. On the right: the transformation of the image registration applied to Sl. The purple color indicates the error of the transformation.

34 / 66

slide-35
SLIDE 35

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

EKF-SLAM

35 / 66

slide-36
SLIDE 36

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

EKF-SLAM

  • EKF: Bayesian filter
  • State-Estimation of non-linear

system

  • Using normally distributed

Gaussian noise

  • Three Stages
  • 1. Prediction Stage
  • 2. Stage Augmentation Stage
  • 3. Update Stage

36 / 66

slide-37
SLIDE 37

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Prediction Stage

37 / 66

slide-38
SLIDE 38

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

getLastCovariance(C)

  • Takes the last 7 × 7 Matrix of C

C =                                         σ1

11

σ1

22

α σ1

33

σ1

44

σ1

55

β σ1

66

σ1

77

          A B C ... D E F           σn

11

σn

22

δ σn

33

σn

44

σn

55

γ σn

66

σn

77

                                       

38 / 66

slide-39
SLIDE 39

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

composition(Xt, Ct, O, Co)

  • Performs Composition ⊕
  • X+ = Xt ⊕ O
  • Calculates Covariance Matrix
  • C+ = J1⊕ · C t · JT

1⊕ + J2⊕ · C o · JT 2⊕

39 / 66

slide-40
SLIDE 40

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Augmentation Stage

40 / 66

slide-41
SLIDE 41

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

addCovariance(C, C +

t )

  • Not only adding (true for diagonal)

C =                                       σ1

11

σ1

22

α σ1

33

σ1

44

σ1

55

β σ1

66

σ1

77

         A B C ... D E F          σn

11

σn

22

σn

33

σn

44

σn

55

σn

66

σn

77

                                     

  • Except for diagonal
  • e.q. B and E are calculated
  • B = A · JT

1⊕

  • E = J1⊕ · C

41 / 66

slide-42
SLIDE 42

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Update Stage

  • Dependent on Image Registration
  • Main part of EKF
  • Executes Kalman Equations
  • Corrects State Vector
  • Key Features of this study:
  • Calculation of Observation Function h
  • Calculation of Observation Matrix H
  • Calculation of Innovation y

42 / 66

slide-43
SLIDE 43

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

calcHkK(X +, z)

  • observation function h
  • Based on z relative motions from X are calculated
  • hk = ⊖X k ⊕ X 2
  • Comparable hk (State Vector) and zk (Image Registration)
  • Multiple Loop Closings h =

     h1 h2 . . . hk     

43 / 66

slide-44
SLIDE 44

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

calcHkK(Sl, Sr, In)

  • observation matrix H
  • As many rows as Loop Closings (times 7)
  • As many columns as many states are stored in X + (times 7)
  • Stores Jacobian Matrices
  • Partially derivatives of h with respect to X +

H = ∂h ∂X +

  • ˆ

X +

  • Elements of H not referring to used states are 0

H =

  • ∂h1

∂X 2

. . .

∂h1 ∂X k ∂h2 ∂X 3

. . .

∂h2 ∂X k

  • 44 / 66
slide-45
SLIDE 45

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

innovation(h, z)

  • Innovation should be big if z and h are different
  • Innovation should be 0 if z and h are similar
  • In general: Difference between h and z
  • y = z − h
  • Translation: subtraction
  • Due to quaternions special treatment necessary

45 / 66

slide-46
SLIDE 46

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

innovation(h, z)

  • Different quaternions - similar orientation
  • qz = [0.996, −0.010, 0.014, 0.083] (1.55◦, −1.38◦, 9.50◦)
  • qh = [−0.996, −0.018, 0.001, −0.083] (0.04◦, 2.09◦, 9.55◦)
  • yq = qz − qh = [1.992, 0.007, 0.013, 0.166]
  • Solution: Absolute values
  • yq = |qz| − |qh|
  • yq = [0.0000, −0.0073, 0.0134, −0.0003]

46 / 66

slide-47
SLIDE 47

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Pseudocode

47 / 66

slide-48
SLIDE 48

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Results

48 / 66

slide-49
SLIDE 49

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Results

  • System Used
  • Laptop (Intel core i7 (2 2.9Ghz), 8GB RAM and SSD)
  • Ubuntu 12.04
  • MATLAB R2013a (single CPU core used)
  • The mission was recorded with ROS (Robot Operation System)
  • rosbag provided offline playback (recorded with Fugu-C)
  • Set-Up
  • Fugu-C (Bumblebee 2 1032 × 776 pixel)
  • Watertank inside the UIB (7m × 4m × 1.5m)
  • Visual Odometry calculated with LIBVISO2
  • Ground Truth: Seabed printed on a Poster

49 / 66

slide-50
SLIDE 50

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Results

  • Test
  • 23.42m sweeping task
  • Different noise levels
  • System-Response to less accurate Odometry
  • Error Definition:
  • Difference between Ground Truth
  • odometry
  • EKF-SLAM
  • Divided by the length of the Trajectory
  • Error units are meters per travelled meter

50 / 66

slide-51
SLIDE 51

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Results

  • Quantitative Results

Figure: Comparison between visual odometry and EKF-SLAM trajectory mean error (∅) with respect to the ground truth. Error is measured in meters per traveled meter.

51 / 66

slide-52
SLIDE 52

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Results

  • Quantitative Results

Figure: Comparison between state mean errors using raw odometry and EKF pose estimates. The standard deviation is set to 0.1σ.

52 / 66

slide-53
SLIDE 53

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Results

  • Quantitative Results

Figure: Comparison run time of different key-frame separations and error. Used noise level 2.

  • Separation of 4 already faster than Mission-Time

53 / 66

slide-54
SLIDE 54

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Results

  • Qualitative Results

Blue: Ground Truth, Black: Odometry, Red: EKF-SLAM

Figure: Example result with a noise level of two. Additionally the eight loop closings are plotted (magenta lines).

54 / 66

slide-55
SLIDE 55

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Results

  • Qualitative Results

Blue: Ground Truth, Black: Odometry, Red: EKF-SLAM

Figure: Example result with a noise level of four.

55 / 66

slide-56
SLIDE 56

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Results

  • Qualitative Results

Blue: Ground Truth, Black: Odometry, Red: EKF-SLAM

Figure: Example result with a noise level of six.

56 / 66

slide-57
SLIDE 57

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Results

  • Video
  • Separation of 2
  • Noise Level of 3
  • Playback 4x

57 / 66

slide-58
SLIDE 58

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Conclusion

58 / 66

slide-59
SLIDE 59

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Conclusion

  • Summary
  • Pose based visual EKF-SLAM approach
  • Underwater localization
  • Only Stereo Camera Data
  • Pure 6 Degrees of Freedom
  • Orientation is represented as quaternions
  • Improvement up to 86.1%
  • With Separation of 4 already Execution-Time under

Mission-Time

  • Future Work
  • Fine-Tuning: Measurement and Observation Covariance

Matrices

  • Further test in the sea
  • ROS implementation

59 / 66

slide-60
SLIDE 60

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Literature I

Burguera, A., Bonin-font, F., and Oliver, G. (2014). Towards Robust Image Registration for Underwater Visual SLAM. In International Conference on Computer Vision, Theory and Applications (VISSAP), Lisboa. Eustice, R. M., Pizarro, O., and Singh, H. (2008). Visually Augmented Navigation for Autonomous Underwater Vehicles. Ieee Journal Oceanic Engineering, 33:103–122. Schattschneider, R., Maurino, G., and Wang, W. (2011). Towards stereo vision SLAM based pose estimation for ship hull inspection. Oceans 2011, pages 1–8.

60 / 66

slide-61
SLIDE 61

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Autonomous Underwater Vehicles (AUVs)

  • Remotely Operated Vehicles (ROVs)
  • Tethered
  • Support Vessels
  • Limited operative range
  • Autonomous Underwater Vehicles
  • (Try to) Overcome this limitations
  • Highly repetitive, long or hazardous missions
  • Self-Powered
  • Independent (support ships and weather)
  • Reduction of
  • missions costs
  • human resources
  • execution time

61 / 66

slide-62
SLIDE 62

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

Applications

  • Maintenance
  • Rescue Operations
  • Surveying
  • Infrastructure Inspections
  • Sampling

62 / 66

slide-63
SLIDE 63

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion 63 / 66

slide-64
SLIDE 64

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

getLastState(X)

  • Takes the last 7 Elements of X

64 / 66

slide-65
SLIDE 65

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

innovation(h, z)

  • yq = qz − qh = [1.99274, 0.007344, 0.013427, 0.166257]
  • Pure subtraction: Big innovation (not right!)
  • Solution: Absolute values
  • yq = |qz| − |qh|
  • yq = [0.0000, −0.0073, 0.0134, −0.0003]

65 / 66

slide-66
SLIDE 66

Introduction 3D Transformation Image Registration Visual EKF-SLAM Results Conclusion

innovationCov(C +, H Cm)

  • S = H · C · HT + R
  • Measurement Matrix R
  • Size of R depends on number of detected Loop Closings

R =   Cm Cm Cm   (1)

66 / 66