State-space models for 3D visual tracking

Transcription

1 SA-1 State-space models for 3D visual tracking Doz. G. Bleser Prof. Stricker Computer Vision: Object and People Tracking

2 Example applications Head tracking, e.g. for mobile seating buck Object tracking, e.g. for visual grasping in robotics Camera tracking, e.g. for Augmented Reality Human motion capture 2

3 Outline Preliminaries: Recap: camera geometry, 3D reconstruction vs. pose estimation 3D transformations, rigid body motion, rotation parametrisations Multi-purpose (non)linear Gaussian state-space models for 3D visual tracking x t = g x t 1, u t, ε t, ε t ~N(0, R t ) z t = h x t, δ t, δ t ~N(0, Q t ) This is the most general statespace model form Example applications 3

4 Typical estimation problems Pose estimation in 3D. The state contains: pose of moving object observed by stationary camera relative to static world frame pose of moving camera observing stationary features in the world pose of moving object observed by moving camera poses of an articulated body world Similar estimation problems (it doesn t matter, whether object or camera are moving) camera object 4

5 Recap: 3D reconstruction (triangulation) Estimation of 3D points given corresponding 2D coordinates and camera poses. 3D feature location 2D feature location (from image processing) Camera pose 5

6 Recap: camera pose estimation Camera pose estimation based on corresponding 2D and 3D coordinates. Tracking of landmarks/features in the world. 3D feature location 2D feature location (from image processing) Camera pose 6

7 Object pose estimation Stationary camera, moving object (essentially the same problem) y o x c z c z o x o y c Object pose {R oc, o c } t 7

8 Recap: world/object-to-camera-mapping Rigid transformation: world/object to camera space (3D-3D) (to be estimated) Camera projection (3D-2D): intrinsic parameters (assumed calibrated) m w Image formation (perfect pinhole): m p = Π K (R m w +t) m p 3D rotation Translation y T c Π x y z T x z z R, t w Perspective projection 8

9 3D articulated body Transformation hierarchy: rigid segments (bones) and joints with varying DoF Estimation parameters: (relative) poses of each joint 3 2 Kinematic hand model Stick-figure (kinematic model) 9

10 SA-1 Kinematics Rigid body motion, rotation representation

11 How can we parametrise pose? R = r 11 r 12 r 13 r 21 r 22 r 23 R 3 3 t = r 31 r 32 r 33 t x t y t z R 3 Rows of R are the coordinate axes of the new frame, given in the old frame 11

12 Properties of rotation matrices Rotation: Special-Orthogonal group in 3D SO 3 Preserves angles and distances (orthogonal) v T w R T R 1 T Rv Rw R I R 1 v T R R T T Rw Columns have all unit length T T T R 1R 1 1, R 2R 2 1, R 3R 3 1 and are orthogonal to each T T T other R R 0, R R 0, R R matrix elements, 6 constraints 3 degrees of freedom (DoF) 12

13 Rotation 1: Euler angles Parametrizing 3D rotations: Euler angles (x-y-z) R x α = cos (α) sin α 0 sin (α) cos (α) R y β = R z γ = cos (β) 0 sin (β) sin (β) 0 cos (β) cos (γ) sin (γ) 0 sin (γ) cos (γ) Sequence of 3 elementary rotations around (x, y, z): R α, β, γ = R x α R y β R z γ Order not standardized! 13

14 Rotation 1: Euler angles Easy to understand Minimal number of 3 parameters Intuitive geometric interpretation Periodicity of angles needs to be handled Singularity for some particular configurations, e.g. gimbal lock: loss of one DoF, if two gimbals are in the same plane R α, π 2, γ 14

15 Rotation 2: exponential map (axis angle) Axis-angle: w, θ, w = 1 Definition: rotation vector ρ ρ = w θ = ρ x ρ y ρ z θ = ρ Theorem: for any rotation R SO 3, there exists a vector ρ such that: R = exp(ρ) Cross product matrix: 0 ρ z ρ y ρ = ρ z 0 ρ x ρ y ρ x 0 Matrix exponential 15

16 Rotation 2: exponential map (axis angle) From ρ = w θ to R (Rodrigues formula): R = exp ρ = I 3 + w sin θ + w 2 (1 cos(θ)) Minimal number of 3 parameters No singularities Complex formulae Ambiguity for w, if θ = 180 Periodicity of angle needs to be handled 16

17 Rotation 3: Quaternions Quaternions = hyper-complex numbers with 4 components q = a + bi + cj + dk = a, w Scalar Vector Hamilton axioms i i = j j = k k = i j k = j k i = k i j = 1 Quaternion product q 1 q 2 = a 1, w 1 a 2, w 2 = a 1 a 2 w 1 T w 2, a 1 w 2 + a 2 w 1 + w 1 w 2 17

18 Rotation 3: Quaternions Unit quaternions: q = a 2 + b 2 + c 2 + d 2 =1 Quaternion conjugate: q = a, w q = a, w q 1 = q q = 1 can be written as q = cos θ 2, w sin θ 2 Represents a rotation about axis w by angle θ In order to rotate a 3D point X: Write it as a quaternion q X = 0, X Compute the product q X = q q X q Get back the rotated point: q X = 0, X 18

19 Rotation 3: Quaternions Equivalence between unit quaternion and rotation matrix q X = q q X q X = R q X R q = a 2 + b 2 c 2 d 2 2bc 2ad 2ac + 2bd 2ad + 2bc a 2 b 2 + c 2 d 2 2cd 2ab 2bd 2ac 2ab + 2cd a 2 b 2 c 2 + d 2 Question: What is R 1 q? R 1 q = R(a, w) conjugate=inverse in case of a unit quaternion 19

20 Rotation 3: Quaternions No singularities Simple expression (polynomial) for rotation Ambiguity: Rot q = Rot( q) Redundant parameters (4 instead of 3) needs an additional, nonlinear constraint: q = 1 Methods to enforce the constraint: Normalization after each measurement update step (brute-force ) Add a penalty term: (1 q 2 ) during optimization 20

21 How can we represent rigid pose Translation: t = R(x) t x t y t z R 3 The chosen representation provides the parameters to be estimated in the state Rotation: choose appropriate representation, x, (rule-of-thumbs) Linear least squares estimation: rotation matrix Nonlinear least squares estimation: Euler angles, axis angle (minimal parametrization) Interpolation: quaternions Extended Kalman filter: often quaternions (also other representations) 21

22 Kinematics: translational motion Velocity: Acceleration: v p = p t a v = p = v t = 2 p 2 t Assuming constant velocity: v p = v t = p 0 + [vt] Assuming constant acceleration: p = v t = a 2 t = p at2 This holds for n-dimensional vectors 22

23 Kinematics: rotational motion Complicated in 3D! Angular velocity (cf. exponential map): ω = wθ Instantaneous rotation axis Angular acceleration: Differential equation: ω = ω t R = ωr Integration yields (rectangular rule): R = exp ωt R Rotation speed around this axis r ω ω r Rodrigues formula 23

24 Kinematics: rotational motion Differential equation based on quaternions: q = 1 ω q 2 Integration yields (rectangular rule): t q = exp ω q 2 Quaternion exponential: Closed-form solution available Model can be simplified with small-angle approximation r ω ω r Bibliography: Kleppner/Kolvenkow: An introduction to mechanics Shuster: A Survey of Attitude Representations 24

25 SA-1 Multi-purpose (non)linear Gaussian statespace models for 3D visual tracking All noises are assumed zero-mean Gaussian distributed Some slides are based on G. Panin

26 Recap: motion model Motion/Dynamical model: a probabilistic model describing the state evolution in time, possibly based on a control input x t = g x t 1, u t, ε t deterministic function of x t 1 and u t v t 1 + p t 1 ε t v t random component g(x t 1, u t, 0) pt Deterministic term: in absence of pertubation, gives predicted state Process noise ε: gives the uncertainty about the prediction 26

27 Translational motion (linear) Unconstrained models without control input (holds for n = 1 3D case) Notation: x, p, v, a, j, ε n-vectors and I, 0, T ΔtI n n diagonal matrices State Motion model Assumptions x = p x t+δt = x t + Tε t (also: Brownian motion) Const. position, white noise velocity ε t = v t x = p v x t+δt = I T 0 I x t + T 2 2 T ε t Const. velocity, white noise acceleration ε t = a t x = p v a x t+δt = I T T2 2 0 I T 0 0 I x t + T 3 3 T 2 2 T ε t Const. acceleration, white noise jerk ε t = j t 27

28 Translational motion (linear) Unconstrained models with control input State Motion model Assumptions x = p x t+δt = x t + T(u t + ε t ) Const. velocity, velocity as noisy input v t = u t + ε t x = p v x t+δt = I T 0 I x t + T 2 2 T (u t + ε t ) Const. acceleration, acceleration as noisy input a t = u t + ε t Smaller state, more efficient Control input enters estimate immediately, not filtered 28

29 Translational motion (linear) Ballistic trajectory (gravity modelled as noisy control input) State Motion model Assumptions x = p v x t+δt = I T 0 I x t + T 2 2 T (u t +ε t ) Const. acceleration due to gravity, u t = g, with white noise acceleration, ε t The model doesn t have to be physically motivated Basically, we are free to model whatever we want Constrained Brownian motion (in 1D) State Motion model Assumptions x = p x t+δt = ax t + bε t a 1 Random walk in position 29

30 Exercise: translational motion in 2D 1. Implement the different motion models in 2D 2. Choose noise settings 3. Simulate a trajectory 4. Plot the results 30

31 Rotational motion in 3D Linear (analogous to translational case) in case of Euler angles/rates/accelerations representation Often, quaternions,q, and angular velocity, ω, are used nonlinear State Motion model Assumptions x = q ω x t+δt = exp Δt 2 ω t + ε t q t ω t + ε t Const. angular velocity, random walk in angular velocity ε t Possible model variations: Constant orientation/acceleration assumptions can be made Angular velocity can be modelled as control input 31

32 Rigid body motion Translational and rotational models can be combined For articulated bodies, several (relative) poses are combined in one state vector (more in the next lecture) 32

33 Recap: measurement model Measurement/observation model: a probabilistic model describing, how measurements are generated from the state Explicit form: (with additive noise for simplicity) z t = h x t + δ t ~N(0,Q t ) deterministic function of x t + random component Implicit form: p z t x t = p z t h x t N(z t h x t ;0,Q t ) z t h x t Measurement likelihood given the state Deterministic term: gives expected measurement Measurement noise δ: imprecise model, noisy data, 33

34 Pixel level measurement The measurement is a set of pixels (e.g. in a texture patch), or a segmentation map can be computationally intense h x t z t Textured 3D model for predicting feature appearance image Feature points tracked based on surrounding texture patches using optical flow (with brightness model) 34

35 Feature level measurement The measurement is a set of matched/tracked feature points (their 2D pixel positions) Typical model for a perspective camera (assume perfect pinhole): Pose with quaternion m w z = h x q t + δ m p = Π K (R q m w +t) + δ p h x t As before: perspective projection R(q), t w c z t = m p 35

36 Object level measurement The measurement is the camera/object pose The measurement model seems trivial: What is the covariance of the pose? z = h x + δ q t = q t + δ Pose can t be measured directly estimated from a set of features, e.g. using least squares estimation Expected pose q t Question: Which other problem do we have here? Measured pose q t 36

37 SA-1 Example applications

38 Example: ball trajectory 38

39 Example: ball tracking Assumptions: calibrated camera setup State (per ball): Motion model: Measurement model: 39

40 Example: ball tracking Assumptions: calibrated camera setup State (per ball): position, velocity of ball in world frame Motion model: ballistic trajectory Measurement model: Feature level: detected balls in each image Object level: triangulated 3D coordinate Which problems could violate the Markov assumption? How is the state initialized? 40

41 Application: augmented reality Camera on HMD to track head Tracking based on CAD model and optical flow, online reconstruction of feature points 41

42 Application: mobile seating buck augment real car with virtual parts (e.g. design review) Requirements: Easy-to-setup head tracking solution Minimal delay (for see-through HMD) High precision Overcome occlusions Wide field of view (fov) Approach here: Perspective camera mounted on the head tracks several artificial landmarks in the car 42

43 Application: mobile seating buck Advantages: Wide fox (up to 180 horizontal) Easy set-up (just 1 marker) Stable pose (less features required due to IMU) High update rates (e.g. 100 Hz) Modeling of head motion possible in EKF framework Modified approach: Camera-IMU (inertial measurement unit) system with fisheye lens mounted on the head tracks one single marker visualized on the navigation display 43

44 Fisheye lenses Perspective lens Small field of view: ~45 Fisheye lens Large field of view: ~180 44

45 3D gyroscope: Inertial sensors Measures angular velocity in sensor frame, s, with respect to global frame, g Typical model: Slowly varying bias (quasi-static) z ω = ω s + bω s + δ ω 3D accelerometer: Measures linear acceleration in sensor frame, s, with respect to global frame, g Typical model: Acceleration due to gravity z a = s s g s + ba s + δ a z a = R sg (sg g g ) + ba s + δ a 45

46 Why visual-inertial sensor fusion? Cameras: accurate but slow (e.g. 25 Hz) Inertial sensors: fast (e.g. 100 Hz) but not accurate Camera-IMUs: Fast and accurate (complementary sensors) 46

47 47

48 Application: mobile seating buck Assumptions/boundary conditions: More rotational than translational head motion Camera and inertial sensor frame identical: c = s Global and object/world frame identical: w = g Calibrated fisheye camera (each pixel, m p, corresponds to a normalized ray, r c, in the camera frame) 48

49 Visual-inertial head tracking (model gravity) State: x = c w cw q cw T Position, velocity, orientation of camera in world frame Motion model: constant velocity, constant angular velocity c w cw q cw t+δt = c w + Tcw + T2 2 εc cw + Tε c exp T 2 ω c + ε ω q t t Standard const. vel. model (better damped model) Angular velocity from gyroscopes as control input Possible variants: Angular velocity in state, gyroscope measurement Gyroscope bias can also be estimated as a quasi-static parameter 49

50 Visual-inertial head tracking (model gravity) Measurement model (vision): feature-level λr c = R q cw m w + c w + δ r 0 = λr c R q cw m w + c w + δ r Measurement model (accelerometer, used as inclinometer): z a = R q cw cw g w + δ a =0 Constant velocity assumption z a = R q cw g w + δ a Provides information about 2 angles Variants: constant acceleration assumption, requires acceleration in state 50

51 Exercise: visual-inertial head tracking Write the state-space model assuming constant acceleration 1. Variant 1: Model accelerometer data as control inputs 2. Variant 2: Model accelerometer data as measurements 3. Discuss the advantages/disadvantages of both approaches 4. How could the constrained translational motion be incorporated (the head does not move outside the car)? 51

52 Head tracking for Augmented Reality Goal: reliable camera tracking in a larger volume with very agile camera motions Given: textured CAD model [Bleser and Stricker, 2008] 52

53 Head tracking for Augmented Reality Image processing approach: Feature points tracked based on surrounding texture patches Appearance predicted with help of CAD model (needs good pose prediction, otherwise tracking loss) Optical flow tracking (with brightness model) Estimation approach: Visual-inertial sensor fusion as before Const. acceleration, const. angular velocity model Inertial measurements as control inputs [Bleser and Stricker, 2008] 53

54 Simultaneous localisation and mapping Goal: tracking in partially known environments Image processing approach: Initialisation with CAD model (edge-based) Natural feature tracking with optical flow Estimation approach: 3D feature positions estimated in separate 3D Extended Kalman filters Robust weighted least squares for camera pose [Bleser et al., 2007] 54

55 Visual-inertial human motion capture Goal: track upper body motions and locate body with respect to workspace [Bleser et al., 2011] 55

56 Visual-inertial human motion capture Kinematic body model with rigid segments and joints (varying DoF) State: Euler angles/rates/accelerations of each joint (hierarchical), global position of root Motion model: Joints: constant angular acceleration Root: constant position Measurements: Inertial measurements Egocentric camera measurements: 2D wrist positions in image Camera pose with respect to workspace (marker-based) 56

57 Exercise: human motion capture (outside-in) Suggest a state-space model for visual outside-in tracking Assumption: markers exactly in joints Kinematic model 57

58 Application: hand pose tracking Kinematic hand model with rigid segments and joints (varying DoF) State: joint orientations as axis angle (hierarchical), global position of root Measurements: Twin edges (indicate fingers) Skin colour to remove outliers End points of fingers By Nils Petersen 58

59 Summary Various tracking problems can be solved in the Bayesian tracking framework Basic toolkit of multi-purpose state-space models The tracking solution is only as good as your model! Modeling skills need to be trained! start with some of the suggested exercises Videos and papers describing the presented works: 59

60 Organisation Exercise: 3 rd exercise will be uploaded today Next lectures: People tracking based on RGB-D data (by Markus Weber) Discussion of 3 rd exercise (as last lecture) 60