Our detection method often yields multiple object hypotheses that need to be verified further through post-processing. We improve the robustness of our pose verification method by evaluating the matching likelihood of the pose hypotheses
6.3. Object Detection and Real-Time Tracking
Figure 6.5.: Auto-regressive state-transition model. Particles are propagated ac- cording to the twist ξt estimated from the previous two time steps, affected by Wiener process noise dWt.
over multiple frames within a particle filter framework. For instance, details that allow for disambiguating views on the object may not be immediately visible in the first frame. The particle filter resumes tracking from the detected pose hypotheses using an auto-regressive motion model and an improved proposal distribution that utilizes our MRSMap registration method.
A further advantage of using a particle filter over tracking-by-optimization is the maintenance of multiple pose hypotheses instead of only a single one. In difficult situations such as fast camera motions, partial occlusions, or ambiguous views on the object, tracking with a single hypothesis may fail, since it does not represent uncertainty in pose. In our particle filter framework, tracking- by-optimization is performed with several pose hypotheses. It is integrated with object detection to initialize the tracked poses or to reinitialize the filter if tracking cannot be resumed.
6.3.3.1. State-Transition Model
We propagate each particle with a guess of its current velocity using an auto- regressive (AR) state dynamics model. For representing 6-DoF poses and veloc- ities we choose the SE(3) group and its associated Lie algebra se(3). Members T ∈SE(3) are homogenous transformation matrices while elementsξ∈se(3) are twistsξ=vT, ωTT with linear and angular velocitiesv andω. The exponential map T = exp(ξb∆t) transforms twists into transformation matrices. Its inverse is the logarithmic map ξb∆t= log(T). With ξbwe denote the representation of twists as matrices in R4×4,
ξb:=
0 −ωz ωy vx ωz 0 −ωx vy
−ωy ωx 0 vz
0 0 0 0
(6.36)
There exists a one-to-one mappingT(x) between posesxparametrized in trans- lation vector and quaternion for rotation, and homogeneous transformation ma- trices, thus we will continue to refer to the particle state as poses x.
As in Choi and Christensen (2012b) we model the state-transition by the first-order, discrete-time AR state dynamics
T(xt) =T(xt−1) expξbt−1∆t+dWt√
∆t ξbt−1=λAR 1
∆t logT(xt−2)−1T(xt−1),
(6.37) with process parameter λAR. Uncertainty in the state transition is introduced through the Wiener process noise dWt
√
∆t with dWt =P6i=1i,tEi. The ran- dom variable t ∼N(0,Σξ) is normal distributed and adds noise in the twist coordinates through the basis elementsEi of se(3),
E1:=
0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0
,E2:=
0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0
, E3:=
0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0
,
(6.38) E4:=
0 0 0 0
0 0 −1 0
0 1 0 0
0 0 0 0
,E5:=
0 0 1 0 0 0 0 0
−1 0 0 0 0 0 0 0
, E6:=
0 −1 0 0
1 0 0 0
0 0 0 0
0 0 0 0
.
(6.39) This state-transition model estimates the velocity of a particle from the poses of the last two time steps. The process parameter λAR allows for adjusting the scale of this velocity according to the confidence in the velocity estimate. We parametrize the noisei,t=0i,t+vi,t|v| with constant noise0i,t and a component vi,t that scales with linear or rotational velocity.
6.3.3.2. Observation Model
Observations are RGB-D images zt. We transform the current image into a MRSMap ms,t and determine the likelihood of observing the current image in the model map using the matching likelihood in Sec. 6.2.1.1,
p(zt|xt) =p(ms,t|xt, mm). (6.40) 6.3.3.3. Improved Proposal Distribution
If we would utilize the state-transition model for the proposal distribution, many particles would be required to cover the 7-dimensional pose space well for ac- curate and robust tracking. Instead, we propose to use an improved proposal
6.3. Object Detection and Real-Time Tracking
Figure 6.6.: Particle filtering with improved proposal distributions. Each parti- cle is registered from its predicted pose. The registration is regular- ized by the pose distribution determined from the state-transition model. Regularized registration yields an improved proposal that the particles are sampled from.
distribution that also considers the current RGB-D image zt to obtain a good guess on the pose of the particles already in the sampling step. The particles Xt−1 are first propagated individually according to the motion model towards new predictions Xt−1. We optimize the predicted particle poses to align the current image with the model using our registration method (see Fig. 6.6). The improved proposal distribution
p
x[i]t |x[i]t−1, zt, ut
=p
x[i]t |mm, x[i]t−1, ms,t
=η[i] p(ms,t|x[i]t , mm)p
x[i]t |x[i]t−1
.
(6.41)
is normal distributed with the regularized registration estimate xe[i]t as mean with associated uncertainty Σ
xe[i]t
(see Sec. 3.2.2.4). In order to approximate p
x[i]t |x[i]t−1
with a normal distribution in x[i]t , we apply the unscented trans- form (Julier and Uhlmann, 1997). We propagate sigma points of the process noise through the state-transition model and recover mean and covariance of the pose distribution from the propagated sigma points.
6.3.3.4. Importance Weights
The importance weights Wt of the particles are reweighted according to the mismatch between the target and the proposal distribution (see Sec. 6.1.3).
With our choice of proposal distribution, the importance weights are w[i]=p
zt|x[i]t−1, ut
=η[i]−1=
Z
p(ms,t|xt, mm)p
xt|x[i]t−1, ξt
dxt.
(6.42)
The weights correspond to the observation likelihood under the predicted dis- tribution for the particle’s pose according to the state-transition model. We consider the uncertainty in the predicted pose for our observation likelihood in Eq. (6.23), and propagate the uncertainty in twist to the difference measures between surfels and normals.
The matching likelihood of surfels in Eq. (6.22) has two factors which both involve the pose variable in a non-linear mapping. Due to the neglectance of correlations between spatial and color dimensions, we can focus on the spatial dimensions and define the differences between the spatial and the color surfel distributions as
dp(ss, sm, xt) :=àps,m−T(xt)àps,s, and
dc(ss, sm, xt) :=àcs,m−àcs,s, (6.43) respectively. Pose uncertainty only propagates to the spatial difference. In order to propagate twist uncertainty, we reformulate the spatial difference as a function of twist ξ, the pose from the previous time step xt−1, and the time increment
∆t,
dp(ss, sm, ξ, xt−1,∆t) :=àps,m−T(xt−1) expξ∆tb àps,s. (6.44) Using first-order error propagation, we obtain the covariance contributed to the spatial difference
Σpξ(ss, sm, ξ, xt−1,∆t) :=JξpΣξJξpT, (6.45) with Jξp :=∇ξdp(ss, sm, ξ, xt−1,∆t). It adds to the spatial covariances of the surfels, such that the total covariance of the spatial difference is
Σp(ss, sm, ξ, xt−1,∆t) := Σps,m+R(xt)Σps,sR(xt)T+ Σpξ(ss, sm, ξ, xt−1,∆t), (6.46) where T(xt) =T(xt−1) expξ∆tb . To determine the derivative Jξp, we approxi- mate the spatial difference by
dp(ss, sm, ξ, xt−1,∆t)≈àps,m−T(xt−1)I+ξ∆tb àps,s (6.47) through truncating the series expansion of the exponential map. The derivative forξ then approximately is
Jξp≈ −∆t T(xt−1)∇ξξbàps,s. (6.48)
6.3. Object Detection and Real-Time Tracking
We also consider twist uncertainty for the angular difference dn(ss, sm, xt) :=
arccos(nTmR(xt)ns) of the normals. We rephrase the angular difference in terms of the rotational velocityω of the twist, previous pose, and time difference, i.e., dn(ss, sm, ω, xt−1,∆t) := arccos(nTmR(xt−1) exp (ω∆t)b ns), (6.49) and approximate the exponential map such that
dn(ss, sm, ω, xt−1,∆t)≈arccos(nTmR(xt−1) (I+ω∆t)b ns), (6.50) Through first-order error propagation, we determine the variance
σξn(ss, sm, ω, xt−1,∆t)2:=JξnΣξJξnT, (6.51) where we defined Jξn :=∇ωdn(ss, sm, ω, xt−1,∆t). It contributes to the total variance of the normal estimate
(σn(ss, sm, ω, xt−1,∆t))2:= (σ0n)2+σξn(ss, sm, ω, xt−1,∆t)2. (6.52) The derivative approximately is
Jξn≈ − ∆t
q
1−dn(ss, sm, xt)2
nTmR(xt−1) (∇ωω)b ns. (6.53)
In summary, the resulting observation likelihood for the scene map ms of the current image zt is
p(ms|mm, xt−1, ξt)≈ Y
ss∈ms
p(ss|sm, xt−1, ξt) (6.54)
= Y
ss∈ms
Z
p(ss|sm, xt)p(xt|xt−1, ξt)dxt−1 (6.55) with
Z
p(ss|sm, xt)p(xt|xt−1, ξt)dxt−1= (6.56) N(dp(ss, sm, xt); 0,Σp(ss, sm, ξt, xt−1,∆t)) (6.57)
ãN(dc(ss, sm); 0,Σc(ss, sm)) (6.58)
ãNdn(ss, sm, xt),(σn(ss, sm, ωt, xt−1,∆t))2, (6.59) and Σc(ss, sm) := Σc(ss) + Σc(sm).
Figure 6.7.: Improved proposals on particle clusters. We gain computational ef- ficiency by clustering closeby particles and establishing an improved proposal per cluster.
6.3.3.5. Efficient Approximation to the Improved Proposal Distribution
Registering each particle individually at high frame rates would be computa- tionally demanding. Instead, we propose to identify modes of the density es- timate p(xt |Xt−1), to cluster the particles that belong to the mode, and to perform only a single registration per cluster. Fig. 6.7 illustrates this approach.
We employ a clustering of the particles with a fixed threshold on translation and rotation. For efficient clustering, a kd-tree is constructed from the posi- tion estimates of the particles. Particles in a limited volume and with similar orientations are clustered together until all particles have been assigned.
In order to construct an improved proposal for each cluster as in Eq. (6.41), registration is performed starting from the mean of a cluster. The pose distri- bution for the state-transition model is approximated using the mean velocity of the particles in the cluster and their mean pose from the previous time step.
The resulting improved proposal is used to sample the particles in the same cluster. The importance weights of each particle are still evaluated separately for each particle by using individually predicted pose estimatesx[i]t in Eq. (6.54).
Surfel associations are shared between the particles within a cluster to further increase efficiency. If the particles are distributed within a single cluster, we limit the processing of the RGB-D image to the relevant parts as in Sec. 6.3.2.
We further note that when the estimate of the tracker is good, the discrete distribution given by the particles typically has a single mode. However, after initialization or when uncertainy increases, multiple modes need to be consid- ered.
6.3. Object Detection and Real-Time Tracking
Figure 6.8.: Joint object detection, pose estimation, and tracking in a particle filter framework.