A brute force approach to depth camera odometry

Jonathan IsraelONERA - The French Aerospace Lab

jonathan.israel@onera.fr

Aurelien PlyerONERA - The French Aerospace Lab

LAGA - Paris 13aurelien.plyer@onera.fr

Abstract

By providing direct access to 3D information of the envi-ronment, depth cameras are particularly useful for percep-tion applications such as Simultaneous Localization AndMapping or object recognition. With the introduction ofthe Kinect in 2010, Microsoft released a low cost depthcamera that is now intensively used by researchers, espe-cially in the field of indoor robotics. This paper introducesa new 3D registration algorithm that can deal with consid-erable sensor motion. The proposed approach is designedto take advantage of the powerful computational scalabilityof Graphics Processing Units (GPU).

1. IntroductionFrame registration is one of the primary processing

stages that requires to be executed in many 3D video ap-plications. When a 3D sensor such as the Kinect is robot-mounted or hand-moved, its attitude variation can show sig-nificant movements, especially large rotations, which maylead a local registration method such as Iterative ClosestPoint (ICP) to fail. Furthermore, in the case of the Kinect,the temporal registration of the huge amount of data cap-tured by the sensor (640 × 480 16-bits pixels at 30Hz) canhardly be fully processed in real-time applications. Themethod presented in this paper relies on a probabilisticframework where a global matching criterion applied to ex-tracted features can be evaluated in parallel in the projec-tive plane defined by the sensor camera. We show that moststeps of this algorithm suit particularly well to a real-timeGPU-based implementation since they rely mostly on vec-tor and matrix multiplications or element-wise operations.

This document is organized as follows. We give a briefoverview of 3D registration methods in section 2. The gen-eral framework and the key steps of the proposed methodare described in section 3. We demonstrate experimentallyin section 4 the suitability of our approach for real-timeprocessing. Conclusions and perspectives are given in sec-tion 5.

2. Related workThe objective of 3D registration is to find the Euclidean

motion between two 3D data sets such as range images orpoint clouds. When a good estimate of the relative trans-formation is available, the registration problem can be ef-ficiently solved using local registration methods. Since itsintroduction by Besl and McKay [2], the ICP technique hasbeen widely used by the community. As they are basedon the underlying assumption that a good initialization isknown, ICP-like techniques can easily get trapped in a localminimum. Even in the presence of small displacements, theEuclidean distance used in ICP suffers from its difficulty tocapture the sensor rotation. In [1], the authors overcome thislimitation by introducing a new metric that can be used forpoint-to-surface distance estimation in the matching phase.The robustness of this modified ICP comes at a cost of ahigher computational complexity, which is already anotherdrawback of local and iterative registration methods. For in-stance, the most time-consuming stage of the ICP is the de-termination of the closest point, which is basically ofO(n2)for 3D scans containing n points. Optimized search struc-tures like k−d trees or box decomposition trees are used innumerous publications such as [7] or [12] to reach a com-plexity of O(n log n).

Reduced search spaces can also be used during the cor-respondence stage. For instance, [1] use a sliding windowwhich limits candidates for each point within an angularwindow. In [9], the neighbourhood relationships in bothframes are used to restrict recursively the nearest neighboursearch. Several approximation techniques such as the latterallow us to reach aO(n) complexity but are not necessarilyconvenient for a massively parallel implementation, mainlydue to their recursive nature.

In order to further decrease the computational complex-ity of the registration, several authors reduce directly thenumber of points that are processed. In [13], the authorsperform point reduction and generate approximate k − dtrees by subsampling the data depending on their distanceto the sensor. A Frustrum ICP is used in [11] to removeiteratively 3D points from non-overlapping areas. The reg-

2011 IEEE International Conference on Computer Vision Workshops978-1-4673-0063-6/11/$26.00 c©2011 IEEE

1141

istration precision of subsampling techniques that dismissor approximate some of the original data depend stronglyon the subsampling scheme. Nevertheless, those techniquesremain fast and efficient and real-time processing has evenbeen recently reported on subsampled RGB-D data [6]. Fi-nally, reduction of the computational payload can also beachieved while preserving efficiency with estimations basedon feature matching. Registration is then performed directlyon selected points with low-dimension features [4, 17] or onextracted 3D contours [8, 14]. Those kinds of approachescan be extended to a multi-resolution framework wheredetails are introduced incrementally to increase the solu-tion resolution while controlling the computational payload[18].

While optimization techniques tried mostly to reduce thedata size or to approximate the nearest neighbour searchstage, several implementations of recent algorithms wereadapted to take advantage of the computational power ofthe GPU. Different variants of the ICP such as Modified It-erative Closest Point [10], Expectation-Maximization ICPand soft-assign [19] or ICP based on k − d tree prioritysearch [16] allowed a huge reduction of the processing timewhile preserving robustness and precision. [15] presents aCUDA-based implementation of a point-to-plane 3D reg-istration technique where the costly corresponding pointsearch is replaced with a direct estimation based on a pointto plane projection. In [5], exhaustive search in a reduceddimension space is performed.

By computing in a projective space a matching crite-rion based on extracted 3D features, the method proposedin this paper allows a fast and exhaustive search throughthe full 6 degree-of-freedom space. As opposed to most ofICP-derivated techniques, it does not rely on any one-to-one point correspondence assumption or any reweightingscheme whose parameters are often hard to tune automati-cally.

3. Proposed method

We propose in this paper an approach that takes advan-tage of the particular geometry of the depth image, in con-trary to many ICP-like techniques that are designed for thegeneral registration of two unstructured 3D point clouds.In our case, a 3D point cloud computed from a depth im-age is associated with the inverse projection of pixel depthmeasurements. This implies that the sensor depth imageplane has a coherent projective geometry that can be usedto compute approximate neighbourhoods and distances. Amatching criterion can be computed very efficiently in thisprojective space and thus evaluated in parallel in the full 6Dtransformation space. To achieve fast computation on re-cent and massively parallel processors, we comply with thetwo following rules:

��

��

���������������

���������������

������ ���������

Figure 1. Global decomposition of the algorithm. The processingwhich is executed at each data acquisition from a video stream isencircled in orange.

• the algorithm will have a Single Process, Multiple Datascheme for parallel scalability,

• the criterion and projective operations will rely asmuch as possible on vector and matrix multiplicationsor element-wise operations.

Sparse structures (3D contours corresponding to a list ofedges) are extracted from the depth image and used in apose score evaluation for different movement hypotheses.A likelihood function is defined on the set of all possibletransformations and used in the final decision process. Inthe following section, we will develop our approach fromthe abstract mathematical framework to the implementationdetails.

3.1. Algorithm overview

The proposed algorithm includes two main stages, seefig. 1. In a first step, a set of features F1 = {F1

i }i=1...N1

(resp. F2 = {F2i }i=1...N2 ) is extracted from the reference

depth image d1 (resp. the current depth image d2). These3D features are derived from a 2D criterion map which isdefined on the projective plane of each depth image. In ourexperiment, we choose the depth image gradient to obtain3D contours as shown in fig. 5b. In a second step, we evalu-ate simultaneously the likelihood function L(θ|d1,d2) overa large set of hypotheses with displacement parameters be-longing to the 6D space Θ = {θl}l=1...M . This likelihood isdefined as a normalized score function computed on the setof features extracted from both frames under the hypothesisθ:

L(θ|d1,d2) =1

L

N2∑i=1

g(F2i , Sθ(F

2i ,F1)) (1)

where L is a normalization factor and g is a score functional.The score function Sθ is the average value of the integratedpseudo-distance from a feature F2

i extracted from the cur-rent frame to the nearest features of the reference frame un-der the transformation hypothesis θ:

Sθ(F2i ,F1) =

1

‖F2i ‖

∫F2i

||N1(P1(φθ(X)))− φθ(X)||dX

(2)

1142

In eq.(2), X ∈ R3 belongs to the feature F2i and

φθ : R3 → R3 is the 3D Euclidean transformation corre-sponding to the current parameter θ. ‖F2

i ‖ is the arc lengthof the 3D contour F2

i . P1 : R3 → R2 is the projection fromR3 to the reference depth image plane. Its computation sup-poses that the sensor calibration has been done previously.N1 : R2 → R3 is the function that gives for every 3D pointprojected in the reference depth image plane the nearest 3Dpoint that belongs to a feature extracted from the referencedata. In the score evaluation, the nearest point is not founddirectly in R3. Instead, its approximation is given by theN1 function applied to the projection of the feature F2

i inthe reference plane under the current hypothesis with trans-formation parameter θ. This allows a very fast processingas we will see in the implementation details of section 3.3.

3.2. Pratical issues

In this section we present the chosen instantiations ofthe feature extraction and the score evaluation used in theproposed approach.

Feature extraction

The depth image gradient is computed with a Sobel mask.We use a classical edge linking scheme based on the thresh-olded gradient norm image and the gradient orientation.The features are 3D contours corresponding to the repro-jection in R3 of the detected edges. This approach can beeasily extended to any kind of features such as segments orcurvature based salient keypoints for instance.

Score evaluation

In order to favour long edges that are projected close toedges from the reference frame, we will choose a functionalg defined by:

g(Fki , Sθ) = ‖Fki ‖e−αSθ (3)

In all our experiments, α was set to 1.

3.3. Implementation details

The proposed approach is tightly linked with implemen-tation possibilities given on recent computer architectures.

Two important processing blocks are detailed here:

• the computing of the Nk function based on extractedfeatures,

• the evaluation of the likelihood L(θ|d1,d2).

The N1 function aims at providing a fast access to thenearest 3D points belonging to a feature in the reference im-age projective plane. First, a gradient convolution algorithmis used to compute the norm gradient image. Then, based

� � � � � �

� �

�

�

�

�

� � � �

�

�

�

�

� � � �

�

�

�

� � �

�

�

� �

��

�� �� �� �� �� �� �� ��� ����� �

Figure 2. Tiled parallel version of the causal filter used during thefirst scan in the distance map computation. The processor ID as-sociated with each tile is indicated. Time dependencies betweentile processing are represented by arrows. Since the blocks in eachdiagonal can be executed simultaneously, the basic execution timewill be dominated by the time corresponding to 11 block execu-tions instead of 36 in a fully sequential approach.

on two successive scans of the thresholded gradient image,we compute a Chebyshev distance map that keeps track ofthe coordinates of the nearest feature. The first scan (resp.the second scan) is a direct scan (resp. an inverse scan) ofthe thresholded gradient image where the computation ofeach block is performed after the computation of the up andleft (resp. down and right) blocks. This stage is the mainsequential part of our implementation but remains very fastfor our depth images (taking around 20ms in practice). Aswe show in fig. 2, this algorithm can be easily parallelizedwith a tiled strategy. For instance, a theoretical speed-upfactor of 3 (which corresponds roughly to the 36

11 gain factor)can be reached on the basis of a 6 processor core workingon 36 tiles of a binary contour image. For access efficiency,N1 has been put in texture memory.

����������������

Figure 3. Cuda implementation of the likelihood function. Greenblocks are Cuda computation blocks and red circles are Cudathreads.

The likelihood hypothesis test has been totally imple-mented in CUDA and performed on the GPU. Special at-tention has been paid to the mapping beetwen data and thecomputing hierarchy (grid, blocks, threads). We assignedone hypothesis to each Cuda block and map the differentthreads of each block to features, as illustrated in fig. 3.

Each thread computes a feature scoreg(F2

i , Sθk(F2i ,F1)) with the help of a fast Bresen-

1143

0 0.1 0.2 0.3 0.4 0.5 0.60

0.01

0.02

0.03

0.04

0.05

0.06

0.07

0 2 4 6 8 10 12 14 160

0.2

0.4

0.6

0.8

1

1.2

1.4

1.6

1.8

2

Figure 4. Left: Translation residuals w.r.t the initial translation (in meter). Right : Rotation residuals w.r.t the initial rotation (in degree).

ham line-drawing algorithm [3]. After the reduction ofthreads, each block computes the likelihood L(θk|d1,d2)under the hypothesis θk.

4. Experimental results

4.1. Precision analysis

We followed the methodology proposed in [1] to eval-uate the precision of our method. We matched each of 30scans against itself with random initial sensor location. Allthe scans were taken in the conference room shown fig. 5a.The sensor initial translation varied from 0m to 0.5m whileits rotation on all axes (roll, pitch, yaw) varied from 0◦ to15◦. We evaluated separately the translation and rotationeffects. The residual rotations and translations are given infig. 4. Since rotation residuals are mostly related to rollangles, we represent only roll angle residuals w.r.t. ini-tial roll angles. The mean registration error remains quitelow even if it shows a significant quantization effect (dueto the search space quantization and to approximations inthe nearest neighbour search). In practice, the quantizationstep should be chosen accordingly with the desired preci-sion. The average time processing per hypothesis rangedfrom 0.5µs to 1.5µs depending on the total number of hy-potheses. Evaluation was performed on a computer whosespecifications are given in table 1. We emphasize the factthat in practice and due to prior constraints or pose predic-tion, the search space will never be isotropic and will focusaround specific values.

Table 1. Hardware specifications

CPU Intel Core i7 @ 2.67 GHz

Memory 8 Go

GPU nVidia GeForce GTX 460

OS Linux 2.6

4.2. Qualitative evaluation

We tested our approach on a depth image sequenceof several minutes that was captured with a hand-moved Kinect. Several tests have been conducted, witha time separation ranging from 0.1s to 1s betweenthe reference frame and the current one. The searchspace parameters {Tx, Ty, Tz, θ, φ, ψ} were defined ac-cordingly, ranging from {0.1m, 0.1m, 0.1m, 5◦, 5◦, 5◦} to{0.5m, 0.5m, 0.5m, 15◦, 15◦, 15◦}. The number of steps ineach dimension was chosen from 6 to 13, thus the computa-tion time ranged from a few dozen of ms to a few seconds.As can be seen in fig. 5, good results were obtained on struc-tured environments. The registration showed heterogeneousquality in “3D textureless” environments with low geomet-ric saliency like corridors. As we expected, results are in-sensitive to large sensor motions as long as the search spacebounds are large enough to include the solution.

5. Conclusion and future work

In this paper, we described and presented an experimen-tal validation of a new registration method based on a sim-ilarity metric defined in the camera projective plane. Thefocus was on the derivation of the likelihood used in thematching process. Experimental results have shown thatthis brute force algorithm can be used to provide real-timeregistration of depth camera data. Future work will focuson coupling the pose estimation with a particle filtering ap-proach and its embedding in a global and multi-scale SLAMframework.

References

[1] L. Armesto, J. Minguez, and L. Montesano. A generalizationof the metric-based iterative closest point technique for 3dscan matching. In Robotics and Automation (ICRA), 2010IEEE International Conference on, pages 1367 –1372, may2010.

1144

������������������

�����������������

�����������������������

������

Figure 5. a) The RGB image of a conference room. b) The Kinect depth image. Extracted features are superimposed in red. c) A viewof the reference frame (red) and the current frame before registration (blue). d) The reference frame (red) and the current frame afterregistration (green).

[2] P. Besl and H. McKay. A method for registration of 3-dshapes. Pattern Analysis and Machine Intelligence, IEEETransactions on, 14(2):239 –256, feb 1992.

[3] J. E. Bresenham. Algorithm for computer control of a digitalplotter. IBM Syst. J., 4:25–30, March 1965.

[4] J. Chu and C. mei Nie. Multi-view point clouds registrationand stitching based on sift feature. In Computer Researchand Development (ICCRD), 2011 3rd International Confer-ence on, volume 1, pages 274 –278, march 2011.

[5] O. Enqvist, F. Jiang, and F. Kahl. A brute-force algorithm forreconstructing a scene from two projections. In Conferenceon Computer Vision and Pattern Recognition, 2011.

[6] N. Fioraio and K. Konolige. Realtime visual and point cloudslam. In Robotics Science and Systems Conference (RSS2011).

[7] M. Greenspan and M. Yurick. Approximate k-d tree searchfor efficient icp. In 3-D Digital Imaging and Modeling, 2003.3DIM 2003. Proceedings. Fourth International Conferenceon, pages 442 – 448, oct. 2003.

[8] S. Hu, H. Zha, and A. Zhang. Registration of multiple laserscans based on 3d contour features. In Information Visual-ization, 2006. IV 2006. Tenth International Conference on,pages 725 –730, july 2006.

[9] T. Jost and H. Hugli. A multi-resolution icp with heuris-tic closest point search for fast and robust 3d registration ofrange images. In 3-D Digital Imaging and Modeling, 2003.3DIM 2003. Proceedings. Fourth International Conferenceon, pages 427 – 433, oct. 2003.

[10] Y. Kitaaki, H. Okuda, H. Kage, and K. Sumi. High speed 3-d registration using gpu. In SICE Annual Conference, 2008,pages 3055 –3059, aug. 2008.

[11] S. May, D. Droeschel, S. Fuchs, D. Holz, and A. Nuchter.Robust 3d-mapping with time-of-flight cameras. In Intelli-gent Robots and Systems, 2009. IROS 2009. IEEE/RSJ Inter-national Conference on, pages 1673 –1678, oct. 2009.

[12] A. Nuchter, K. Lingemann, and J. Hertzberg. Cached k-dtree search for icp algorithms. In 3-D Digital Imaging andModeling, 2007. 3DIM ’07. Sixth International Conferenceon, pages 419 –426, aug. 2007.

1145

[13] A. Nuchter, K. Lingemann, J. Hertzberg, and H. Surmann.6d slam with approximate data association. In AdvancedRobotics, 2005. ICAR ’05. Proceedings., 12th InternationalConference on, pages 242 –249, july 2005.

[14] K. Ohno, T. Nomura, and S. Tadokoro. Real-time robot tra-jectory estimation and 3d map construction using 3d camera.In Intelligent Robots and Systems, 2006 IEEE/RSJ Interna-tional Conference on, pages 5279 –5285, oct. 2006.

[15] S.-Y. Park, S.-I. Choi, J. Moon, J. Kim, and Y. W. Park. Real-time 3d registration of stereo-vision based range images us-ing gpu. In Applications of Computer Vision (WACV), 2009Workshop on, pages 1 –6, dec. 2009.

[16] D. Qiu, S. May, and A. Nuchter. Gpu-accelerated nearestneighbor search for 3d registration. In Proceedings of the7th International Conference on Computer Vision Systems:Computer Vision Systems, ICVS ’09, pages 194–203, Berlin,Heidelberg, 2009. Springer-Verlag.

[17] R. Rusu, N. Blodow, Z. Marton, and M. Beetz. Aligningpoint cloud views using persistent feature histograms. In In-telligent Robots and Systems, 2008. IROS 2008. IEEE/RSJInternational Conference on, pages 3384 –3391, sept. 2008.

[18] M. Strand, F. Erb, and R. Dillmann. Range image registra-tion using an octree based matching strategy. In Mechatron-ics and Automation, 2007. ICMA 2007. International Con-ference on, pages 1622 –1627, aug. 2007.

[19] T. Tamaki, M. Abe, B. Raytchev, and K. Kaneda. Soft-assign and em-icp on gpu. In Networking and Computing(ICNC), 2010 First International Conference on, pages 179–183, nov. 2010.

1146