Ian Mahon SLAM Summer School 2009

Size: px
Start display at page:

Download "Ian Mahon SLAM Summer School 2009"

Transcription

1 1 AUV SLAM SLAM Summer School January Ian Mahon Australian Centre for Field Robotics Slide 1

2 2 Overview Introduction Underwater navigation sensors The benefits of SLAM for underwater vehicles The Sirius AUV and its applications SLAM Information-form pose-based SLAM Recovering state estimates and covariances Generating loop-closure hypotheses Perception: stereo-vision loop-closure observations Motion estimation Outlier rejection SLAM demonstration ti Slide 2

3 3 Introduction Goal of the ACFR AUV project: Develop methods to collect high resolution, georeferenced datasets relatively quickly and cheaply to aid marine science Applications Biological surveys Geological surveys Marine archeology Currently our most frequently requested task is gathering data for the creation and monitoring of protected marine park areas. Slide 3

4 4 Sensors for Underwater Navigation Dead reckoning sensors such as Doppler Velocity Logs (DVL) and Inertial sensors are typical DVL provides po velocity eoc relative ea eto osea-floor in three axes, and contains tilt sensors and a compass RD Instruments Workhorse Navigator DVL Slide 4

5 5 Sensors for Underwater Navigation No GPS - signals absorbed by water A network of acoustic beacons can provide external e position o observations o s Limited range. The operating frequency controls a trade-off between range and resolution They take time to deploy and survey Due to occlusion and multi-path issues, their performance is limited when the vehicle is near the sea-floor, particularly in the structures such as reefs, ridges and canyons that are the locations AUV missions Slide 5

6 6 Why Use SLAM? Since our goal is collecting rich sensor data (images and sonar), it would be a waste not to try to use it to improve the navigation SLAM provides a source of dead-reckoning d drift correction SLAM works best near the sea-floor, in complex (informative) terrain, where the performance of acoustic networks is poor Since we make use of sensors on board the vehicle, we are not dependant on external infrastructure such as acoustic beacons We can operate relatively quickly and cheaply, performing three surveys in a day from a small support ship Slide 6

7 7 The Sirius AUV Slide 7

8 8 The Sirius AUV Computers Horizontal Thrusters Vertical Thruster Obstacle Avoidance Sonar Rear Strobe DVL Multibeam Sonar Conductivity, Temperature Depth, Chlorophyll, CDOM Batteries Stereo Cameras Front Strobe Slide 8

9 9 IMOS AUV Call for Proposals Undertaken Proposed Anticipated Slide 9

10 10 Surveys off Ningaloo Reef, Western Australia Invited by AIMS to undertake surveys near Exmouth WA Dense surveys targeting ti sponge dominated d habitats t based on Bathymetry and prior video transects Explored canyons in m depths Slide 10

11 11 Relic Reefs Surveys at the Great Barrier Reef Topas seismic profile at Hydrographers Passage (Image courtesy of K. Thornborough and P. Davies, USyd) Slide 11

12 12 Nocturnal Cuttlefish Camouflage Whyalla SA Ian Mahon Slide 12

13 13 AUV SLAM using Stereo-Vision Selection of a Framework Feature based Pose based The decision should be made based on the properties p of the sensor and feature extractor Properties of wide-baseline visual feature association algorithms (SIFT, SURF, MSER etc.): High precision (low numbers of false-positives) Low recall rates (low numbers of features are matched) Feature extraction and association may be slower than real-time Slide 13

14 14 AUV SLAM using Stereo-Vision Feature-based SLAM Out of the hundred or thousands of features extracted from an image, only a few will be associated during a loop closure event. Which features should be added to the state vector? Pose-based SLAM No need to predict which features will be matched Use whatever features are available to create a relative pose constraint A delayed observation can easily be applied to two previously augmented poses. Slide 14

15 15 Information-form Pose-based SLAM State vector Trajectory states Information form Current vehicle states Relationship Slide 15

16 16 Information-form Pose-based SLAM Sparsity of the information matrix Dead-reckoning with augmented poses Adding SLAM loop closure constraints Slide 16

17 17 Information-form Pose-based SLAM Prediction (with augmentation) Current vehicle state estimates are required The previous pose can be marginalised if it is not required Only keep poses that may be useful for loop- closures Slide 17

18 18 Information-form Pose-based SLAM Observation Innovation Update where Estimates for the observed states are required Slide 18

19 19 State Recovery Complete state recovery Solve the linear system: Solution with Cholesky decomposition: Recover x Recover P General system: Decompose A: Forward solve for Z: Lower triangular system Backward solve for X: Upper triangular system O(n 3 ) in general Variable ordering is important to reduce fill-in Slide 19

20 20 Cholesky Decomposition Natural Ordering Neighbours of the marginalised pose form a clique Fill-in shown in red Slide 20

21 21 Cholesky Decomposition AMD Ordering AMD order: 5,4,1,2,3 One block of fill-in compared to three blocks for natural ordering Slide 21

22 22 Cholesky Modifications Update an existing Cholesky decomposition of the information matrix, instead of recomputing it each time it is required (in this case each time an image is acquired) Original Updated Row addition Row removal Update Downdate Slide 22

23 23 Cholesky Modifications Which parts of the factorisation get modified? The columns corresponding to the modified variables, and any variables ordered after the modified variables Update example: Forward solve result esut Slide 23

24 24 Exact Partial Recovery Most common recovery operation is the vehicle states Most common observation operations are vehicle state t observations (depth, attitude, velocity etc.) Constrain the variable ordering to leave the current vehicle states last Backward solve: All states estimates change Only solve for the last block (the vehicle states): The result is constant time, exact vehicle state recovery Slide 24

25 25 Generating Loop Closure Hypotheses The joint distribution of pairs of poses is required Joint distribution of feature i and vehicle pose The use of previously recovered covariance matrices ti is a conservative strategy t Approximate (conservative) covariance Recovered from last column of the covariance matrix How should the conservative poses be updated? Slide 25

26 26 Pose Covariance Recovery The Sparse Inverse Takahashi's Equation Substitute the Cholesky decomposition into the inverse equation Upper triangular Recovering an element in the lower-triangle of the inverse To obtain the block diagonal (the pose covariances) the sparse inverse is recovered Less conservative poses means fewer loop closure hypotheses are generated (fewer image analysis operations) Slide 26

27 27 AUV SLAM Loop-closure Hypotheses Due to the down-looking cameras on the AUV, a set of pose pair candidated can found using geometry Simplifications No roll or pitch Flat terrian Images bounded by a circle Image overlap exists if Slide 27

28 28 AUV SLAM Loop-closure Hypotheses We need to calculate the likelihood of image overlap from the joint distribution of two poses Slide 28

29 29 Stereo Vision Relative Pose Estimator Aim: Estimate the change in the vehicle pose that has occurred between two sets of stereo image pairs to produce loop-closure observations for SLAM Requirements: Must be able to perform with small sets of features from images with low levels of overlap Must be capable of handling the location and association errors present in visual feature observations. Estimates must be suitable fusion in a SLAM framework Slide 29

30 30 Image Consistency Slide 30

31 31 The Relative Pose Estimation Process No need to reinvent the wheel For odometry: Lucas-Kanade tracker SSD, NCC etc. association For loop closing: SIFT, SURF, MSER etc. Slide 31

32 32 Motion Estimation Methods Relative pose estimation with 3D point registration Transformation: 3D registration error: Slide 32

33 33 Motion Estimation Methods Minimise Euclidean registration errors: Only relative pose states (not features) estimated Pose of b relative to a The uncertainty of triangulated features is ignored The uncertainty of triangulated features is not equal is all directions (uncertainty in depth is greatest) Results are noisy Fast (SVD) solution exists Useful for initial estimates for iterative optimisation methods Slide 33

34 34 Motion Estimation Methods Bundle Adjustment Maximum likelihood motion and structure estimation Relative pose states }Feature position states Observation is image coordinates of each feature in all four images } Pose a left camera Pose a right camera Predicted (reprojected) }image coords of feature i Pose b left camera Pose b right camera Slide 34

35 35 Motion Estimation Methods Bundle Adjustment (continued) Minimise image reprojection errors Taking advantage of sparse Jacobian allows efficient implementation Jacobian of predicted observation of feature i w.r.t pose states Jacobian of predicted observation of feature i w.r.t feature position states Slide 35

36 36 Motion Estimation Methods Efficient Maximum Likelihood 3D Registration Bundle adjustment is the standard to which approximate methods should be compared. Can we make a good approximation for stereo data? The triangulated feature positions provide an accurate, compact and convenient summary of the raw image coordinates Minimise the 3D registration error Mahalanobis distance Relative pose states }Feature position states Triangulated feature pos in frame a Triangulated feature pos in frame b Slide 36

37 37 Motion Estimation Methods Efficient ML 3D Registration (continued) Sparse structure like bundle adjustment Jacobian of predicted observation of feature i w.r.t pose states Jacobian of predicted observation of feature i w.r.t feature position states Accuracy equivalent to bundle adjustment (triangulated feature positions are a good summary of the raw image coordinate observations) Faster Predicted observation functions much simpler Compact observation vector - 6 states instead of 8 2 reference frames instead of 4 One transformation ti required, instead of four transformations ti and four image projection operations Slide 37

38 38 Outlier Rejection Epipolar Geometry A calibrated stereo-rig is used A feature in one image must be located on an epipolar line in the second image Only outliers with error perpendicular to the epipolar line can be rejected Slide 38

39 39 Outlier Rejection RANSAC Generate a number of motion hypotheses using a minimal set of samples Evaluate the hypothesis by counting the number of consistent features Number of hypotheses required to generate a set of samples only containing i inliers with confidence p m - number of hypotheses n - number of samples used to generate a hypothesis p - confidence of generating an inlier sample set α - fraction of outliers, initially set to a conservative value, and refined after each iteration Slide 39

40 40 Outlier Rejection RANSAC (continued) Mahalanobis distance inlier test: Problems: Even inliers have errors No minimal set is likely to produce the optimal motion parameters. Good estimates come from fusing lots of data Likely to accept less than the predicted ratio of inliers A better motion estimate is required to reject the outliers at the rate predicted by the Mahalanobis test Slide 40

41 41 Outlier Rejection Robust Estimation Least squares is highly sensitive to outliers M-estimators reduce the influence of outliers The ρ-function should increase less than least squares to reduce the influence of outliers A tuning parameter controls a trade-off between robustness and efficiency Huber Cauchy Fair Welsch Slide 41

42 42 Outlier Rejection Redescending example: Cauchy Non convex (redescending) Introduces local minima Good for outlier resistance Slide 42

43 43 Outlier Rejection Using redescending ρ-functions (local minima) Perform iterative optimisation using initial hypotheses generated by the RANSAC sampling procedure I assume that we can converge from the estimate produced by a minimal set of inliers Between selection of a ρ-function and tuning parameter, can t you rig whatever outcome you want? A concern when fitting a line to a single data set With appropriate simulation, many trials can be run to check the selection of parameters Image coordinates are limited to the size of the image. The errors are therefore bounded and can be easily simulated Slide 43

44 44 Outlier Rejection Loop-closure simulation 6 inliers 2 location outliers 2 association outliers Slide 44

45 45 Results Odometry comparison to DVL Slide 45

46 46 Results Odometry comparison to DVL Slide 46

47 47 SLAM Demo Slide 47

48 48 3D Terrain Models by Matthew Johnson-Roberson Slide 48

49 49 Conclusions SLAM has reached the state where it is possible to apply it to field robots performing useful tasks Status of SLAM in the ACFR AUV project: Loop-closures are not being applied to the estimator on-line on the vehicle A global optimisation method should be used instead of the iterative EIF approach to produce the optimal map after the mission have been performed Accurate localisation li is important t Maps we have built have been used to successfully select sampling locations for Brittle Stars observed in missions at the Great Barrier Reef Data gathered at relic reef sites will be used to determine sites for core drilling Slide 49

50 50 References Efficient view-based SLAM using visual loop closures I.J. Mahon, S.B. Williams, O. Pizarro and M. Johnson-Roberson, IEEE T-RO Vision-based Navigation for Autonomous Underwater Vehicles, Ian Mahon, PhD thesis. Simultaneous Localisation and Mapping and Dense Stereoscopic Seafoor Reconstruction using an AUV, Stefan B. Williams, Oscar Pizarro, Ian Mahon, Matthew Johnson-Roberson, ISER AUV-Assisted Surveying Of Relic Reef Sites, S.B. Williams, O. Pizarro, M. Johnson-Roberson, I. Mahon, J. Webster, R. Beaman,T. Bridge, OCEANS Slide 50

51 51 Resources sba sparse bundle adjustment library: SURF feature detection Original binary - Open-source implementation in OpenCV CHOLMOD - Sparse Cholesky decomposition and modifications edu/research/sparse/cholmod/ Slide 51

52 52 The End Slide 52

This is the author s version of a work that was submitted/accepted for publication in the following source:

This is the author s version of a work that was submitted/accepted for publication in the following source: This is the author s version of a work that was submitted/accepted for publication in the following source: Warren, Michael, Corke, Peter, Pizarro, Oscar, William, Stefan, & Upcroft, Ben (2012) Bi-objective

More information

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

Nonlinear State Estimation for Robotics and Computer Vision Applications: An Overview 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

More information

Multiview Stereo COSC450. Lecture 8

Multiview Stereo COSC450. Lecture 8 Multiview Stereo COSC450 Lecture 8 Stereo Vision So Far Stereo and epipolar geometry Fundamental matrix captures geometry 8-point algorithm Essential matrix with calibrated cameras 5-point algorithm Intersect

More information

Toward Real-Time Visually Augmented Navigation for Autonomous Search and Inspection of Ship Hulls and Port Facilities

Toward Real-Time Visually Augmented Navigation for Autonomous Search and Inspection of Ship Hulls and Port Facilities Toward Real-Time Visually Augmented Navigation for Autonomous Search and Inspection of Ship Hulls and Port Facilities Ryan M. Eustice Dept. of Naval Architecture & Marine Engineering University of Michigan

More information

Structure from motion

Structure from motion Structure from motion Structure from motion Given a set of corresponding points in two or more images, compute the camera parameters and the 3D point coordinates?? R 1,t 1 R 2,t 2 R 3,t 3 Camera 1 Camera

More information

Structure from motion

Structure from motion Structure from motion Structure from motion Given a set of corresponding points in two or more images, compute the camera parameters and the 3D point coordinates?? R 1,t 1 R 2,t R 2 3,t 3 Camera 1 Camera

More information

Davide Scaramuzza. University of Zurich

Davide Scaramuzza. University of Zurich Davide Scaramuzza University of Zurich Robotics and Perception Group http://rpg.ifi.uzh.ch/ Scaramuzza, D., Fraundorfer, F., Visual Odometry: Part I - The First 30 Years and Fundamentals, IEEE Robotics

More information

Structure from Motion. Introduction to Computer Vision CSE 152 Lecture 10

Structure from Motion. Introduction to Computer Vision CSE 152 Lecture 10 Structure from Motion CSE 152 Lecture 10 Announcements Homework 3 is due May 9, 11:59 PM Reading: Chapter 8: Structure from Motion Optional: Multiple View Geometry in Computer Vision, 2nd edition, Hartley

More information

SLAM with SIFT (aka Mobile Robot Localization and Mapping with Uncertainty using Scale-Invariant Visual Landmarks ) Se, Lowe, and Little

SLAM with SIFT (aka Mobile Robot Localization and Mapping with Uncertainty using Scale-Invariant Visual Landmarks ) Se, Lowe, and Little SLAM with SIFT (aka Mobile Robot Localization and Mapping with Uncertainty using Scale-Invariant Visual Landmarks ) Se, Lowe, and Little + Presented by Matt Loper CS296-3: Robot Learning and Autonomy Brown

More information

Application questions. Theoretical questions

Application questions. Theoretical questions The oral exam will last 30 minutes and will consist of one application question followed by two theoretical questions. Please find below a non exhaustive list of possible application questions. The list

More information

CS 395T Lecture 12: Feature Matching and Bundle Adjustment. Qixing Huang October 10 st 2018

CS 395T Lecture 12: Feature Matching and Bundle Adjustment. Qixing Huang October 10 st 2018 CS 395T Lecture 12: Feature Matching and Bundle Adjustment Qixing Huang October 10 st 2018 Lecture Overview Dense Feature Correspondences Bundle Adjustment in Structure-from-Motion Image Matching Algorithm

More information

Localization and Map Building

Localization and Map Building Localization and Map Building Noise and aliasing; odometric position estimation To localize or not to localize Belief representation Map representation Probabilistic map-based localization Other examples

More information

Long-term motion estimation from images

Long-term motion estimation from images Long-term motion estimation from images Dennis Strelow 1 and Sanjiv Singh 2 1 Google, Mountain View, CA, strelow@google.com 2 Carnegie Mellon University, Pittsburgh, PA, ssingh@cmu.edu Summary. Cameras

More information

CS 532: 3D Computer Vision 7 th Set of Notes

CS 532: 3D Computer Vision 7 th Set of Notes 1 CS 532: 3D Computer Vision 7 th Set of Notes Instructor: Philippos Mordohai Webpage: www.cs.stevens.edu/~mordohai E-mail: Philippos.Mordohai@stevens.edu Office: Lieb 215 Logistics No class on October

More information

Data Association for SLAM

Data Association for SLAM CALIFORNIA INSTITUTE OF TECHNOLOGY ME/CS 132a, Winter 2011 Lab #2 Due: Mar 10th, 2011 Part I Data Association for SLAM 1 Introduction For this part, you will experiment with a simulation of an EKF SLAM

More information

Alignment of Centimeter Scale Bathymetry using Six Degrees of Freedom

Alignment of Centimeter Scale Bathymetry using Six Degrees of Freedom Alignment of Centimeter Scale Bathymetry using Six Degrees of Freedom Ethan Slattery, University of California Santa Cruz Mentors: David Caress Summer 2018 Keywords: point-clouds, iterative closest point,

More information

Augmented Reality, Advanced SLAM, Applications

Augmented Reality, Advanced SLAM, Applications Augmented Reality, Advanced SLAM, Applications Prof. Didier Stricker & Dr. Alain Pagani alain.pagani@dfki.de Lecture 3D Computer Vision AR, SLAM, Applications 1 Introduction Previous lectures: Basics (camera,

More information

(W: 12:05-1:50, 50-N202)

(W: 12:05-1:50, 50-N202) 2016 School of Information Technology and Electrical Engineering at the University of Queensland Schedule of Events Week Date Lecture (W: 12:05-1:50, 50-N202) 1 27-Jul Introduction 2 Representing Position

More information

Bundle Adjustment in Large-Scale 3D Reconstructions based on Underwater Robotic Surveys

Bundle Adjustment in Large-Scale 3D Reconstructions based on Underwater Robotic Surveys Bundle Adjustment in Large-Scale 3D Reconstructions based on Underwater Robotic Surveys Chris Beall, Fran Dellaert, Ian Mahon, Stefan B. Williams College of Computing, Georgia Institute of Technology,

More information

Computer Vision I - Algorithms and Applications: Multi-View 3D reconstruction

Computer Vision I - Algorithms and Applications: Multi-View 3D reconstruction Computer Vision I - Algorithms and Applications: Multi-View 3D reconstruction Carsten Rother 09/12/2013 Computer Vision I: Multi-View 3D reconstruction Roadmap this lecture Computer Vision I: Multi-View

More information

Robot Mapping. Least Squares Approach to SLAM. Cyrill Stachniss

Robot Mapping. Least Squares Approach to SLAM. Cyrill Stachniss Robot Mapping Least Squares Approach to SLAM Cyrill Stachniss 1 Three Main SLAM Paradigms Kalman filter Particle filter Graphbased least squares approach to SLAM 2 Least Squares in General Approach for

More information

Graphbased. Kalman filter. Particle filter. Three Main SLAM Paradigms. Robot Mapping. Least Squares Approach to SLAM. Least Squares in General

Graphbased. Kalman filter. Particle filter. Three Main SLAM Paradigms. Robot Mapping. Least Squares Approach to SLAM. Least Squares in General Robot Mapping Three Main SLAM Paradigms Least Squares Approach to SLAM Kalman filter Particle filter Graphbased Cyrill Stachniss least squares approach to SLAM 1 2 Least Squares in General! Approach for

More information

Stereo and Epipolar geometry

Stereo and Epipolar geometry Previously Image Primitives (feature points, lines, contours) Today: Stereo and Epipolar geometry How to match primitives between two (multiple) views) Goals: 3D reconstruction, recognition Jana Kosecka

More information

Overview. EECS 124, UC Berkeley, Spring 2008 Lecture 23: Localization and Mapping. Statistical Models

Overview. EECS 124, UC Berkeley, Spring 2008 Lecture 23: Localization and Mapping. Statistical Models Introduction ti to Embedded dsystems EECS 124, UC Berkeley, Spring 2008 Lecture 23: Localization and Mapping Gabe Hoffmann Ph.D. Candidate, Aero/Astro Engineering Stanford University Statistical Models

More information

MTRX4700: Experimental Robotics

MTRX4700: Experimental Robotics Stefan B. Williams April, 2013 MTR4700: Experimental Robotics Assignment 3 Note: This assignment contributes 10% towards your final mark. This assignment is due on Friday, May 10 th during Week 9 before

More information

Robotic Perception and Action: Vehicle SLAM Assignment

Robotic Perception and Action: Vehicle SLAM Assignment Robotic Perception and Action: Vehicle SLAM Assignment Mariolino De Cecco Mariolino De Cecco, Mattia Tavernini 1 CONTENTS Vehicle SLAM Assignment Contents Assignment Scenario 3 Odometry Localization...........................................

More information

Direct Methods in Visual Odometry

Direct Methods in Visual Odometry Direct Methods in Visual Odometry July 24, 2017 Direct Methods in Visual Odometry July 24, 2017 1 / 47 Motivation for using Visual Odometry Wheel odometry is affected by wheel slip More accurate compared

More information

Towards Gaussian Multi-Robot SLAM for Underwater Robotics

Towards Gaussian Multi-Robot SLAM for Underwater Robotics Towards Gaussian Multi-Robot SLAM for Underwater Robotics Dave Kroetsch davek@alumni.uwaterloo.ca Christoper Clark cclark@mecheng1.uwaterloo.ca Lab for Autonomous and Intelligent Robotics University of

More information

A Systems View of Large- Scale 3D Reconstruction

A Systems View of Large- Scale 3D Reconstruction Lecture 23: A Systems View of Large- Scale 3D Reconstruction Visual Computing Systems Goals and motivation Construct a detailed 3D model of the world from unstructured photographs (e.g., Flickr, Facebook)

More information

BIL Computer Vision Apr 16, 2014

BIL Computer Vision Apr 16, 2014 BIL 719 - Computer Vision Apr 16, 2014 Binocular Stereo (cont d.), Structure from Motion Aykut Erdem Dept. of Computer Engineering Hacettepe University Slide credit: S. Lazebnik Basic stereo matching algorithm

More information

COS Lecture 13 Autonomous Robot Navigation

COS Lecture 13 Autonomous Robot Navigation COS 495 - Lecture 13 Autonomous Robot Navigation Instructor: Chris Clark Semester: Fall 2011 1 Figures courtesy of Siegwart & Nourbakhsh Control Structure Prior Knowledge Operator Commands Localization

More information

Structure from Motion CSC 767

Structure from Motion CSC 767 Structure from Motion CSC 767 Structure from motion Given a set of corresponding points in two or more images, compute the camera parameters and the 3D point coordinates?? R,t R 2,t 2 R 3,t 3 Camera??

More information

Dealing with Scale. Stephan Weiss Computer Vision Group NASA-JPL / CalTech

Dealing with Scale. Stephan Weiss Computer Vision Group NASA-JPL / CalTech Dealing with Scale Stephan Weiss Computer Vision Group NASA-JPL / CalTech Stephan.Weiss@ieee.org (c) 2013. Government sponsorship acknowledged. Outline Why care about size? The IMU as scale provider: The

More information

ICRA 2016 Tutorial on SLAM. Graph-Based SLAM and Sparsity. Cyrill Stachniss

ICRA 2016 Tutorial on SLAM. Graph-Based SLAM and Sparsity. Cyrill Stachniss ICRA 2016 Tutorial on SLAM Graph-Based SLAM and Sparsity Cyrill Stachniss 1 Graph-Based SLAM?? 2 Graph-Based SLAM?? SLAM = simultaneous localization and mapping 3 Graph-Based SLAM?? SLAM = simultaneous

More information

Master Automática y Robótica. Técnicas Avanzadas de Vision: Visual Odometry. by Pascual Campoy Computer Vision Group

Master Automática y Robótica. Técnicas Avanzadas de Vision: Visual Odometry. by Pascual Campoy Computer Vision Group Master Automática y Robótica Técnicas Avanzadas de Vision: by Pascual Campoy Computer Vision Group www.vision4uav.eu Centro de Automá

More information

Humanoid Robotics. Least Squares. Maren Bennewitz

Humanoid Robotics. Least Squares. Maren Bennewitz Humanoid Robotics Least Squares Maren Bennewitz Goal of This Lecture Introduction into least squares Use it yourself for odometry calibration, later in the lecture: camera and whole-body self-calibration

More information

CSE 527: Introduction to Computer Vision

CSE 527: Introduction to Computer Vision CSE 527: Introduction to Computer Vision Week 10 Class 2: Visual Odometry November 2nd, 2017 Today Visual Odometry Intro Algorithm SLAM Visual Odometry Input Output Images, Video Camera trajectory, motion

More information

3D Environment Reconstruction

3D Environment Reconstruction 3D Environment Reconstruction Using Modified Color ICP Algorithm by Fusion of a Camera and a 3D Laser Range Finder The 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems October 11-15,

More information

Computer Vision Lecture 17

Computer Vision Lecture 17 Computer Vision Lecture 17 Epipolar Geometry & Stereo Basics 13.01.2015 Bastian Leibe RWTH Aachen http://www.vision.rwth-aachen.de leibe@vision.rwth-aachen.de Announcements Seminar in the summer semester

More information

Computer Vision Lecture 17

Computer Vision Lecture 17 Announcements Computer Vision Lecture 17 Epipolar Geometry & Stereo Basics Seminar in the summer semester Current Topics in Computer Vision and Machine Learning Block seminar, presentations in 1 st week

More information

Epipolar Geometry CSE P576. Dr. Matthew Brown

Epipolar Geometry CSE P576. Dr. Matthew Brown Epipolar Geometry CSE P576 Dr. Matthew Brown Epipolar Geometry Epipolar Lines, Plane Constraint Fundamental Matrix, Linear solution + RANSAC Applications: Structure from Motion, Stereo [ Szeliski 11] 2

More information

Computational Optical Imaging - Optique Numerique. -- Single and Multiple View Geometry, Stereo matching --

Computational Optical Imaging - Optique Numerique. -- Single and Multiple View Geometry, Stereo matching -- Computational Optical Imaging - Optique Numerique -- Single and Multiple View Geometry, Stereo matching -- Autumn 2015 Ivo Ihrke with slides by Thorsten Thormaehlen Reminder: Feature Detection and Matching

More information

arxiv: v1 [cs.cv] 28 Sep 2018

arxiv: v1 [cs.cv] 28 Sep 2018 Camera Pose Estimation from Sequence of Calibrated Images arxiv:1809.11066v1 [cs.cv] 28 Sep 2018 Jacek Komorowski 1 and Przemyslaw Rokita 2 1 Maria Curie-Sklodowska University, Institute of Computer Science,

More information

W4. Perception & Situation Awareness & Decision making

W4. Perception & Situation Awareness & Decision making W4. Perception & Situation Awareness & Decision making Robot Perception for Dynamic environments: Outline & DP-Grids concept Dynamic Probabilistic Grids Bayesian Occupancy Filter concept Dynamic Probabilistic

More information

Monocular Visual-Inertial SLAM. Shaojie Shen Assistant Professor, HKUST Director, HKUST-DJI Joint Innovation Laboratory

Monocular Visual-Inertial SLAM. Shaojie Shen Assistant Professor, HKUST Director, HKUST-DJI Joint Innovation Laboratory Monocular Visual-Inertial SLAM Shaojie Shen Assistant Professor, HKUST Director, HKUST-DJI Joint Innovation Laboratory Why Monocular? Minimum structural requirements Widely available sensors Applications:

More information

Visual Odometry. Features, Tracking, Essential Matrix, and RANSAC. Stephan Weiss Computer Vision Group NASA-JPL / CalTech

Visual Odometry. Features, Tracking, Essential Matrix, and RANSAC. Stephan Weiss Computer Vision Group NASA-JPL / CalTech Visual Odometry Features, Tracking, Essential Matrix, and RANSAC Stephan Weiss Computer Vision Group NASA-JPL / CalTech Stephan.Weiss@ieee.org (c) 2013. Government sponsorship acknowledged. Outline The

More information

Real-Time Vision-Based State Estimation and (Dense) Mapping

Real-Time Vision-Based State Estimation and (Dense) Mapping Real-Time Vision-Based State Estimation and (Dense) Mapping Stefan Leutenegger IROS 2016 Workshop on State Estimation and Terrain Perception for All Terrain Mobile Robots The Perception-Action Cycle in

More information

An idea which can be used once is a trick. If it can be used more than once it becomes a method

An idea which can be used once is a trick. If it can be used more than once it becomes a method An idea which can be used once is a trick. If it can be used more than once it becomes a method - George Polya and Gabor Szego University of Texas at Arlington Rigid Body Transformations & Generalized

More information

Tightly-Integrated Visual and Inertial Navigation for Pinpoint Landing on Rugged Terrains

Tightly-Integrated Visual and Inertial Navigation for Pinpoint Landing on Rugged Terrains Tightly-Integrated Visual and Inertial Navigation for Pinpoint Landing on Rugged Terrains PhD student: Jeff DELAUNE ONERA Director: Guy LE BESNERAIS ONERA Advisors: Jean-Loup FARGES Clément BOURDARIAS

More information

Modelling and Simulation of the Autonomous Underwater Vehicle (AUV) Robot

Modelling and Simulation of the Autonomous Underwater Vehicle (AUV) Robot 21st International Congress on Modelling and Simulation, Gold Coast, Australia, 29 Nov to 4 Dec 2015 www.mssanz.org.au/modsim2015 Modelling and Simulation of the Autonomous Underwater Vehicle (AUV) Robot

More information

Mobile Robotics. Mathematics, Models, and Methods. HI Cambridge. Alonzo Kelly. Carnegie Mellon University UNIVERSITY PRESS

Mobile Robotics. Mathematics, Models, and Methods. HI Cambridge. Alonzo Kelly. Carnegie Mellon University UNIVERSITY PRESS Mobile Robotics Mathematics, Models, and Methods Alonzo Kelly Carnegie Mellon University HI Cambridge UNIVERSITY PRESS Contents Preface page xiii 1 Introduction 1 1.1 Applications of Mobile Robots 2 1.2

More information

CS231A Midterm Review. Friday 5/6/2016

CS231A Midterm Review. Friday 5/6/2016 CS231A Midterm Review Friday 5/6/2016 Outline General Logistics Camera Models Non-perspective cameras Calibration Single View Metrology Epipolar Geometry Structure from Motion Active Stereo and Volumetric

More information

Structure from Motion

Structure from Motion Structure from Motion Outline Bundle Adjustment Ambguities in Reconstruction Affine Factorization Extensions Structure from motion Recover both 3D scene geoemetry and camera positions SLAM: Simultaneous

More information

Localization, Where am I?

Localization, Where am I? 5.1 Localization, Where am I?? position Position Update (Estimation?) Encoder Prediction of Position (e.g. odometry) YES matched observations Map data base predicted position Matching Odometry, Dead Reckoning

More information

Structure from motion

Structure from motion Structure from motion Structure from motion Given a set of corresponding points in two or more images, compute the camera parameters and the 3D point coordinates?? R 1,t 1 R 2,t 2 R 3,t 3 Camera 1 Camera

More information

Index. 3D reconstruction, point algorithm, point algorithm, point algorithm, point algorithm, 263

Index. 3D reconstruction, point algorithm, point algorithm, point algorithm, point algorithm, 263 Index 3D reconstruction, 125 5+1-point algorithm, 284 5-point algorithm, 270 7-point algorithm, 265 8-point algorithm, 263 affine point, 45 affine transformation, 57 affine transformation group, 57 affine

More information

Colorado School of Mines. Computer Vision. Professor William Hoff Dept of Electrical Engineering &Computer Science.

Colorado School of Mines. Computer Vision. Professor William Hoff Dept of Electrical Engineering &Computer Science. Professor William Hoff Dept of Electrical Engineering &Computer Science http://inside.mines.edu/~whoff/ 1 Bundle Adjustment 2 Example Application A vehicle needs to map its environment that it is moving

More information

Visual SLAM. An Overview. L. Freda. ALCOR Lab DIAG University of Rome La Sapienza. May 3, 2016

Visual SLAM. An Overview. L. Freda. ALCOR Lab DIAG University of Rome La Sapienza. May 3, 2016 An Overview L. Freda ALCOR Lab DIAG University of Rome La Sapienza May 3, 2016 L. Freda (University of Rome La Sapienza ) Visual SLAM May 3, 2016 1 / 39 Outline 1 Introduction What is SLAM Motivations

More information

DYNAMIC POSITIONING CONFERENCE September 16-17, Sensors

DYNAMIC POSITIONING CONFERENCE September 16-17, Sensors DYNAMIC POSITIONING CONFERENCE September 16-17, 2003 Sensors An Integrated acoustic positioning and inertial navigation system Jan Erik Faugstadmo, Hans Petter Jacobsen Kongsberg Simrad, Norway Revisions

More information

L15. POSE-GRAPH SLAM. NA568 Mobile Robotics: Methods & Algorithms

L15. POSE-GRAPH SLAM. NA568 Mobile Robotics: Methods & Algorithms L15. POSE-GRAPH SLAM NA568 Mobile Robotics: Methods & Algorithms Today s Topic Nonlinear Least Squares Pose-Graph SLAM Incremental Smoothing and Mapping Feature-Based SLAM Filtering Problem: Motion Prediction

More information

Zürich. Roland Siegwart Margarita Chli Martin Rufli Davide Scaramuzza. ETH Master Course: L Autonomous Mobile Robots Summary

Zürich. Roland Siegwart Margarita Chli Martin Rufli Davide Scaramuzza. ETH Master Course: L Autonomous Mobile Robots Summary Roland Siegwart Margarita Chli Martin Rufli Davide Scaramuzza ETH Master Course: 151-0854-00L Autonomous Mobile Robots Summary 2 Lecture Overview Mobile Robot Control Scheme knowledge, data base mission

More information

L12. EKF-SLAM: PART II. NA568 Mobile Robotics: Methods & Algorithms

L12. EKF-SLAM: PART II. NA568 Mobile Robotics: Methods & Algorithms L12. EKF-SLAM: PART II NA568 Mobile Robotics: Methods & Algorithms Today s Lecture Feature-based EKF-SLAM Review Data Association Configuration Space Incremental ML (i.e., Nearest Neighbor) Joint Compatibility

More information

3D Model Acquisition by Tracking 2D Wireframes

3D Model Acquisition by Tracking 2D Wireframes 3D Model Acquisition by Tracking 2D Wireframes M. Brown, T. Drummond and R. Cipolla {96mab twd20 cipolla}@eng.cam.ac.uk Department of Engineering University of Cambridge Cambridge CB2 1PZ, UK Abstract

More information

Monocular Visual Odometry

Monocular Visual Odometry Elective in Robotics coordinator: Prof. Giuseppe Oriolo Monocular Visual Odometry (slides prepared by Luca Ricci) Monocular vs. Stereo: eamples from Nature Predator Predators eyes face forward. The field

More information

INS aided subsurface positioning for ROV surveys

INS aided subsurface positioning for ROV surveys INS aided subsurface positioning for ROV surveys M. van de Munt, Allseas Engineering B.V., The Netherlands R van der Velden, Allseas Engineering B.V., The Netherlands K. Epke, Allseas Engineering B.V.,

More information

Robot Mapping. SLAM Front-Ends. Cyrill Stachniss. Partial image courtesy: Edwin Olson 1

Robot Mapping. SLAM Front-Ends. Cyrill Stachniss. Partial image courtesy: Edwin Olson 1 Robot Mapping SLAM Front-Ends Cyrill Stachniss Partial image courtesy: Edwin Olson 1 Graph-Based SLAM Constraints connect the nodes through odometry and observations Robot pose Constraint 2 Graph-Based

More information

A single ping showing threshold and principal return. ReturnStrength Range(m) Sea King Feature Extraction. Y (m) X (m)

A single ping showing threshold and principal return. ReturnStrength Range(m) Sea King Feature Extraction. Y (m) X (m) Autonomous Underwater Simultaneous Localisation and Map Building Stefan B. Williams, Paul Newman, Gamini Dissanayake, Hugh Durrant-Whyte Australian Centre for Field Robotics Department of Mechanical and

More information

Index. 3D reconstruction, point algorithm, point algorithm, point algorithm, point algorithm, 253

Index. 3D reconstruction, point algorithm, point algorithm, point algorithm, point algorithm, 253 Index 3D reconstruction, 123 5+1-point algorithm, 274 5-point algorithm, 260 7-point algorithm, 255 8-point algorithm, 253 affine point, 43 affine transformation, 55 affine transformation group, 55 affine

More information

C280, Computer Vision

C280, Computer Vision C280, Computer Vision Prof. Trevor Darrell trevor@eecs.berkeley.edu Lecture 11: Structure from Motion Roadmap Previous: Image formation, filtering, local features, (Texture) Tues: Feature-based Alignment

More information

ME 597/747 Autonomous Mobile Robots. Mid Term Exam. Duration: 2 hour Total Marks: 100

ME 597/747 Autonomous Mobile Robots. Mid Term Exam. Duration: 2 hour Total Marks: 100 ME 597/747 Autonomous Mobile Robots Mid Term Exam Duration: 2 hour Total Marks: 100 Instructions: Read the exam carefully before starting. Equations are at the back, but they are NOT necessarily valid

More information

Uncertainties: Representation and Propagation & Line Extraction from Range data

Uncertainties: Representation and Propagation & Line Extraction from Range data 41 Uncertainties: Representation and Propagation & Line Extraction from Range data 42 Uncertainty Representation Section 4.1.3 of the book Sensing in the real world is always uncertain How can uncertainty

More information

Srikumar Ramalingam. Review. 3D Reconstruction. Pose Estimation Revisited. School of Computing University of Utah

Srikumar Ramalingam. Review. 3D Reconstruction. Pose Estimation Revisited. School of Computing University of Utah School of Computing University of Utah Presentation Outline 1 2 3 Forward Projection (Reminder) u v 1 KR ( I t ) X m Y m Z m 1 Backward Projection (Reminder) Q K 1 q Presentation Outline 1 2 3 Sample Problem

More information

COMPARATIVE STUDY OF DIFFERENT APPROACHES FOR EFFICIENT RECTIFICATION UNDER GENERAL MOTION

COMPARATIVE STUDY OF DIFFERENT APPROACHES FOR EFFICIENT RECTIFICATION UNDER GENERAL MOTION COMPARATIVE STUDY OF DIFFERENT APPROACHES FOR EFFICIENT RECTIFICATION UNDER GENERAL MOTION Mr.V.SRINIVASA RAO 1 Prof.A.SATYA KALYAN 2 DEPARTMENT OF COMPUTER SCIENCE AND ENGINEERING PRASAD V POTLURI SIDDHARTHA

More information

Multibody reconstruction of the dynamic scene surrounding a vehicle using a wide baseline and multifocal stereo system

Multibody reconstruction of the dynamic scene surrounding a vehicle using a wide baseline and multifocal stereo system Multibody reconstruction of the dynamic scene surrounding a vehicle using a wide baseline and multifocal stereo system Laurent Mennillo 1,2, Éric Royer1, Frédéric Mondot 2, Johann Mousain 2, Michel Dhome

More information

Structure from Motion

Structure from Motion /8/ Structure from Motion Computer Vision CS 43, Brown James Hays Many slides adapted from Derek Hoiem, Lana Lazebnik, Silvio Saverese, Steve Seitz, and Martial Hebert This class: structure from motion

More information

Simultaneous Localization and Mapping (SLAM)

Simultaneous Localization and Mapping (SLAM) Simultaneous Localization and Mapping (SLAM) RSS Lecture 16 April 8, 2013 Prof. Teller Text: Siegwart and Nourbakhsh S. 5.8 SLAM Problem Statement Inputs: No external coordinate reference Time series of

More information

Robust Geometry Estimation from two Images

Robust Geometry Estimation from two Images Robust Geometry Estimation from two Images Carsten Rother 09/12/2016 Computer Vision I: Image Formation Process Roadmap for next four lectures Computer Vision I: Image Formation Process 09/12/2016 2 Appearance-based

More information

Last lecture. Passive Stereo Spacetime Stereo

Last lecture. Passive Stereo Spacetime Stereo Last lecture Passive Stereo Spacetime Stereo Today Structure from Motion: Given pixel correspondences, how to compute 3D structure and camera motion? Slides stolen from Prof Yungyu Chuang Epipolar geometry

More information

Independently Moving Objects Detection based on Stereo Visual Odometry

Independently Moving Objects Detection based on Stereo Visual Odometry Independently Moving Objects Detection based on Stereo Visual Odometry Abaabu Amina Instituto Superior Técnico, Lisbon, Portugal ist179454@mail.ist.utl.pt Abstract In this project we propose an approach

More information

Motion estimation of unmanned marine vehicles Massimo Caccia

Motion estimation of unmanned marine vehicles Massimo Caccia Motion estimation of unmanned marine vehicles Massimo Caccia Consiglio Nazionale delle Ricerche Istituto di Studi sui Sistemi Intelligenti per l Automazione Via Amendola 122 D/O, 70126, Bari, Italy massimo.caccia@ge.issia.cnr.it

More information

Autonomous Mobile Robot Design

Autonomous Mobile Robot Design Autonomous Mobile Robot Design Topic: EKF-based SLAM Dr. Kostas Alexis (CSE) These slides have partially relied on the course of C. Stachniss, Robot Mapping - WS 2013/14 Autonomous Robot Challenges Where

More information

Autonomous Navigation in Complex Indoor and Outdoor Environments with Micro Aerial Vehicles

Autonomous Navigation in Complex Indoor and Outdoor Environments with Micro Aerial Vehicles Autonomous Navigation in Complex Indoor and Outdoor Environments with Micro Aerial Vehicles Shaojie Shen Dept. of Electrical and Systems Engineering & GRASP Lab, University of Pennsylvania Committee: Daniel

More information

ORB SLAM 2 : an Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras

ORB SLAM 2 : an Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras ORB SLAM 2 : an OpenSource SLAM System for Monocular, Stereo and RGBD Cameras Raul urartal and Juan D. Tardos Presented by: Xiaoyu Zhou Bolun Zhang Akshaya Purohit Lenord Melvix 1 Outline Background Introduction

More information

LUMS Mine Detector Project

LUMS Mine Detector Project LUMS Mine Detector Project Using visual information to control a robot (Hutchinson et al. 1996). Vision may or may not be used in the feedback loop. Visual (image based) features such as points, lines

More information

Vision-based Localization of an Underwater Robot in a Structured Environment

Vision-based Localization of an Underwater Robot in a Structured Environment Vision-based Localization of an Underwater Robot in a Structured Environment M. Carreras, P. Ridao, R. Garcia and T. Nicosevici Institute of Informatics and Applications University of Girona Campus Montilivi,

More information

APPLICATIONS AND CHALLENGES FOR UNDERWATER SWIMMING MANIPULATORS

APPLICATIONS AND CHALLENGES FOR UNDERWATER SWIMMING MANIPULATORS APPLICATIONS AND CHALLENGES FOR UNDERWATER SWIMMING MANIPULATORS Jørgen Sverdrup-Thygeson AMOS Days October 2017 Introduction NEEDS FOR SUBSEA INSPECTION, MAINTENANCE AND REPAIR The number of new subsea

More information

Revising Stereo Vision Maps in Particle Filter Based SLAM using Localisation Confidence and Sample History

Revising Stereo Vision Maps in Particle Filter Based SLAM using Localisation Confidence and Sample History Revising Stereo Vision Maps in Particle Filter Based SLAM using Localisation Confidence and Sample History Simon Thompson and Satoshi Kagami Digital Human Research Center National Institute of Advanced

More information

Outline. Data Association Scenarios. Data Association Scenarios. Data Association Scenarios

Outline. Data Association Scenarios. Data Association Scenarios. Data Association Scenarios Outline Data Association Scenarios Track Filtering and Gating Global Nearest Neighbor (GNN) Review: Linear Assignment Problem Murthy s k-best Assignments Algorithm Probabilistic Data Association (PDAF)

More information

Computational Optical Imaging - Optique Numerique. -- Multiple View Geometry and Stereo --

Computational Optical Imaging - Optique Numerique. -- Multiple View Geometry and Stereo -- Computational Optical Imaging - Optique Numerique -- Multiple View Geometry and Stereo -- Winter 2013 Ivo Ihrke with slides by Thorsten Thormaehlen Feature Detection and Matching Wide-Baseline-Matching

More information

Simultaneous Localization

Simultaneous Localization Simultaneous Localization and Mapping (SLAM) RSS Technical Lecture 16 April 9, 2012 Prof. Teller Text: Siegwart and Nourbakhsh S. 5.8 Navigation Overview Where am I? Where am I going? Localization Assumed

More information

CS 4495 Computer Vision A. Bobick. Motion and Optic Flow. Stereo Matching

CS 4495 Computer Vision A. Bobick. Motion and Optic Flow. Stereo Matching Stereo Matching Fundamental matrix Let p be a point in left image, p in right image l l Epipolar relation p maps to epipolar line l p maps to epipolar line l p p Epipolar mapping described by a 3x3 matrix

More information

UAV Autonomous Navigation in a GPS-limited Urban Environment

UAV Autonomous Navigation in a GPS-limited Urban Environment UAV Autonomous Navigation in a GPS-limited Urban Environment Yoko Watanabe DCSD/CDIN JSO-Aerial Robotics 2014/10/02-03 Introduction 2 Global objective Development of a UAV onboard system to maintain flight

More information

Mul$-objec$ve Visual Odometry Hsiang-Jen (Johnny) Chien and Reinhard Kle=e

Mul$-objec$ve Visual Odometry Hsiang-Jen (Johnny) Chien and Reinhard Kle=e Mul$-objec$ve Visual Odometry Hsiang-Jen (Johnny) Chien and Reinhard Kle=e Centre for Robo+cs & Vision Dept. of Electronic and Electric Engineering School of Engineering, Computer, and Mathema+cal Sciences

More information

CS5670: Computer Vision

CS5670: Computer Vision CS5670: Computer Vision Noah Snavely, Zhengqi Li Stereo Single image stereogram, by Niklas Een Mark Twain at Pool Table", no date, UCR Museum of Photography Stereo Given two images from different viewpoints

More information

Stereo Vision. MAN-522 Computer Vision

Stereo Vision. MAN-522 Computer Vision Stereo Vision MAN-522 Computer Vision What is the goal of stereo vision? The recovery of the 3D structure of a scene using two or more images of the 3D scene, each acquired from a different viewpoint in

More information

A Summary of Projective Geometry

A Summary of Projective Geometry A Summary of Projective Geometry Copyright 22 Acuity Technologies Inc. In the last years a unified approach to creating D models from multiple images has been developed by Beardsley[],Hartley[4,5,9],Torr[,6]

More information

CSCI 5980/8980: Assignment #4. Fundamental Matrix

CSCI 5980/8980: Assignment #4. Fundamental Matrix Submission CSCI 598/898: Assignment #4 Assignment due: March 23 Individual assignment. Write-up submission format: a single PDF up to 5 pages (more than 5 page assignment will be automatically returned.).

More information

Structure from Motion

Structure from Motion 11/18/11 Structure from Motion Computer Vision CS 143, Brown James Hays Many slides adapted from Derek Hoiem, Lana Lazebnik, Silvio Saverese, Steve Seitz, and Martial Hebert This class: structure from

More information

Fundamental matrix. Let p be a point in left image, p in right image. Epipolar relation. Epipolar mapping described by a 3x3 matrix F

Fundamental matrix. Let p be a point in left image, p in right image. Epipolar relation. Epipolar mapping described by a 3x3 matrix F Fundamental matrix Let p be a point in left image, p in right image l l Epipolar relation p maps to epipolar line l p maps to epipolar line l p p Epipolar mapping described by a 3x3 matrix F Fundamental

More information

Selection and Integration of Sensors Alex Spitzer 11/23/14

Selection and Integration of Sensors Alex Spitzer 11/23/14 Selection and Integration of Sensors Alex Spitzer aes368@cornell.edu 11/23/14 Sensors Perception of the outside world Cameras, DVL, Sonar, Pressure Accelerometers, Gyroscopes, Magnetometers Position vs

More information