Point Cloud Registration Summary
2023-4-27 11:24:41
Catalogue
Global Registration
Global registration is a feature based registration method, it doesn’t need an alignment for initialization. In Open3D, the global registration is used to get the rough alignment, and use its results as the initialization of the local registration methods, such as ICP(Iterative Closest Point).
In Open3D pipeline, the global registration contains several steps: extract geometry features and RANSAC. In this section, each step and its basic knowledge will be illustrated.
Extract Geometry
First of all, the point cloud need to be downsampled to voxels, then using the fast point feature histograms(FPFH) to get the feature vectors. In Open3D, the method to downsample the point cloud into voxels is:
Then normals of these voxels could be extimated:
radius_normal = voxel_size * 2
print(":: Estimate normal with search radius %.3f." % radius_normal)
pcd_down.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))
Here the normal estimation function using KDTreeSearchParaHybridas parameter to accelerate. One of the most important parts of global registration is, feature vector extraction. The Open3D provide the method proposed by R.Rusu and N.Bloddow[2009].
radius_feature = voxel_size * 10
print(":: Compute FPFH feature with search radius {:.3f}" .format(radius_feature))
pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
pcd_down,
o3d.geometry.KDTreeSearchParamHybrid(radius_feature, max_nn=100))
RANSAC
After extracting the feature vector(in Open3D is a 33 dimensions space), we can align the point cloud roughly using RANSAC. In each RANSAC iteration, ransac_nrandom points are picked from the source point cloud. Their corresponding points in the target point cloud are detected by querying the nearest neighbor in the 33-dimensional FPFH feature space. A pruning step takes fast pruning algorithms to quickly reject false matches early.
result = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
source_down, target_down, source_fpfh, target_fpfh, True, distance_threshold,
o3d.pipelines.registration.TransformationEstimationPointToPoint(False),
3, [
o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.99),
o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(distance_threshold)],
o3d.pipelines.registration.RANSACConvergenceCriteria(1000000000, 0.999))
The parameters in the function, for example CorrespondenceCheckerBasedOnDistance, CorrespondenceCheckerBasedOnEdgeLengthand CorrespondenceCheckerBasedOnNormalare represent for the threshold of if aligned point clouds are close, if the lengths of any two arbitrary edges (line formed by two vertices) individually drawn from source and target correspondences are similarand vertex normal affinity of any correspondences, respectively. The detail could be found in the document of Open3D.
Local Refine
Point-to-plane ICP
To achieve the best result, the Open3D recommends using ICP for final registration adjustment.
result = o3d.pipelines.registration.registration_icp(
source, target, distance_threshold, result_ransac.transformation,
o3d.pipelines.registration.TransformationEstimationPointToPlane())
Basic Knowledge
Voxel
Voxel is a unique presentation for point cloud in 3D computer graphics. Just as the pixels in the 2D bitmap, the voxel does not contain the coordinates or value, it just regularly sampled the whole space.
KDTree Search
KDTree(k-dimensional tree) is usually used in the nearest neighbor and approximate nearest neighbor search.
Construct a KDTree
Input: Point Cloud with k dimension
Output: KDTree
1. Initilize the seperating axis: Calculate the variance of each of the dimensions, setthe largest variance dimension as the axis, mark it as r.
2. Determine the node: Search the current dataset along the axis, find out the median value data and puting it at the current node.
3. Decide branch:
Left branch: At the current axis, the data that value is smaller than the median divided to the left branch;
Right branch: At the current axis, the data that value is larger than the median divided to the right branch.
4. Update the axis: r = (r+1) % k.
5. Determine the child node:
Left node: Do the step 2 at left branch;
Right node: Do the step 2 at right branch.
Nearest Neighbour Search
The nearest neighbor search(NN) for KDTree is similar to KNN, by using the tree properties, KDTree search can be done efficiently to quickly eliminate large portions of the search space. The proceeds are as follows:
1. Starting with the root node, the algorithm moves down the tree recursively, in the same way that it would if the search point were being inserted (i.e. it goes left or right depending on whether the point is lesser than or greater than the current node in the split dimension).
2. Once the algorithm reaches a leaf node, it checks the node point and if the distance is better than the "current best", that node point is saved as the "current best".
3. The algorithm unwinds the recursion of the tree, performing the following steps at each node:
1. If the current node is closer than the current best, then it becomes the current best.
2. The algorithm checks whether there could be any points on the other side of the splitting plane that are closer to the search point than the current best. In concept, this is done by intersecting the splitting hyperplane with a hypersphere around the search point that has a radius equal to the current nearest distance. Since the hyperplanes are all axis-aligned this is implemented as a simple comparison to see whether the distance between the splitting coordinate of the search point and current node is lesser than the distance (overall coordinates) from the search point to the current best.
1. If the hypersphere crosses the plane, there could be nearer points on the other side of the plane, so the algorithm must move down the other branch of the tree from the current node looking for closer points, following the same recursive process as the entire search.
2. If the hypersphere doesn't intersect the splitting plane, then the algorithm continues walking up the tree, and the entire branch on the other side of that node is eliminated.
4. When the algorithm finishes this process for the root node, then the search is complete.
The hyperplane or the hypersphere are the plane or the sphere 1 dimension less than a ambient plane or a standard n-sphere.
KDTree and KNN
The algorithm can be extended in several ways by simple modifications. It can provide the k nearest neighbours to a point by maintaining k current bests instead of just one. A branch is only eliminated when k points have been found and the branch cannot have points closer than any of the k current bests.
FPFH Feature Extraction1
FPFH(fast point feature histograms), is a simplified version of PFH(point feature histogram).
PFH:
The PFH use several descriptors to construct the relationship of a point and its nearest neighbor in a given distance. The algorithm follows:
-
For each point p p p, all of p p p’s neighbors enclosed in the sphere with a given radius r are selected(k-neighborhood);
-
For every pair of points p i p_i pi and p j p_j pj ( i ≠ j ) (i\ne j) (i=j) in the k-neighborhood of p p p and their estimated normals n i n_i ni and n j n_j nj( p i p_i pi being the point with a smaller angle between its associated normal and the line connecting the points), we define a Darboux w v n wvn wvn frame ( u = n i , v = ( p i − p j ) × u , w = u × v u=n_i, v=(p_i-p_j)\times u, w=u\times v u=ni,v=(pi−pj)×u,w=u×v) and compute the angular variations of n i n_i ni and n j n_j nj as follows:
α = v ⋅ n j ϕ = ( u ⋅ ( p j − p i ) ) ∖ ∥ p j − p i ∥ θ = a r c t a n ( w ⋅ n j , u ⋅ n j ) (1) \begin{aligned} & \alpha=v\cdot n_j \\ & \phi=(u\cdot (p_j-p_i))\setminus \left \| p_j-p_i \right \| \\ & \theta=arctan(w\cdot n_j,u\cdot n_j) \end{aligned} \tag{1} α=v⋅njϕ=(u⋅(pj−pi))∖∥pj−pi∥θ=arctan(w⋅nj,u⋅nj)(1)
The PFH descriptor is consist of these four descriptors. To build a histogram, the algorithm analysis all the tetrads, that is, to divide the eigenvalues space into b sub-spaces, and count the point number in these sub-spaces. Finally, PFH get histogram according to all the two-point relationship of neighbor, the complexity is O ( n ⋅ k 2 ) O(n\cdot k^2) O(n⋅k2). Thus, the PFH uses a histogram to describe the space feature of points and their neighbors(Normal estimation is required) .
FPFH:
In the paper proposed by Rusu1 , a faster version of FPH is illustrated, named fast point feature histograms(FPFH). This algorithm could decrease the calculation complexity to O ( n ⋅ k ) O(n\cdot k) O(n⋅k), but keep the main ability of detecting features.
- In a first step, for each query point p p p we compute only the relationships between itself and its neighbors – we will call this the Simplified Point Feature Histogram (SPFH);
F P F H ( p ) = S P F ( p ) + 1 k ∑ i = 1 k 1 w k ⋅ S P F ( p k ) (2) \begin{align*} &FPFH(p)=SPF(p)+\frac{1}{k} \sum_{i=1}^{k} \frac{1}{w_k} \cdot SPF(p_k) \end{align*}\tag{2} FPFH(p)=SPF(p)+k1i=1∑kwk1⋅SPF(pk)(2)
where the weight ω k ω_k ωk represents the distance between query point p p p and a neighbor point p k p_k pk in a given metric space.
- For each point we re-determine its k k k neighbors and use the neighboring SPFH values to weight the final histogram of p p p (called FPFH). Usually use KDTree searching.
The FPFH does not fully interconnect all neighbors of p q p_q pq , and thus missing some value pairs which might contribute to capture the geometry around p q p_q pq; the PFH models a precisely determined surface around p q p_q pq while the FPFH includes additional point pairs outside the r radius sphere(though at most 2r away); Finally, Because the re-weighting scheme, the FPFH combines SPFH values and re-captures some of the point neighboring value pairs.
RANSAC2
Random sample consensus (RANSAC) is an iterative method to estimate parameters of a mathematical model from a set of observed data that contains outliers, when outliers are to be accorded no influence on the values of the estimates. It is a learning technique to estimate parameters of a model by random sampling of observed data.
RANSAC uses the voting scheme to find the optimal fitting result. Data elements in the dataset are used to vote for one or multiple models. The implementation of this voting scheme is based on two assumptions: that the noisy features will not vote consistently for any single model (few outliers) and there are enough features to agree on a good model (few missing data).
- Randomly select a sample subset, which contains the minimal amounts to fit a model;
- The algorithm checks the elements in the entire dataset and selects the data that consistent with the estimated model in the step 1 as inliers, and the others that do not fit the model within some error threshold are as outliers.
RANSAC will iteratively repeat these two steps until the obtained consensus set in certain iteration has enough inliers. The algorithm detail goes like:
1. Select a random subset of the original data. Call this subset the hypothetical inliers.
2. A model is fitted to the set of hypothetical inliers.
3. All data are then tested against the fitted model. All the data points (of the original data) that fit the estimated model well, according to some model-specific loss function, are called the consensus set (i.e., the set of inliers for the model).
4. The estimated model is reasonably good if sufficiently many data points have been classified as a part of the consensus set.
5. The model may be improved by re-estimating it by using all the members of the consensus set. The fitting quality as a measure of how well the model fits to the consensus set will be used to sharpen the model fitting as iterations goes on (e.g., by setting this measure as the fitting quality criteria at the next iteration).
ICP Registration
ICP(Iterative Closest Point) has been widely used for point cloud registration in both research and industry. In these part, 3 types of ICP algorithms will be introduced, refer to the paper of Rusinkiewicz20013. Generally, the aim of ICP algorithm is to find a mapping from source point cloud to target point cloud, as two steps:
- Find correspondence K = { ( p , q ) } \Kappa=\{(p,q)\} K={(p,q)}from target point cloud P P P, and source point cloud Q Q Q transformed with current transformation matrix T T T.
- Update the transformation T T T by minimizing an objective function E ( T ) E(T) E(T) define over the correspondence set K \Kappa K.
Point-to-point ICP
The point-to-point ICP algorithm is proposed by BeslandMcKay19924.
E ( T ) = ∑ ( p ， q ) ∈ κ ∥ p − T q ∥ 2 (3) \begin{align*} &E(T)=\sum_{(p，q)\in \kappa}^{} \left \| p-Tq \right \|^2 \end{align*}\tag{3} E(T)=(p，q)∈κ∑∥p−Tq∥2(3)
This algorithm is to find the mapping T T T, which minimizes the overall space distance between the target point cloud and mapped one.
Point-to-plane ICP
The point-to-plane ICP algorithm is proposed by ChenAndMedioni19925.
E ( T ) = ∑ ( p ， q ) ∈ κ ( ( p − T q ) ⋅ n p ) 2 (4) \begin{align*} &E(T)=\sum_{(p，q)\in \kappa}^{} ((p-Tq)\cdot n_p)^2 \end{align*}\tag{4} E(T)=(p，q)∈κ∑((p−Tq)⋅np)2(4)
Where the n p n_p np is the normal of point P P P. This algorithm not only keep the space distance but also the angle of normal and connect line of correspondence smallest. The point-to-plane ICP algorithm has a faster convergence speed than the point-to-point ICP algorithm.
There are many other derivative algorithms of ICP, such as Robust ICP, Colored point cloud registration and so on, will not mention here. However, most of these ICP algorithms need a initial mapping, which is key to the overall registration result. That’s why using global registration to roughly align the point cloud as the initial. As for 3D face scanning, I believe we can obtain the initial mapping from feature detection, such as the face feature detector of DNN and cascade. I will talk about this in another blog.
Code Example
Here is the code of global registration, along with a fast version proposed by Zhou20166. We should mind that, the RANSAC is a randomly optimizing algorithm, whose results heavily depend on the parameters in FPFH feature extraction, or the voxel size, iteration times and other thresholds. As I tried, this global and local combined algorithm is not suitable for precise point cloud registration, because we may need to change the parameters time by time. And if we set the voxel to small or iteration time too large, we may get more accurate results, but the time cost must not be neglected.
# -*- coding: utf-8 -*-
"""
Created on Wed Apr 26 15:32:03 2023
@author: weist
"""
import open3d as o3d
import numpy as np
import copy
import time
import os
import sys
def draw_registration_result(source, target, transform):
#ply1.paint_uniform_color([1, 0.706, 0])
#ply2.paint_uniform_color([0, 0.651, 0.929])
source_view = copy.deepcopy(source)
target_view = copy.deepcopy(target)
source_view.transform(transform)
o3d.visualization.draw_geometries([source_view, target_view])
def save_pcd(source, target, transform):
Combined_pcd = source.transform(transform)+target
o3d.io.write_point_cloud('Mapped_PCD.ply', Combined_pcd)
def preprocess_point_cloud(pcd, voxel_size):
print(":: Downsample with a voxel size {:.3f}" .format(voxel_size))
pcd_down = pcd.voxel_down_sample(voxel_size)
radius_normal = voxel_size * 2
print(":: Estimate normal with search radius {:.3f}" .format(radius_normal))
pcd_down.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))
radius_feature = voxel_size * 5
print(":: Compute FPFH feature with search radius {:.3f}" .format(radius_feature))
pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
pcd_down,
o3d.geometry.KDTreeSearchParamHybrid(radius_feature, max_nn=100))
return pcd_down, pcd_fpfh
def execute_global_registration(source_down, target_down, source_fpfh, target_fpfh, voxel_size):
distance_threshold = voxel_size * 1.5
# distance_threshold = voxel_size * 0.8
print(":: RANSAC registration on downsampled point clouds.")
print(" Since the downsampling voxel size is {:.3f}" .format(voxel_size))
print(" we use a liberal distance threshold {:.3f}" .format(distance_threshold))
result = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
source_down, target_down, source_fpfh, target_fpfh, True, distance_threshold,
o3d.pipelines.registration.TransformationEstimationPointToPoint(False),
3, [
o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(distance_threshold)],
o3d.pipelines.registration.RANSACConvergenceCriteria(100000, 0.999))
return result
def refine_registration(source, target, result_ransac, voxel_size):
distance_threshold = voxel_size * 0.4
print(":: Point-to-plane ICP registration is applied on orginal point")
print(" cloud to refine the alignment. This time we use a stirct")
print(" distance threshold {:.3f}" .format(distance_threshold))
result = o3d.pipelines.registration.registration_icp(
source, target, distance_threshold, result_ransac.transformation,
o3d.pipelines.registration.TransformationEstimationPointToPlane())
return result
def execute_fast_global_registration(source_down, target_down, source_fpfh, target_fpfh, voxel_size):
distance_threshold = voxel_size * 0.4
print(":: Apply fast global registration with distance threshold {:.3f}" .format(distance_threshold))
result = o3d.pipelines.registration.registration_fgr_based_on_feature_matching(
source_down, target_down, source_fpfh, target_fpfh,
o3d.pipelines.registration.FastGlobalRegistrationOption(maximum_correspondence_distance=distance_threshold))
return result
if __name__ == '__main__':
demo = o3d.data.DemoICPPointClouds()
ply1 = o3d.io.read_point_cloud(demo.paths[0])
ply2 = o3d.io.read_point_cloud(demo.paths[1])
trans_init = np.asarray([[0.0, 0.0, 1.0, 0.0], [1.0, 0.0, 0.0, 0.0],
[0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]])
ply1.transform(trans_init)
draw_registration_result(ply1, ply2, np.identity(4))
voxel_size = 0.01
source_down, source_fpfh = preprocess_point_cloud(ply1, voxel_size)
target_down, target_fpfh = preprocess_point_cloud(ply2, voxel_size)
start = time.time()
result_ransac = execute_global_registration(source_down, target_down, source_fpfh, target_fpfh, voxel_size)
print("Global registration took {:.3f} sec.\n" .format(time.time()-start))
print(result_ransac)
print("Result transform: {}" .format(result_ransac.transformation))
draw_registration_result(source_down, target_down, result_ransac.transformation)
draw_registration_result(ply1, ply2, result_ransac.transformation)
result_icp = refine_registration(ply1, ply2, result_ransac, voxel_size)
print(result_icp)
draw_registration_result(ply1, ply2, result_icp.transformation)
# save_pcd(ply1, ply2, result_icp.transformation)
# The result of fast registration is not good.
start = time.time()
result_fast = execute_fast_global_registration(source_down, target_down, source_fpfh, target_fpfh, voxel_size)
print("fast global registration took {:.3f} sec.\n" .format(time.time()-start))
print(result_fast)
draw_registration_result(source_down, target_down, result_fast.transformation)
draw_registration_result(ply1, ply2, result_fast.transformation)
result_icp = refine_registration(ply1, ply2, result_fast, voxel_size)
print(result_icp)
draw_registration_result(ply1, ply2, result_icp.transformation)
Results:
Original point cloud:
Point cloud after global registration and local refine:
Reference
Rusu R B, Blodow N, Beetz M. Fast point feature histograms (FPFH) for 3D registration[C]//2009 IEEE international conference on robotics and automation. IEEE, 2009: 3212-3217. ↩︎ ↩︎
Fischler M A, Bolles R C. Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography[J]. Communications of the ACM, 1981, 24(6): 381-395. ↩︎
Rusinkiewicz S, Levoy M. Efficient variants of the ICP algorithm[C]//Proceedings third international conference on 3-D digital imaging and modeling. IEEE, 2001: 145-152. ↩︎
Paul J. Besl and Neil D. McKay, A Method for Registration of 3D Shapes, PAMI, 1992. ↩︎
Chen Y, Medioni G. Object modelling by registration of multiple range images[J]. Image and vision computing, 1992, 10(3): 145-155. ↩︎
Zhou Q Y, Park J, Koltun V. Fast global registration[C]//Computer Vision–ECCV 2016: 14th European Conference, Amsterdam, The Netherlands, October 11-14, 2016, Proceedings, Part II 14. Springer International Publishing, 2016: 766-782. ↩︎