Pose tracking of magnetic objects

Transcription

1 Pose tracking of magnetic objects Niklas Wahlström Department of Information Technology, Uppsala University, Sweden Novmber 13, 2017 Seminar Vi2

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

3 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

4 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

5 Magnetometer measurement models 1. Common use: Magnetometer provides orientation heading information. Assume that the magnetometer (almost) only measures the local (earth) magnetic field. 3 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

6 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. 3 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

7 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

8 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

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

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

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

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

13 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

14 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

15 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

16 z-coordinate [m] z-coordinate [m] Experiment 1 Trajectory 0.3 Trajectory y-coordinate [m] x-coordinate [m] y-coordinate [m] accuracy is 8 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

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

18 Experiment 2 - results - position Position [m] Time [s] Black: Ground truth position. Color: Estimated position 10 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

19 Experiment - results - orientation 100 Orientation [degree] Time [s] Black: Ground truth orientation. Color: Estimated orientation 11 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

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

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

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

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

24 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

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

26 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. Magnetic mapping: Build a map of the (indoor) magnetic field. 18 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

27 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

28 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

29 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

30 Gaussian process regression Objective: Estimate f(u) from noisy observations y k = f(u k ) + e k f(u) u / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

31 Gaussian process regression Objective: Estimate f(u) from noisy observations y k = f(u k ) + e k f(u) u / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

32 Gaussian process regression Objective: Estimate f(u) from noisy observations y k = f(u k ) + e k f(u) u / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

33 Gaussian process regression Objective: Estimate f(u) from noisy observations y k = f(u k ) + e k f(u) u / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

34 Gaussian process regression Objective: Estimate f(u) from noisy observations y k = f(u k ) + e k f(u) u / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

35 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 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

36 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 [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, Accepted. 23 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

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

38 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

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

40 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ä ( 25 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

41 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

42 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

43 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 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

44 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

45 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

46 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

47 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

48 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

49 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

50 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

51 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 x axis [m] y axis [m] 0 The magnetic environment Training data 30 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

52 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

53 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 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

54 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 [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 arxiv: / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

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

56 Building magnetic field maps (2) x-component Encode physical knowledge in the kernel. Use reduced-rank GP regression based on the method from [1] 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

57 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 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

58 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 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

59 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

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

61 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

62 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 B = 0 (divergence free) H = 0 (curl free) 34 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

63 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 B = 0 (divergence free) There exist covariance H = 0 (curl free) functions for this! 34 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

64 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 B = 0 (divergence free) There exist covariance H = 0 (curl free) functions for this! 1 µ 0 B H = M 34 / 17 niklas.wahlstrom@it.uu.se Seminar Vi2

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