Supplement: Efficient Global Point Cloud Alignment using Bayesian Nonparametric Mixtures

Transcription

1 Supplement: Efficient Global Point Cloud Alignment using Bayesian Nonparametric Mixtures Julian Straub Trevor Campbell Jonathan P. How John W. Fisher III Massachusetts Institute of Technology 1. Rotational Alignment Details 1.1. The matrix Ξ kk In the main text, we are given two unit vectors µ 1k and µ k in R 3. We define Ξ kk = Ξµ 1k, µ k ), where Ξu, v) R 4 4 is defined by u T q v) = q T Ξu, v)q, where u = u i, u j, u k ), v = v i, v j, v k ), and q = q i, q j, q k, q r ). By standard quaternion rotation formula, we have u i u T q v) = u j u k 1 qj q k q i q j q k q r ) q i q k + q j q r ) v i q i q j + q k q r ) 1 qi q k q j q k q i q r ) v j q i q k q j q r ) q j q k + q i q r ) 1 qi q j v k T = q i u j v j u k v k ) + q j u i v i u k v k ) + q k u i v i u j v j ) + q i q j u j v i + u i v j ) + q j q k u k v j + u j v k ) + q i q k u i v k + u k v i ) + q i q r u k v j u j v k ) + q j q r u i v k u k v i ) + q k q r u j v i u i v j ) + u T v Rearranging the quadratic expression in q into the form q T Mq, we find the formula for Ξu, v): 1.. Quadratic upper bound on f u i v i u j v j u k v k u j v i + u i v j u i v k + u k v i u k v j u j v k Ξu, v) = u j v i + u i v j u j v j u i v i u k v k u j v k + u k v j u i v k u k v i u i v k + u k v i u j v k + u k v j u k v k u i v i u j v j u j v i u i v j u k v j u j v k u i v k u k v i u j v i u i v j u T v First, for any z [a, b] where 0 a b, we can express z as a convex combination of a and b, i.e. Since f z) = e z e z z nonnegative), we have z = λa + 1 λ)b = λ = z a b a 1) for z 0 is convex this can be shown by taking the second derivative and showing it is ) ) fz) = f z = f λa + 1 λ)b λf a) + 1 λ)f b) 3) ) fb) fa) b = z fa) a ) fb) b a + b a. 4) In the main text, since we know l kk z kk q) u kk for any q Q, we can use the above upper bound formula with a = l kk and b = u kk. The first two authors contributed equally to this work. ) 1

2 Figure 1: The three subdivision patterns due to the choice of the green edge of a tetrahedron displayed in 3D. Colors designate different edge types: corner edges blue) from an edge midpoint to a vertex; tie edges orange) between two edge midpoints, running along a tetrahedron face; and skew edges green) between two edge midpoints, running through the inside of the tetrahedron Derivation of the γ N bound Lemma 1. Let γ N be the minimum dot product between any two tetrahedral vertices at refinement level N. Then γ N γ N 1 γ N. 5) Proof. Let the vertices of the projected tetrahedron be q i, i {1,, 3, 4}. Let γ = min j k qj T q k, Γ = max j k qj T q k and define the vertex between q i and q j as q ij = qi+qj q i+q j. Upon subdividing the tetrahedron, there are three different types of edge in the new smaller tetrahedra. Refer to Fig. 1 for a depiction of these three types. The first type of edge blue in Fig. 1) is a corner edge from a vertex to an edge midpoint. The cosine angle between the vertices created by a corner edge is 1 + q qi T T q ij = i q j 1 + γ. 6) The second type of edge orange in Fig. 1) is a tie edge from an edge midpoint to an edge midpoint along a face. The cosine angle between the vertices created by a tie edge is qijq T ik = 1 + qt i q k + qi T q j + qj T q k 1 + qi T q 1 + qt i q k + qi T q j + γ j 1 + q T i q k 1 + qi T q j To see the rightmost inequality, consider the minimization 1 + q T i q k > 1 + 3γ 1 + γ). 7) min x,y 1 + γ + x + y 1 + x 1 + y s.t. γ x, y Γ. 8) The optimum solution is at x = y = γ, since the function is symmetric and monotonic in x, y: ) d 1 + γ + x + y dx 1 + x 1 = 1 + y x 1 γ + y ) 1 + y 1 + x) 1 > x 1 γ + Γ ) > y 1 + γ 9) The final type of edge green in Fig. 1) is a skew edge from an edge midpoint to an edge midpoint through the interior of the tetrahedron. The cosine angle between vertices created by a skew edge is qijq T kl = qt i q k + qi T q l + qj T q k + qj T q l. 10) 1 + qi T q j 1 + qk T q l Note that we can choose any of three skew edges in our refinement. Therefore, we can formulate bounding the skew edge dot product as a process where nature creates three skew edges, and we select the best one i.e. the one of maximum dot

3 product). Thus, in the worst case, nature solves the following problem: given a selection of a skew edge, minimize its dot product such that the other two dot products are lower and thus nature forces us to pick that edge). Let s 1 = q T 1 q 3 + q T q 4 p 1 = q T 1 q 3 )q T q 4 ) s = q T 1 q 4 + q T q 3 p = q T 1 q 4 )q T q 3 ) s 3 = q T 1 q + q T 3 q 4 p 3 = q T 1 q )q T 3 q 4 ). 11) Then without loss of generality, we assume the ordering s 1 + s 1 + s 3 + p 3 s 1 + s s + p s + s s 1 + p 1. 1) Now since the function fx, y) = 1 + x)1 + y) constrained by x + y = c, x, y 0, reaches its maximum at x = y = c, we can reduce all of the fractions above until 1 + s i + p i = 1 + s i /), and therefore redefining x i = s i /, this problem is reduced to minimizing the maximum fraction of x 1 + x 1 + x 3 x 1 + x x x + x x 1. 13) Note that while the ordering of the inequalities may switch, we can assume without loss of generality that the above holds since we can simply redefine labels 1,, and 3 accordingly). Next, note that the first inequality above implies that x x 3, and the second inequality likewise implies that x 1 x. Therefore, minimizing over x 1 and x while keeping x 3 fixed yields And finally, minimizing over x 3 [γ, Γ] yields For the final result of the proof, note that 1 + γ x x 3. 14) max skew edges qt ijq kl γ 1 + γ. 15) 1 + 3γ 1 + γ) γ 1 + γ γ [0, 1]. 16) 1.4. Proof of Theorem 1 rotational convergence) Theorem 1. Suppose γ 0 = 36 is the initial maximum angle between vertices in the tetrahedra tessellation of S 3, and let { γ0 N max 0, log 1 1 } cos ɛ/) 1. 17) 1 Then at most N refinements are required to achieve a rotational tolerance of ɛ degrees, and BB has complexity Oɛ 6 ). Proof. Using Lemma 1, we know that the minimum dot product between any two vertices in a single cover element Q at refinement level N satisfies γ N γ N γ N 1. 18) This function is monotonically increasing by taking the derivative and showing it is positive). So we recursively apply the bound: γ N γ N 1+γ N 1 + γ N 1+γ N = 4γ N 1 + 3γ N N γ N 1) γ 0. 19)

4 If we require a rotational tolerance of ɛ degrees, we need that cos 1 γ N ɛ noting that the rotation angle between two quaternions is times the angle between their vectors in S 3 ). Therefore, we need Using our lower bound, this is satisfied if γ N cos ɛ/). 0) N γ 0 γ N cos ɛ/) = N log 1) γ 0 cos ɛ/) ) Since N must be a nonnegative integer, the formula in Eq. 17) follows. At search depth M, the BB algorithm will have examined at most M tetrahedra, where M = N ) = 600 8N ) Using the formula for N in Eq. 17) and noting 8 = 3 ), we have γ 1 ) 3 ) ) ) cos ɛ/) M = O cosɛ/) 1 = O. 3) 1 1 cos ɛ/) Finally, using the Taylor expansion of cosine, 1 ) ɛ 3 ) M = O = O ɛ 6). 4) ɛ 1.5. Derivation for the l kk and u kk optimization We need to show that maximizing µ T q ν) for q Q is equivalent to maximizing µ T v for v = Mα, α 0, α R 4, for some M R 3 4. The following lemma establishes this fact. Lemma. Let Q be a projected tetrahedron cover element on S 3 with vertices q i, i = 1,..., 4, define m R 3 satisfying m = 1 i.e. m S ), and let M be the set of vectors reached by rotating m by q Q, Then M can be described as a combination of vectors in R 3 via where m i q i m R 3, and M [m 1 m 4 ] R 3 4. M { x R 3 : x = q m, q Q }. 5) M = { x R 3 : x = 1, x = Mα, α R 4 +}. 6) Proof. In this proof, we make use of quaternion notation. If q = xi + yj + zk + w is a quaternion, then its pure component is q = xi + yj + zk, its scalar component is q = w, and conjugation is denoted q. To begin the proof, note that q Q implies that q = Qα for some α R 4 +, by definition. Since q m is a rotation of a vector, it returns a pure quaternion; thus, q m = q m = = i,j q i mqj = i,j i,j q i mqi q i qj = i,j q i mq j m i q i q j 7) where α i is the i th component of α. Now note that q i q j is the quaternion that rotates m j to m i : q i q j ) m j = q i q j )m j q i q j ) = q i q j q j mq j q j q i = q i mq i = m i. 8)

5 Therefore, the axis of rotation of q i qj is the unit vector directed along m j m i, and the angle is θ ij. Since m j m i = sin θ ij ) m j m i, we have that ) T i q i qj sin θ ij /) = m j m i j + cos θ ij sin θ ij w. 9) k Using this expansion along with the identity rs = r s + s r + r s, we have that q m = i,j = i,j = i m i q i q j m i q i qj + m i ) q i qj α i m i + i j m i q i qj + m i ) q i qj = i α i m i + i<j α iα j m i + m j ) cos ) + sin/) sin θ ij m i m j m i ) + m j m i m j )) ) 30) Now noting that for any unit vectors a, b R 3 with angle θ between them, we have a b a) = b cos θ)a 31) which can be derived from the triple product expansion identity a b c) = ba c) ca b). So applying this to m i m j m i ) and m j m i m j ) q m = αi m i + ) m i + m j ) cos i i<j + sin θ ) 3) ij/) m j cos θ ij m i + m i cos θ ij m j ) sin θ ij and finally using the double angle formulas, q m = i α i m i + i<j m i + m j ) sec )) 33) combining, thus q m = i,j m i sec ) 34) Since secθ) 0 θ π, π ), the coefficients are 0 π, π). Therefore, q m is a linear combination of the vectors m i with nonnegative coefficients.. Translational Alignment Derivations and Proofs Recall that we reuse notation in this section from the rotational section to simplify the discourse and draw parallels to the rotational problem..1. Linear upper bound on f For any z [a, b] where 0 a b, we can express z as a convex combination of a and b, i.e. z = λa + 1 λ)b = λ = z a b a. 35)

6 And, since fz) = e z is convex, fz) = fλa + 1 λ)b) λfa) + 1 λ)fb) 36) ) ) fb) fa) bfa) afb) = z +. 37) b a b a In the main text, since we know l kk z kk q) u kk for any q Q, we can use the above upper bound formula with a = l kk and b = u kk... Proof of Theorem translational convergence) For translation, we have a similar result to Lemma 1, but it is much simpler to show; the diagonal of each rectangular cell is simply 1/ that of the previous refinement level, i.e. γ N 1 = γ N = Γ N = Γ N 1. 38) Theorem. Suppose γ 0 is the initial diagonal of the translation cell in R 3, and let { γ } 0 N max 0, log. 39) ɛ Then at most N refinements are required to achieve a translational tolerance of ɛ, and BB has complexity Oɛ 3 ). Proof. If γ 0 is the initial diagonal length, then γ N = N γ 0. So to achieve a translational tolerance of ɛ, we need that γ N ɛ, meaning N γ 0 ɛ = N log γ 0 ɛ. 40) Since N must be at least 0 and must be an integer, the formula in the theorem follows. As the branching factor at each refinement is 8, the BB algorithm at level N will have examined at most M cells, where M = N = 8N ) Substituting the result in Eq. 39) and noting 8 = 3 ), we have γ0 ) ) 3 M = O = O ɛ 3). 4) ɛ 3. Additional Results Noise and Outliers The robustness to noise and outliers is important for any alignment method. In Fig. we show the angular and rotational BB+ICP alignment error as a function of noise standard deviation and outlier ratio for the alignment of the full Stanford Bunny. The synthetic data is created by first adding isotropic Gaussian noise and then sampling random outlier points uniformly inside a sphere with twice the radius of the size of the Stanford Bunny. Standard deviations and translational errors are reported as a fraction of the diameter of the original Stanford Bunny point cloud. The error statistics over 336 instantiations of the alignment problem show the robustness of our method to unrealistic amounts of corruption high noise, 60% outliers). Above a noise threshold, surface normal computation fails leading to high alignment error. Gazebo Summer Dataset [1] The dataset consists of 33 scans taken in a mostly unstructured outdoor setting with trees, bushes and a gazebo. We evaluate the alignment in the same way as the Apartment dataset. Figure 3 shows some alignments of the point cloud obtained via BB+ICP. The high degree of clutter, noise and outliers is clearly visible. Despite those difficult conditions, the coarse alignment is correct. Table 1 lists results for the alignment of all scans in the dataset. It is clear that all algorithms have a harder time aligning the scans. Still BB performs well both in speed and quality. Specifically BB gets the coarse alignment right in almost all cases whereas GOGMA fails in 10% more cases. Looking at the alignments this is due to GOGMA vertically flipping scans this is also indicated by the high mean rotational error), whereas BB has no trouble finding the right upward direction due to the strong upward surface normal cluster from the ground.

7 Figure : Evaluation of translational and rotational error under additive isotropic Gaussian noise and outliers. Shaded areas show one standard deviation around the mean solid line). The median errors are indicated with dashed lines. Method BBλ BBλ + GOGMA GOGMA+ GoICP FT Rotation [ ] Translation [m] Inlier % C Inlier % M Inlier % F Time [s] Table 1: Gazebo Summer [1] results for BB, GOGMA, GoICP, FT. We report rotational Rot), translational Tran), timing, and inlier Inl) percentages for C)oarse m; 10 ), M)edium 1m; 5 ) and F)ine 0.5m;.5 ) alignment. Figure 3: Depiction of the BB+ICP alignment of the first 6 LiDAR scans of the Gazebo Summer dataset [1]. While details of the alignment could be improved, the overall large scale alignment is inferred correctly. Different colors indicate distinct LiDAR scans. References [1] F. Pomerleau, M. Liu, F. Colas, and R. Siegwart. Challenging data sets for point cloud registration algorithms. IJRR, 3114): , Dec , 7