This invention relates generally to registration of three-dimensional (3D) data, and more particularly to registering 3D points and 3D planes with each other for real-time reconstruction applications.
3D Reconstruction
Interactive and real-time 3D reconstruction of a scene is used in a number of applications, e.g., robotics, augmented reality, medical imaging, and computer vision. Real-time sparse and dense 3D reconstruction can use passive sensors such as cameras. However, passive sensors have difficulties to reconstruct textureless regions.
For reconstructing textureless regions, active 3D sensors can be used. For example, the Kinect sensor for the Microsoft Xbox uses an IR pattern for acquiring 3D data as a depth map from a viewpoint of the sensor in real time.
Other issues relate to processing time, memory requirement, and accuracy. Because of the field of view and resolution, 3D sensors usually produce a partial reconstruction of a scene. It is desired to provide an accurate and fast registration method that can combine successive partial depth maps and a model of the scene. Inertial sensors are prone to drift. Therefore, the features in an RGB (texture) image or depth map need to be relied on for accurate registration. In addition, depth maps are usually noisy without any higher-level spatial constraint. Furthermore, the point cloud requires a very large memory, and is difficult to compress.
3D-to-3D Registration
Local
Alignment or registration of 3D data is a fundamental problem in computer vision applications, which can be solved using several methods. The registration methods can be local or global. The local methods should start with a good initialization, and register two 3D point clouds using relatively small iterative moves. This is similar to a non-linear minimization method that converges to a global solution with a good initial solution. The most common local method is an iterative closest point (ICP) method, which iteratively determines corresponding 3D points and the moves using a closed-form solution.
Global
Global methods typically consider the entire 3D point cloud, identify some key geometric features (primitives), match the features across point clouds, and generate an optimal hypothesis using a minimal set of correspondences using a RANdom SAmple Consensus (RANSAC) procedure. The coarse registration obtained by global methods is usually followed by local non-linear refinement. Global methods, unlike local methods, do not require initialization. However, global methods can suffer from incorrect and insufficient correspondences. The geometric primitives typically used in global methods are either points, lines, or planes.
Several global registration methods using homogeneous and heterogenous correspondences are known. For example, it is possible to determine a closed-form solution for the registration given point-to-point, line-to-line, plane-to-plane, point-to-line, point-to-plane, or line-to-plane correspondences. One method obtains a global optimal solution from point-to-point, point-to-line, and point-to-plane correspondences using branch-and-bound. Another method uses branch-and-bound to obtain the optimal correspondences as well as transformation for the point-to-point registration problem.
SLAM Using 3D Sensors
In mobile robotics, some 3D-sensor-based methods use a simultaneous localization and mapping (SLAM) system for determining a motion of the sensor as well as reconstructing a scene structure. Those methods typically use geometric features such as point, line, or plane primitives. 3D sensors that provide a planar slice of 3D data, such as 2D laser scanners or ultrasonic sensors, can be used for determining planar, three degrees-of-freedom (DOF) motion. 3D sensors that provide full 3D point clouds, such as structured light scanners, 2D laser scanners attached on moving stages, and the Kinect sensor can be used for determining six DOF motion.
RGB-D mapping extracts keypoints from RGB images, back-projects the points in 3D using depth maps, and uses three point-to-point correspondences to determine an initial estimate of the pose using the RANSAC procedure, which is further refined using the ICP method.
Another method uses three plane-to-plane correspondences in a SLAM system with 3D sensors. That method addresses the correspondence problem using geometric constraints between planes.
Another method uses a combination of smaller field-of-view (FOV) 3D sensor and a larger FOV 2D laser scanner for the SLAM system using both planes and line segments as primitives. That method is designed for a sequential SLAM system that solves a local registration problem, and cannot solve global registration.
KinectFusion registers a current depth map with a virtual depth map generated from a global truncated signed distance function (TSDF) representation by using a coarse-to-fine ICP method. The TSDF representation integrates all previous depth maps registered into a global coordinate system, and enables higher-quality depth map generation than using a single image.
Several other variants of the ICP method are known, but the variants still suffer from local minima issues when the two 3D point clouds are distinct. Registration methods or SLAM systems that solely depend on points suffer from insufficient or incorrect correspondences in textureless regions or regions with repeated patterns. Plane-based techniques suffer from degeneracy issues in scenes containing insufficient number of non-parallel planes.
With 3D sensors such as the Kinect sensor, line correspondences are difficult to obtain because of noisy or missing depth values around depth discontinuities.
The embodiments of the invention provide a method for registering data in two different coordinate systems. The method uses both points and planes as registration primitives. The method can be used by a simultaneous localization and mapping (SLAM) system using 3D sensors. The SLAM system is an application of our registration method.
With the method, it is possible to register 3D data sets in two different coordinate systems using any combination of at least three point and plane primitives, i.e., 3 planes, 2 planes and 1 point, 1 plane and 2 points, and 3 points. The method is particularly concerned with a set of mixed (heterogeneous) primitives, that is at least two 3D points and at least one 3D plane, or at least one 3D point and at least two 3D planes.
That is, the method can use a minimal set of three primitives and a RANSAC procedure to register 3D data and reconstruct a scene. Because the number of planes is significantly smaller than the number of points in 3D data, the RANSAC procedure used by the preferred embodiments uses a primitive combination involving more planes than points, e.g., minimally two planes and one point.
In contrast to prior art methods that mainly use points for registration, the present method has the following advantages. The correspondence search and registration is faster due to the smaller number of plane primitives, and can be performed in real time. The method produces plane-based 3D models that are more compact than point-based models. The method provides global registration without suffering from local minima, or initialization problems as in the prior art local registration methods.
The embodiments of the invention provide a method for registering 3D data in two different coordinate systems. As shown in
The registration method can be used by a simultaneous localization and mapping (SLAM) system, as shown in
Our SLAM system selects 210 a set of 3D point and 3D plane primitives from the input (referred to as measurements 211) and registers 220 the points and planes with respect to a set of point and plane primitives in a global map 225 (referred to as landmarks 221). Each set includes at least one 3D point and at least one 3D plane. The third primitives in the sets can be either both points or both planes. The registration is performed by registering the points with points in the two sets, and the planes with planes in the two sets.
The SLAM system performs the registration method with a RANdom SAmple Consensus (RANSAC) procedure 220 using the minimal three primitives. Optimization 230 can be applied to the global map as additional inputs are acquired.
3D Points
Our SLAM system selects one or more keypoints in the 3D point cloud to obtain the 3D point primitives. The keypoints can be selected using 3D keypoint detectors from the 3D point cloud without using a texture image. Example 3D keypoint detectors include Normal Aligned Radial Feature (NARF) and 3D Speeded Up Robust Feature (SURF). Alternatively, the system can select 2D keypoints from each texture image using 2D keypoint detectors and back-project the keypoints using the corresponding depth value to obtain the 3D point primitives. Example 2D keypoint detectors include Scale-Invariant Feature Transform (SIFT) and Speeded Up Robust Feature (SURF). Those 3D and 2D keypoint detectors also provide a feature descriptor for each detected keypoints.
Each point primitive is represented by a position pm and a descriptor Dm, (pm,Dm) of a keypoint. The descriptor can be used for matching the 3D point primitives between the measurements and landmarks.
3D Planes
To select planes from the 3D point cloud, we use the following iterative procedure:
1. Randomly select a set of reference points in the 3D point cloud;
2. Determine an optimal plane for each reference point using nearby points inside a local window;
3. Determine all inliers that form a connected component with the reference point with respect to a grid graph of the depth map;
4. Identify an optimal plane with a sufficient and maximum number of inliers; and
5. Remove inliers from the 3D point cloud.
Each plane primitive is represented by plane parameters πm and a set of inliers Im. The plane parameters form a 4D vector πm=(nmT,dm)T, where nm is a unit normal vector, and dm is a distance to the origin of the coordinate system.
Registration
A pose 240 of the sensor i.e., a 6-DOF rigid body transformation with respect to a coordinate system of the global map) is determined by registering the primitives in the measurements with respect to the primitives in the landmarks in the global map. Our registration method using both points and planes and our RANSAC procedure are described below.
Map Update
Our SLAM system updates 250 the global map by adding point and plane primitives selected from keyframes. The current frame is considered a keyframe only if the pose is sufficiently different from previous keyframes (e.g., the translation and rotation of the current frame are different from the translations and rotations of previous keyframes by more than predetermined thresholds). Each point landmark is represented by the position pl and the descriptor Dl as ((pl,Dl). Each plane landmark is represented by plane parameters πl=(nlT,dl)T and a set of inliers Il as ((πl,Il).
Map Optimization
To jointly refine the registration between all keyframes, our SLAM system performs optimization 230 by bundle adjustment, using both the points and the planes. The bundle adjustment is performed in a separate thread asynchronously from a main thread for tracking the camera pose using the registration method. The details of the bundle adjustment are described below.
Registration Using Points and Planes
Our registration method uses point-to-point and plane-to-plane correspondences and provides a closed-form solution. The method is applicable to three or more correspondences. Thus, the method can be used to generate hypotheses using the minimal number of three correspondences in our RANSAC procedure, as well as to refine the initial hypothesis given by the RANSAC procedure using all inlier correspondences.
Closed-Form Solution for Mixed Point-to-Point and Plane-to-Plane Correspondences
Let {pi} and {p′i}, i=1, . . . , M, be corresponding 3D points, and {πj=(njT,dj)T} and {π′j=(n′jT,d′j)T}, j=1, . . . , N, be corresponding 3D planes in two different coordinate systems. We determine a 6-DOF rigid body transformation [R, t] between the coordinate systems, where R is a 3×3 rotation matrix and t is a 3×1 translation vector.
Solutions for Individual Cases
For point-to-point correspondences, the rotation and translation components can be decoupled. Let
be the centroids of the 3D point sets, and qi=pi−
is obtained by using quaternion representation of rotation or singular value decomposition (SVD). Using the estimated rotation {circumflex over (R)}, the translation is a difference between the rotated centroids
{circumflex over (t)}=
For the plane-to-plane correspondence case, the rotation is obtained by minimizing the error
which can be solved similar to the case of point-to-point correspondences. For determining the translation, we can stack the following liner constraint for three or more planes and solve the linear system:
n′jTt=d′j−dj. (4)
Here, T is a transpose operator.
Solution for Mixed Case
For the mixed case, we have point-to-point and plane-to-plane correspondences. We exploit the decoupling used in the individual cases to determine the rotation and the translation. To determine the rotation, we combine Eqs. (1) and (3) as
where wj are the weights for planes, which can change for each plane correspondence. Eq. (5) shares the same form as Eqs. (1) and (3), and the optimal rotation is obtained in the same manner. Specifically, we define a 3×3 correlation matrix K as
Let K=UDVT be a singular value decomposition (SVD) of K. Then, the optimal rotation {circumflex over (R)} is
To determine the translation t, we minimize the following error:
This corresponds to defining a linear system
with a diagonal weight matrix W=diag(M, M, M, w1, . . . , wN). Then, a weighted least-squares solution is t=(ATWA)−1ATWb.
Degeneracy Issues
To uniquely select R and t, the correlation matrix K in Eq. (6) and matrix A in Eq. (9) should satisfy certain conditions. To uniquely determine R, the rank of the correlation matrix K should be greater than one, and at least one of the following conditions should be true:
1. det(UVT)=1.
2. The minimum singular value of K is a simple root.
For the translation t to be uniquely determined, the matrix A in Eq. (9) should be rank three.
The matrices K and A satisfy the above properties if the correspondences possess at least one of the following: 3 planes, 2 planes and 1 point, 1 plane and 2 points, and 3 points. In the RANSAC procedure in our SLAM system, we use these minimal numbers of correspondences to generate hypotheses.
RANSAC Procedure
In contrast to correspondences just in a 2D image space, our 3D primitives provide several invariants that can be used to identify false matches. Given corresponding points and planes in two different coordinate systems, certain geometric entities determined in one coordinate system should substantially match the corresponding entities in the second coordinate system.
We identify the following three invariants in each coordinate system: I1 based on the distance between two points; I2 based on the distance between a point and a plane; and I3 based on the angle between two plane normals.
Corresponding geometric primitives can be associated with an invariant vector I=(i1,i2,i3), where i1, i2, and i3 correspond to the number of invariants with respect to the type I1, I2, and I3, respectively.
All the corresponding triplets involving points and planes possess a total of three invariants: 3 points: I=(3,0,0); 1 plane and 2 points; I=(1,2,0); 2 planes and 1 point: I=(0,2,1); and 3 planes: I=(0,0,3).
An interpretation tree or a branch-and-bound method can be used to identify false matches using these invariants. For our RANSAC procedure, we use a simple interpretation-tree-based pruning.
Prior to deleting mismatches based on the invariants, we obtain some initial correspondences. In the case of points, we use the SURF descriptors to obtain the correspondences. In the case of planes, we start with all possible correspondences and delete the mismatches based on the invariants. The RANSAC procedure that uses three planes is much faster than the one using three points because the number of planes in 3D data is usually much smaller than the corresponding number of points. Moreover, because planes are generated by many points, planes are less affected by noise in 3D data, leading to a more accurate registration. Therefore, as shown in
A termination condition for the RANSAC procedure can be a predetermined minimal number of correspondences to be sampled.
Bundle Adjustment with Points and Planes
Our SLAM system optimizes the following variables in the bundle adjustment: Point landmarks pli/=(xi,yi,zi), plane landmarks πlj=(aj,bj,cj,dj), and keyframe poses Tk=(txk,tyk,tzk,θxk,θyk,θzk).
Here, tk=(txk,tyk,tzk) are the (x,y,z) components of the translation of the kth keyframe, and θk=(θxk,θyk,θzk) represent the rotation around (x,y,z) axes. The rotation matrix Rk of the kth keyframe is represented by Rk=Rz(θzk)Ry(θyk)Rx(θxk).
We determine a Jacobian matrix using point/plane primitives associated with each point/plane landmark as follows.
Point Landmarks
For point landmarks, we minimize an Euclidean distance error between a point landmark pli and an associated point pmk=(xmk,ymk,zmk) in the kth keyframe, which is
∥pli−(Rkpmk+tk)∥=0. (10)
Using the current estimate of the landmark {circumflex over (p)}li=({circumflex over (x)}i,ŷi,{circumflex over (z)}i) and the keyframe pose [{circumflex over (R)}k,{circumflex over (t)}k], we linearize Eq. (10) as
From Eq. (11), we obtain three equations separately for each (x,y,z) component. The equation for the x component is
and those for the y and z components can be similarly obtained.
Plane Landmarks
For plane landmarks, we minimize a geometric error defined by a sum of distances between a plane landmark and 3D points sampled from associated planes in a keyframe. Specifically, we uniformly sample 3D points xmk,s from inlier 3D points of a plane primitive πmk, and determine the distance between each sampled point and the associated plane landmark πlj. Thus, the geometric error we minimize is
We linearize Eq. (14) using a current estimate of the plane landmark {circumflex over (π)}lj=(âj,{circumflex over (b)}j,ĉj,{circumflex over (d)}j), and the keyframe pose [{circumflex over (R)}k,{circumflex over (t)}k] as
where {circumflex over (x)}mk,s={circumflex over (R)}kxmk,s+{circumflex over (t)}k=({circumflex over (x)}mk,s,ŷmk,s,{circumflex over (z)}mk,s). After simplification, we have
Geometric Constraints Between Plane Landmarks
Our SLAM system can optionally enforce geometric constraints between plane landmarks in the bundle adjustment. The geometric constraints include parallelism and orthogonality. Orthogonal or parallel planes can be commonly observed in man-made environments, where enforcing the geometric constraints is useful to obtain an accurate registration.
For every pair of plane landmarks, πlj=(aj,bj,cj,dj) and πlj′=(aj′,bj′,cj′,dj′), we check whether the angle between normal vectors of the two planes is sufficiently close to 0 degrees (parallel) or 90 degrees (orthogonal) using a predetermined threshold. If true, then we enforce the following constraint
ajaj′+bjbj′+cjcj′=S, (17)
where S=1 if the two planes are sufficiently close to parallel, while S=0 if the two planes are sufficiently close to orthogonal.
Using current estimates of the two plane landmarks, {circumflex over (π)}lj=(âj,{circumflex over (b)}j,ĉj,{circumflex over (d)}j) and {circumflex over (π)}lj′=(âj′,{circumflex over (b)}j′,ĉj′,{circumflex over (d)}j′), we linearize Eq. (17) as
After simplification, Eq. (18) results in
Solution
Stacking Eqs. (13), (16), and (19) for all landmarks and keyframes results in a linear system JΔ=−ε0, where the Jacobain matrix is J, the error vector is ε0, and the update vector is Δ=(Δpli, Δπlj, ΔTk). We use a Gauss-Newton method to solve the optimization problem.
Implementation
Our real-time SLAM system uses texture images and depth maps having a 640×480 pixel resolution. We visualize the point and plane landmarks superimposed onto the current point cloud when the registration is successful. Our SLAM system always determines the global registration frame-by-frame with respect to the global map. Thus, registration failures do not affect subsequent inputs. Our SLAM system provides reconstructed plane landmarks as a plane-based representation of the scene, which is more compact and provides more semantic information than a point-based representation of the scene.
The invention provides a registration method that uses both points and planes as primitives to register 3D data in two different coordinate systems. Using the registration method, a real-time SLAM system for 3D sensors that uses both point-to-point and plane-to-plane correspondences for registration is developed. Using both primitives enables faster and more accurate registration than only using points. Our SLAM system generates a 3D model as a set of planes, which provides more compact and semantic information of the scene than, strictly point-based representations.
The invention shows that any combination of three 3D point/plane primitives allows for registration between two different coordinate systems. The invention also shows a bundle-adjustment framework that uses both 3D points and 3D planes to jointly optimize the registration between multiple coordinate systems. The bundle-adjustment framework can enforce geometric constraints between 3D planes, such as parallelism and orthogonality, to further improve the registration accuracy for man-made environments.
The use of planes enables both faster reconstruction and compact modeling. Local methods, such as iterative-closest point (ICP) methods, are prone to local minima issues under fast motion of the 3D sensor, while our registration method provides a global solution and avoids local minima problems.
The method is different from methods that extract plane primitives from 3D point clouds that are already registered. Instead, the method uses plane and point primitives selected from individual 3D point cloud for registration. Using planes with points in the registration enables a more efficient and accurate registration than using only points.
As shown in
Because the invention uses both points and planes, failures present in prior art methods are avoided. This mixed mode registration is novel.
A closed-form solution to registration is provided for point-to-point and plane-to-plane correspondences in a unified manner.
Although the invention has been described by way of examples of preferred embodiments, it is to be understood that various other adaptations and modifications can be made within the spirit and scope of the invention. Therefore, it is the object of the appended claims to cover all such variations and modifications as come within the true spirit and scope of the invention.
Number | Name | Date | Kind |
---|---|---|---|
7187809 | Zhao et al. | Mar 2007 | B2 |
8306684 | Nakano | Nov 2012 | B2 |
20040167667 | Goncalves et al. | Aug 2004 | A1 |
20070156286 | Yamauchi | Jul 2007 | A1 |
20110205338 | Choi et al. | Aug 2011 | A1 |
20120053728 | Theodorus et al. | Mar 2012 | A1 |
20120196679 | Newcombe et al. | Aug 2012 | A1 |
20120250978 | Taylor | Oct 2012 | A1 |
20130278719 | Rusert et al. | Oct 2013 | A1 |
Number | Date | Country |
---|---|---|
200928570 | Jul 2009 | TW |
Entry |
---|
Wright et al, “Convex Hulls, Occluding Contours, Aspect Graphs and the Hough Transform,” 1996, Image and Vision Computing 14, pp. 627-634. |
Plane (geometry) [online]. Wikipedia, 2014 [retrieved Feb. 25, 2014]. Retrieved from the Internet: < http://en.wikipedia.org/wiki/Plane—(geometry) >, p. 1. |
Besl et al, A Method for Registration of 3-D Shapes, 1992, IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 14, No. 2, pp. 239-256. |
Lee et al, A Prospective Algorithm for Real Plane Identification from 3D Point Clouds and 2D Edges, 2008, International Conference on Convergence and Hybrid Information Technology, IEEE, pp. 75-82. |
Jeffrey, Mathematics for Engineers and Scientists, 1996, Chapman & Hall, 5th Edition, vol. 1, p. 167. |
Number | Date | Country | |
---|---|---|---|
20140003705 A1 | Jan 2014 | US |