Pose tracking of magnetic objects

Pose tracking of magnetic objects Niklas Wahlström Department of Information Technology, Uppsala University, Sweden Novmber 13, 2017 niklas.wahlstrom@it.uu.se Seminar Vi2

Short about me 2005-2010: Applied Physics and Electrical Engineering - International, Linköping University. 2007-2008: Exchange student, ETH Zürich, Swizerland 2010-2015 : PhD student in Automatic Control, Linköping University Spring 2014, Research visit, Imperial College, London, UK 2016- : Researcher at Department of Information Technology, Uppsala University 1 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

My thesis and my work at Uppsala Three areas: Magnetic tracking and mapping Extended target tracking Deep dynamical models for control Two areas: Constrained Gaussian processes Deep learning and system identification 2 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Magnetometer measurement models 1. Common use: Magnetometer provides orientation heading information. Assume that the magnetometer (almost) only measures the local (earth) magnetic field. 2. My use: Magnetometer(s) to provide position and orientation information. Magnetic tracking: Measure the position and orientation of a known magnetic source. 3 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Sensor setup We use a sensor network with four three-axis magnetometers to determine the position and orientation of a magnet. 4 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Magnetic tracking Advantages Cheap sensors 5 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Magnetic tracking Advantages Cheap sensors Small sensors 5 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Magnetic tracking Advantages Cheap sensors Small sensors Low energy consumption 5 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Magnetic tracking Advantages Cheap sensors Small sensors Low energy consumption No weather dependency 5 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Magnetic tracking Advantages Cheap sensors Small sensors Low energy consumption No weather dependency Passive unit, requires no batteries 5 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Mathematical model - dipole field The magnetic field can be described with a dipole field. B(r) r m J(r ) B(r) = µ ( ) 0 4π r 5 3r r T r 2 I 3 m }{{} =C(r) m 1 2 r J(r )d 3 r 6 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Sensor model - single dipole The measurements can be described with a state-space model x k+1 = F k x k + G k w k, w k N (0, Q), y k,j = h j (x k ) + e k, e k N (0, R) Point target sensor model (one dipole) h j (x k ) = C(r k θ j )m k, x k = [r T k v T k m T k ω T k ]T C(r) = µ 0 4π r 5 (3rrT r 2 I 3 ), Measurement from a sensor network of magnetometers positioned at {θ j } J j=1. Degrees of freedom 3D position 2D orientation 7 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

z-coordinate [m] z-coordinate [m] Experiment 1 Trajectory 0.3 Trajectory 0.25 0.3 0.2 0.25 0.2 0.15 0.15 0.1 0.1 0.05 0.05 0-0.05-0.1-0.2 0.2 0.1 0-0.1 0-0.1 0.1 y-coordinate [m] x-coordinate [m] 0.2-0.2 0-0.05-0.1-0.2-0.15-0.1-0.05 0 0.05 0.1 0.15 0.2 y-coordinate [m] accuracy is 8 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Experiment 2 - setup 9 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Experiment 2 - results - position 0.3 0.2 Position [m] 0.1 0 0.1 0.2 20 25 30 35 40 45 50 Time [s] Black: Ground truth position. Color: Estimated position 10 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Experiment - results - orientation 100 Orientation [degree] 0 100 200 20 25 30 35 40 45 50 Time [s] Black: Ground truth orientation. Color: Estimated orientation 11 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Application 1: 3D painting book 12 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Application 2: Digital water colors 13 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Application 3: Digital table hockey 14 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Application 4: Digital pathology 15 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Stylaero AB In February in this year a company was started around this technology The areas the company has so far one employee. Collaborations with gaming companied and industrial partners have been initiated. 16 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Thank you! 17 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Sensor model - multi-dipole The measurements can be described with a state-space model x k+1 = F k x k + G k w k, w k N (0, Q), y k,j = h j (x k ) + e k, e k N (0, R) Extended target sensor model (a structure of dipoles) L h j (x k ) = C(r k + R k (q k )s l θ j )m l R k (q k )b l, l=1 x k = [r T k v T k q T k ω T k ]T y z x s 1 s2 b 1 b 2 Degrees of freedom 3D position 3D orientation 19 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Magnetic mapping Build a map of the indoor magnetic field. This map can be used for localization. We want a statistical model of the magnetic field - Gaussian processes! 20 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Gaussian processes Gaussian processes can be seed as a distribution over functions f(u) GP ( µ(u), K(u, u ) ), Mean function Covariance function It is a generalization of the multivariate Gaussian distribution f(u 1 ) µ(u 1 ). N (µ, K), where µ =., f(u N ) µ(u N ) K(u 1, u 1 ) K(u 1, u N ) K =... K(u N, u 1 ) K(u N, u N ) 21 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Gaussian process regression Objective: Estimate f(u) from noisy observations y k = f(u k ) + e k 1.5 1 0.5 f(u) 0 0.5 1 1.5 3 2 1 0 u 1 2 3 22 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Modeling the magnetic field The magnetic field H is curl-free, i.e. H = 0 [1] y k = f(x k ) + ε k f(x) GP(0, σ 2 const.i 3 + K curl (x, x )) [1] Niklas Wahlström, Manon Kok, Thomas B. Schön and Fredrik Gustafsson, Modeling magnetic fields using Gaussian processes The 38th International Conference on Acoustics, Speech, and Signal Processing (ICASSP), Vancouver, Canada, May 2013. 23 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Modeling the magnetic field The magnetic field H is curl-free, i.e. H = 0 [1] y k = f(x k ) + ε k f(x) GP(0, σ 2 const.i 3 + K curl (x, x )) If a vector-field is curl-free, a scalar potential ϕ exists H = ϕ [2] y k = ϕ(x) x=xi + ε k ϕ(x) GP ( 0, k lin. (x, x ) + k SE (x, x ) ) [1] Niklas Wahlström, Manon Kok, Thomas B. Schön and Fredrik Gustafsson, Modeling magnetic fields using Gaussian processes The 38th International Conference on Acoustics, Speech, and Signal Processing (ICASSP), Vancouver, Canada, May 2013. [2] Arno Solin, Manon Kok, Niklas Wahlström, Thomas B. Schön and Simo Särkkä, Modeling and interpolation of the ambient magnetic field by Gaussian processes IEEE Transactions on Robotics, 2017. Accepted. 23 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Experiment Build a map of the indoor magnetic field using Gaussian processes. 24 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Experiment Build a map of the indoor magnetic field using Gaussian processes. Optical marker Smartphone Invensense IMU Trivisio IMU DiddyBorg robot board 24 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Experiment Build a map of the indoor magnetic field using Gaussian processes. https://www.youtube.com/watch?v=enlmiuqpvjo 24 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Reduced-Rank GPR Hilbert-space approximation of the covariance operator in terms of an eigenfunction expansion of the Laplace operator in a compact subset of R d. Assume that the measurements are confined to a certain domain. Approximate the covariance using the spectral density and a number of eigenvalues and eigenfunctions. For d = 1: m k(x, x ) S(λ j )φ j (x)φ j (x ) j=1 ( ) φ j (x) = 1 πnj (x+l) L sin 2L, λ j = πj 2L, Converges to the true GP when the number of basis functions and the size of the domain goes to infinity. Hilbert Space Methods for Reduced-Rank Gaussian Process Regression Arno Solin and Simo Särkkä (http://arxiv.org/pdf/1401.5508v1.pdf) 25 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Reduced-Rank GPR Consequences for our problem: Original formulation: 50 or 100 Hz magnetometer data (in 3D) Size of the matrix to invert grows very quickly with each additional second of data Downsampling needed and large buildings become infeasible Reduced-rank formulation: Possible to use all data Size of the problem does not grow for longer data sets 26 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Sequential updating Initialize µ 0 = 0 and Σ 0 = Λ θ (from the GP prior). For each new observation i = 1, 2,..., n update the estimate according to S i = Φ i Σ i 1 [ Φ i ] T + σ 2 noise I 3, K i = Σ i 1 [ Φ i ] T S 1 i, µ i = µ i 1 + K i (y i Φ i µ i 1 ), Σ i = Σ i 1 K i S i K T i. 27 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Spatio-temporal modeling Model the scalar potential magnetic field instead as ϕ(x, t) GP(0, κ lin. (x, x ) + κ SE (x, x )κ exp (t, t )), with ( κ exp (t, t ) = exp t ) t. l time The scalar potential can then sequentially be estimated by adding a time update to the measurement update from before as µ i = A i 1 µ i 1, Σ i = A i 1 Σ i 1 A T i 1 + Q i 1. 28 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Problem formulation We want to find a magnetic map of this object! How should the map be modeled? 29 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Problem formulation We want to find a magnetic map of this object! How should the map be modeled? Use the dipole model? 29 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Problem formulation We want to find a magnetic map of this object! How should the map be modeled? Use the dipole model? Use multiple dipoles? 29 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Problem formulation We want to find a magnetic map of this object! How should the map be modeled? Use the dipole model? Use multiple dipoles? Use a continuum of dipoles! 29 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Problem formulation We want to find a magnetic map of this object! How should the map be modeled? Use the dipole model? Use multiple dipoles? Use a continuum of dipoles! Spatial correlation 29 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Real world experiment Measurements have been collected with a magnetometer An optical reference system (Vicon) has been used for determining the position and orientation of the sensor 1.2 1 0.8 0.6 x axis [m] 0.4 0.2 0 0.2 0.4 0.6 1.5 1 0.5 y axis [m] 0 The magnetic environment Training data 30 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Real world experiment Measurements have been collected with a magnetometer An optical reference system (Vicon) has been used for determining the position and orientation of the sensor The magnetic environment Estimated magnetic content 30 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Modeling the magnetic field The magnetic field H is curl-free, i.e. H = 0 [1] y k = f(x k ) + ε k f(x) GP(0, σ 2 const.i 3 + K curl (x, x )) If a vector-field is curl-free, a scalar potential ϕ exists H = ϕ [2] y k = ϕ(x) x=xi + ε k ϕ(x) GP ( 0, k lin. (x, x ) + k SE (x, x ) ) [1] Niklas Wahlström, Manon Kok, Thomas B. Schön and Fredrik Gustafsson, Modeling magnetic fields using Gaussian processes The 38th International Conference on Acoustics, Speech, and Signal Processing (ICASSP), Vancouver, Canada, May 2013. [2] Arno Solin, Manon Kok, Niklas Wahlström, Thomas B. Schön and Simo Särkkä, Modeling and interpolation of the ambient magnetic field by Gaussian processes ArXiv e-prints, September 2015. arxiv:1509.04634. 31 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Building magnetic field maps (2) x-component Encode physical knowledge in the kernel. 20 y-component 0 20 z-component 40 32 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Building magnetic field maps (2) x-component Encode physical knowledge in the kernel. Use reduced-rank GP regression based on the method from [1]. 20 0 20 y-component z-component 40 [1] Hilbert Space Methods for Reduced-Rank Gaussian Process Regression A. Solin, S. Särkkä. 32 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Building magnetic field maps (2) x-component Encode physical knowledge in the kernel. Use reduced-rank GP regression based on the method from [1]. Use a Kalman filter formulation to allow for sequential updating. 20 0 20 y-component z-component 40 [1] Hilbert Space Methods for Reduced-Rank Gaussian Process Regression A. Solin, S. Särkkä. 32 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Building magnetic field maps (2) x-component Encode physical knowledge in the kernel. Use reduced-rank GP regression based on the method from [1]. Use a Kalman filter formulation to allow for sequential updating. Use a spatio-temporal model to allow for changes in the magnetic field. 20 0 20 40 y-component z-component [1] Hilbert Space Methods for Reduced-Rank Gaussian Process Regression A. Solin, S. Särkkä. 32 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Magnetic fields We use a slightly different version of the magnetostatic equations B = 0, H = 0 1 µ 0 B H = M, Example - = 1 µ 0 B - H = M 33 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Gaussian process + magnetic fields The animation illustrated regression for one scalar function f : R R 34 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Gaussian process + magnetic fields The animation illustrated regression for one scalar function f : R R We want to learn three different vector fields f : R 3 R 3 In addition, these fields should obey 34 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

Simulation Predictions Training data Divergence free Curl free - = 1 µ 0 B - H = M 35 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2