This week at Oceanology Americas we presented a paper on SLAM and Optimal Sensor Fusion and outlined how we have implemented this within our real-time navigation and 3D reconstruction tool, 3D Recon. We have just assembled two 4,000m rated 3D Recon systems. One of these systems is currently undergoing pressure cycle testing while the other is undergoing extensive burn-in testing to ensure long term viability. We expect to have test tank data later in March, so if you'd like to receive some sample data sets please let us know at sales@zupt.com.

1. SLAM and Optimal Sensor Fusion
For Subsea Model Generation and Precise Positioning
w w w . Z u p t . c o m

2. SIMULTANEOUS LOCALIZATION AND MAPPING
An Introduction with Minimal Equations
Motivation?
• We need a way to navigate accurately in GPS denied environments – SLAM lets us do this.
• We may want to navigate where no external sensors are available.
• SLAM lets us explore previously unknown environments autonomously.
2

3. APPLICATIONS
Search and Rescue
3
Widely used today in autonomous vehicle applications – in air
Simple versions used subsea (SLAM to calibrate a LBL beacon)
Air Space Subsea

4. CURRENT EXAMPLES IN OUR INDUSTRY
LBL SLAM
Given:
Inertial, USBL, LBL ranges, depth
Find:
ROV position, velocity, attitude, AND the
locations of unknown transponders.
Map = location of unknown transponder
Localization = ROV position, velocity, and
attitude
4
Stereo Inertial SLAM
Given:
Inertial, imagery (USBL, LBL ranges, depth also
possible inputs)
Find:
ROV position, velocity, attitude, and the locations
of millions of 3D triangulated features.
Map = location of millions of unique
environmental features detected
Localization = ROV position, velocity, and attitude

5. Mapping LocalizationSLAM
WHAT IS SLAM?
SLAM provides the ability to position us while developing knowledge of the environment around us.
Where is the world
around me?
Where am I?
5

6. PROBLEM STATEMENT
What do we know (Given):
Observations - z1,z2,…,zn (sensor information about the world)
Control Input - u1,u2,…,un (information about trajectory changes)
What do we want to know (Find):
Vehicle state - position, velocity, attitude, sensor related error terms
Map state - global feature locations. LBL beacon location, bathymetry or a high resolution model of
our surroundings could be a map state.

7. SLAM PROCESS: INITIALIZATION
• Choose a global frame
• Small initial uncertainty
• Sensor measurements initialize
landmarks
• Sensor could be range info, camera
image, sonar or LiDAR)
Airborne UAV as an example
7

8. SLAM PROCESS: PROPAGATION
• UAV is moving
• Dynamic models estimate the new
location
• But - uncertainty increases
8

9. SLAM PROCESS:
U P D A T I N G A N D T H E N I N I T I A L I Z I N G N E W L A N D M A R K S
• Data association matches previous landmarks
• Uncertainty is decreased
• New landmarks are added
9

10. EKF SLAM: THE CLASSICAL APPROACH
Extended Kalman Filter - SLAM
• Joint State Representation (vehicle and map states are estimated together)
• Only suitable for small map sizes
• Blue “map” states - get really big.
• Taylor series linearizations introduce error
• (if in prop or update phase if the equations are non linear – then represented by Taylor series (sqd, or sqrt)
Single hypothesis data association – can introduce matching error mismatches
10

11. UKF SLAM
• Unscented Kalman Filter SLAM
• Propagate all points – new mean – new covariance
• More accurate when motion and measurement
models are nonlinear
• Easy to implement
• Still scales poorly for large maps
• Still a single hypothesis
11

12. SPARSE EXTENDED INFORMATION FILTERING (SEIF)
Uses inverse of the covariance matrix – so off diag’s get small and can be reduced to zero.
Computational efficiency is increased at the expense of accuracy.
Single hypothesis data association is still an issue.
12

13. SUB MAPPING
Partition into small “sub-maps” – smaller covariance matrix –
only uses features we are currently seeing.
Sets of “sub-maps” are used to update a global map.
Full map correlations are not maintained at every step
EKF is used for “sub-maps”
13

14. FASTSLAM – THE PARTICLE FILTER APPROACH
Vehicle state is represented by discrete samples
Get rid of full covariance –each particle (vehicle state) believes
it is good – but…
Each sample has N Kalman filters attached to it (one for each
landmark) only updated when we have a measurement for
them. Map states independent of one another.
Weights are generated according to an importance function –
measurement innovation/innovation covariance = C-O used to
generate weights
Multi-hypothesis data association
Allows for large Map sizes because features only need to be
updated when they are seen
14

15. OPTIMIZATION BASED SLAM
Maximize likelihood of joint state
Graph SLAM - minimize a graph of constraints built upon vehicle motion and sensor data.
Bundle Adjustment - minimize reprojection error from different viewpoints.
• Can produce accurate results
• Cost of performing optimization is high
15

16. REVIEW OF THESE VARIOUS SLAM ALGORITHMS
Kalman Filter Based:
• EKF and UKF do not work for large maps
• Sub mapping and SEIF reduce computations at the expense of accuracy.
FastSLAM:
• Multi-hypothesis data association
• Works well for large scale maps
Optimization Based:
• Can produce accurate results
• Optimization is costly
16

17. VISUAL INERTIAL SLAM
A stereo/multi camera pair, lights, acquisition/logging system and INS. Fiber to the surface
for any more than 8 hours of data collection. Self contained data logging for up to 8 hours.
17

18. 18
3D RECON – THE BASICS
• Calibration
• Image Acquisition
• Feature Detection
• Feature Matching –feature
descriptors – stereo depth estimation
• Camera reference frame to earth
reference frame – bundle adjustment
– Fast SLAM – Sparse Reconstruction
• Dense Reconstruction – disparity
used to generate dense point cloud
• 3D model in global frame

19. VISUAL-INERTIAL SLAM
O U R I M P L E M E N T A T I O N O F A S U B S E A S L A M S O L U T I O N
Inertial Propagation
Global Feature Matching
SLAM Updates and
Feature Initialization
3D model in
global frame
Vehicle position,
attitude and velocity
Stereo Rectification
Feature Detection
and Description
Local Matching
Triangulation
Lever Arm Adjustments
Imaging Sensors IMU
Dense Stereo Matching
Sparse point cloud with descriptors
Dense Point Cloud
Dense Model Refinement

20. 3D RECON
3D recon is a self contained tool that delivers high
resolution, geospatially correct, 3D models of what
we image subsea.
3D Recon knows where it is with respect to the
features it uses to build the model. 3D Recon is also
a relative positioning system. 3D Recon navigates in
real time while simultaneously building the 3D
model.
3D Recon will not only build the model – but will
allow you to very precisely navigate within the
model - so you can navigate the last 3m, 2m, 5cm
or even 1cm to the defined feature you want to go to
on a structure, within your model.
20

21. 3D RECON - CALIBRATION
We use a checkerboard calibration plate for our
calibration process.
Individual camera calibration technique is complete
in both Matlab and OpenCV. A two component
radial distortion model has the lowest re-projection
error.
Camera to camera calibration is also complete.
The Camera to IMU calibration is complete within
Matlab. We use a single Kalman filter that ends up
with a 183 dimension state vector based on our
checker board.
21
Calibration includes - individual camera, camera
to camera, camera baseline to IMU

22. FEATURE DETECTION AND DESCRIPTION
22
We have included options for very standard machine vision feature detection techniques - ORB,
SIFT, SURF. We have concluded our real time feature detection, feature matching, and triangulation
software development.

23. FEATURE MATCHING
23

24. DISPARITY COMPUTATION
D I F F E R E N C E I N X C O O R D I N A T E ( D E P T H ) I N B O T H I M A G E S
24
Disparity to XYZ Example Calculation:

25. POINT CLOUD GENERATION (SPARSE AND DENSE)
25

26. AUTOMATED DATA CLEANING
26

27. SIMULTANEOUS LOCALIZATION AND MAPPING
27
Sparse SLAM Map and Vehicle Poses Dense Point Cloud Projection using SLAM Poses
Each feature point has XYZ, RBG+descriptor.
Descriptor distance+XYZ distance are used
for global matching for SLAM updates
Each feature point has only XYZ,RBG. Down-
sampling and refinements are made to
further align projected point clouds.

28. DE-NOISING AND MESH GENERATION
28

29. ANALYZE STRUCTURE DEPTH
29

30. IMPROVING POSITIONING – SLAM AND BUNDLE ADJUSTMENT
The navigation solution feeds the image processing and the knowledge gained from the feature matches
improves the navigation – this is enabled within classic Simultaneous Localization and Mapping
(SLAM)/Bundle Adjustment routines. This leads to very repeatable survey data.
The yellow data incorrectly positioned in the left image is used to correctly combine and match (position)
the data in the right image. The same feature now resides in the same geospatial location.
30

31. ACCURACY OF THIS DATA SET <+/-2MM
31

32. APPLICATIONS FOR 3D RECON
Pipeline surveys – High resolution free span data, anode depletion volumes possible
Metrology – Delivers metrology level accuracy, 30mm over 30m (1/1000)
As Built – Delivers exactly what is on the seabed and exactly where it is – import into operator GIS
Dimensional Control at Depth – Structure modeling and subsea offset determination
Under Hull / SPAR Positioning – Tricky to position free moving targets in the water column.
Asset Integrity Monitoring – Automated change detection, position and feature definition.
Augmented Reality (valve ops / structure ID) – Identify a feature, automatically display metadata and
automatically navigate to that specific feature.
Precisely Navigate the last 3m to a known feature
32
Dimensions:
588mm x 100mm x 100mm

33. EMAIL
kv@zupt.com
Contact Info
PHONE
+1 832 295 7280
WEBSITE
www.zupt.com