4.2
Photometric Camera Pose Estimation
As mentioned previously, for photometric camera pose estimation we choose to match between consecutive RGB-D frames instead of matching to the texture predicted from the surface reconstruction. This is motivated by the fact that depending on the con- figuration of the TSDF there may be poor overlap between the camera frustum and the volume, which limits the amount of photometric information which can be used, where distant photometric features are desirable to constrain camera rotation. Fur- thermore, the resolution of the TSDF in terms of voxels may produce a raycast image with a much lower resolution than the image produced by the RGB sensor. By de- fault the Microsoft Kinect and Asus Xtion Pro Live, two of the most popular RGB-D sensors, have automatic exposure and white balance enabled, which can cause unusual coloring of the surface reconstruction over time, again hindering model-based photo- metric tracking. While these functions of the camera can be disabled we have found that it is sometimes desirable to keep them enabled as in general scene illumination can vary to a certain degree.
Given two consecutive RGB-D frames [rgbn−1, dn−1] and [rgbn, dn] we compute
a rigid camera transformation between the two that maximises photoconsistency. Defining V : Ω → R3 to be the back-projection of a point p, dependent on a metric
depth map M : Ω → R and camera intrinsics matrix K made up of the principal points cx and cy and the focal lengths fx and fy:
V (p) = (px− cx)M (p) fx ,(py− cy)M (p) fy , M (p) > (4.10) We also define perspective projection of a 3D point v = (x, y, z)> including deho-
difference in intensity values between two images In−1, In : Ω → N: Ergbd= X p∈L In(p) − In−1 Πn−1(exp( ˆξ)TVn(p)) 2 2 (4.11)
Where L is the list of valid interest points populated in Algorithm 4.1 and T is the current estimate of the transformation from In to In−1. Similar to the geometric
pose estimation method we solve for this transformation iteratively with a three level image pyramid.
4.2.1
Preprocessing
For both pairs we perform preprocessing on the RGB image and depth map. For each depth map we convert raw sensor values to a metric depth map M : Ω → R and we compute an intensity image I = (rgbR∗ 0.299 + rgbG∗ 0.587 + rgbB∗ 0.114) with
I : Ω → N. Following this a three level intensity and depth pyramid is constructed using a 5 × 5 Gaussian kernel for downsampling. We compute the partial derivatives
∂In
∂x and ∂In
∂y using a 3 × 3 Sobel operator coupled with a 3 × 3 Gaussian blur with
σ = 0.8. Each of these steps is carried out on the GPU acting in parallel with one GPU thread per pixel.
4.2.2
Precomputation
As with the ICP method described in Section 4.1, we use projective data association between frames to populate the point correspondences. For the sake of speed we only include point correspondences with a minimum gradient in the intensity image, with the motivation that other low gradient points will not have a significant effect on the final transformation. We implement this optimisation by using a list of interest points, which involves a much larger set of points than a point feature extractor could provide. Compiling this list of points as a parallel operation is done using a basic
4.2. PHOTOMETRIC CAMERA POSE ESTIMATION parallel reduction exploiting shared memory in each CUDA thread block as inspired by a similar operation by van den Braak et al. [123]. Algorithm 4.1 lists the operation as it would operate for each level of the pyramid.
Algorithm 4.1: Interest Point Accumulation Input: ∂In
∂x and ∂In
∂y intensity image derivatives
s minimum gradient scale for pyramid level Output: L list of interest points
kL global point count
Data: α thread block x-dimension β thread block y-dimension γ pixels per thread
ι shared memory local list κ shared memory local index blockIdxCUDA block index threadIdxCUDA thread index in parallel do
i ← β ∗ blockIdx.y + threadIdx.y // compute starting pixel j ← α ∗ γ ∗ blockIdx.x + γ ∗ threadIdx.x
if threadIdx.x = 0 and threadIdx.y = 0 then κ ← 0
syncthreads()
for l ← 0 to γ do // for each pixel in this thread p ← (i, j + l) g2 = ∂In ∂x(p) 2+∂In ∂y(p) 2
if g2 ≥ s then // add pixel if gradient high enough idx ← atomicInc(κ)
ιidx ← p
syncthreads()
b ← α ∗ γ ∗ threadIdx.y + γ ∗ threadIdx.x
for l ← 0 to γ do // reduce local list into global list a ← b + l
if a < κ then
idx ← atomicInc(kL)
Lidx ← ιa
end
In the computation of the Jacobian matrix the projection of each point in Mn−1
is required. For each pyramid level the 3D projection Vn−1(p)of each point p in the
based on a condition results in performance hindering branching and a reduction in pipelining. Empirically it was found to be faster to simply project the entire depth map rather than only project points required in correspondences.
4.2.3
Iterative Transformation Estimation
Our iterative estimation process takes two main steps; (i) populating a list of valid correspondences from the precomputed list of interest points and (ii) solving the linear system for an incremental transformation and concatenating these transformations. The first step involves a reduction similar to the one in Algorithm 4.1, but rather than reducing from a 2D array to a 1D array it reduces from a 1D array to another 1D array; a distinction which results in a notable difference in implementation. Before each iteration we compute the projection of the current estimated transformation T into the image (given the camera intrinsics matrix K) before uploading to the GPU as
RI = KRK−1, tI = Kt. (4.12)
Algorithm 4.2 lists the process of populating a list of point correspondences from the list of interest points which can then be used to construct the Jacobian.
Similar to the previous section, with a list of valid correspondences we need only compute the least-squares solution
arg min
ξ
kJrgbdξ + rrgbdk22 (4.13)
to compute the motion update vector ξ. We first normalise the intensity difference sum σ computed in Algorithm 4.2 to enable a weighted optimisation σ0 = pσ/k
C.
4.2. PHOTOMETRIC CAMERA POSE ESTIMATION
Algorithm 4.2: Correspondence Accumulation Input: L list of interest points
dδ maximum change in point depth
[In−1, Mn−1] previous intensity depth pair
[In, Mn] current intensity depth pair
RI camera rotation in image tI camera translation in image
Output: C correspondence list of the form (p, p0, ∆) kC global point count
σ global intensity difference sum Data: α thread block x-dimension
γ pixels per thread
ι shared memory local list κ shared memory local index blockIdxCUDA block index threadIdxCUDA thread index in parallel do
i ← α ∗ γ ∗ blockIdx.x + γ ∗ threadIdx.x // compute starting point if threadIdx.x = 0 then
κ ← 0 syncthreads()
for l ← 0 to γ do // for each pixel in this thread p ← Li+l
z ← Mn(p)
if isValid(z) then // test if depth valid (x0, y0, z0)>← z(RI(p, 1)>) + tI // transform with estimate
p0 ← (x0 z0,
y0
z0)
> // project into previous image
if isInImage(p0) then d ← Mn−1(p0)
if isValid(d) and |z0 − d| ≤ dδ then // depth delta test
idx ← atomicInc(κ)
ιidx ← (p, p0, In(p) − In−1(p0)) // add valid correspondence
syncthreads() b ← γ ∗ threadIdx.x
for l ← 0 to γ do // reduce local list into global list a ← b + l
if a < κ then
atomicAdd(σ, ιa2∆) // sum total residual
idx ← atomicInc(kC)
Cidx ← ιa
populated including usage of σ0 for weighting. Equation 4.13 is then solved using a
tree reduction on the GPU followed by Cholesky factorisation of the linear system on the CPU.