A Fast and Robust Rotation Search and Point Cloud Registration Method for 2D Stitching and 3D Object Localization

Abstract

Rotation search and point cloud registration are two fundamental problems in robotics, geometric vision, and remote sensing, which aim to estimate the rotation and transformation between the 3D vector sets and point clouds, respectively. Due to the presence of outliers (probably in very large numbers) among the putative vector or point correspondences in real-world applications, robust estimation is of great importance. In this paper, we present Inlier searching using COmpatible Structures (ICOS), a novel, efficient, and highly robust solver for both the correspondence-based rotation search and point cloud registration problems. Specifically, we (i) propose and construct a series of compatible structures for the two problems, based on which various invariants can be established, and (ii) design time-efficient frameworks to filter out outliers and seek inliers from the invariant-constrained random sampling based on the compatible structures proposed. In this manner, even with extreme outlier ratios, inliers can be effectively sifted out and collected for solving the optimal rotation and transformation, leading to our robust solver ICOS. Through plentiful experiments over standard datasets, we demonstrated that: (i) our solver ICOS is fast, accurate, and robust against over 95% outliers with nearly a 100% recall ratio of inliers for rotation search and both known-scale and unknown-scale registration, outperforming other state-of-the-art methods, and (ii) ICOS is practical for use in real-world application problems including 2D image stitching and 3D object localization.

1. Introduction

Rotation search and point cloud registration are two cornerstones in computer vision, robotics, remote sensing, and photogrammetry. The former has found broad applications in satellite attitude estimation [1], image stitching [2], etc., while the latter has been widely applied in 3D scene reconstruction and modeling [3,4,5,6], object localization and recognition [7,8,9], medical imaging [10], Simultaneous Localization and Mapping (SLAM) [11], archeology [12], etc.

1.1. Problems Formulation

Rotation search aims to find the best rotation that aligns two 3D vector sets in different coordinate frames, which can be formulated as follows.

Given two 3D vector sets $\mathcal{U}={\left\{{\mathbf{u}}_i\right\}}_{i=1}^N$ and $\mathcal{V}={\left\{{\mathbf{v}}_i\right\}}_{i=1}^N$ where we consider tuple $({\mathbf{u}}_i,{\mathbf{v}}_i)$ as a vector correspondence, the following relation is satisfied:

$$\mathit{R}{\mathbf{u}}_i+{\mathit{ϵ}}_i={\mathbf{v}}_i,$$

(1)

where $\mathit{R}\in SO\left(3\right)$ (the explicit properties and constraints of $SO\left(3\right)$ can be found in [13,14]) denotes the rotation matrix and ${\mathit{ϵ}}_i\in {\mathbb{R}}^3$ represents the measurement of Gaussian noise with isotropic covariance ${\sigma }^2{\mathbf{I}}_3$ ($\sigma$ is the standard deviation and ${\mathbf{I}}_3\in {\mathbb{R}}^{3\times 3}$ is the identity matrix) and mean $\mu =0$ w.r.t. the $i_{th}$ vector correspondence. Thus, the rotation search problem can be mathematically defined as follows:

Problem 1

(Rotation search). The rotation search problem under the perturbance of noise can be equivalently formulated as a minimization problem such that:

$$\underset{\mathit{R}\in \mathbf{SO}3}{min}\sum _{i=1}^N{\left|\right|\mathit{R}{\mathbf{u}}_i-{\mathbf{v}}_i\left|\right|}^2,$$

(2)

where globally optimal solution $\widehat{\mathit{R}}$ is the best rotation desired.

Point cloud registration consists of estimating the transformation (including scale, rotation, and translation) between two given 3D point clouds. Now, we introduce the correspondence-based point cloud registration problem.

With two 3D point clouds $\mathcal{P}={\left\{{\mathbf{P}}_i\right\}}_{i=1}^N$ and $\mathcal{Q}={\left\{{\mathbf{Q}}_i\right\}}_{i=1}^N$, where tuple $({\mathbf{P}}_i,{\mathbf{Q}}_i)$ represents a point correspondence, we can have the following relation:

$$\mathit{s}\mathit{R}{\mathbf{P}}_i+\mathit{t}+{\mathit{\epsilon }}_i={\mathbf{Q}}_i,$$

(3)

where $\mathit{s}>0$ is the scale, $\mathit{R}\in SO\left(3\right)$ is the rotation matrix, $\mathit{t}\in {\mathbb{R}}^3$ is the translation vector, and ${\mathit{\epsilon }}_i\in {\mathbb{R}}^3$ represents the measurement of Gaussian noise similar to that in Problem 1. The generalized point cloud registration problem can be defined as follows.

Problem 2

(Generalized point cloud registration). Solving the generalized point cloud registration problem under the perturbance of noise is equivalent to the following minimization problem:

$$\underset{\mathit{s}>0,\mathit{R}\in \mathbf{SO}3,\mathit{t}\in {\mathbb{R}}^3}{min}\sum _{i=1}^N{\left|\right|\mathit{s}\mathit{R}{\mathbf{P}}_i+\mathit{t}-{\mathbf{Q}}_i\left|\right|}^2,$$

(4)

where global minimizers $(\widehat{\mathit{s}},\widehat{\mathit{R}},\widehat{\mathit{t}})$ are the optimal solutions to this registration problem.

When the scale aligning the two point clouds is fixed, which means scale $\mathit{s}=1$, we can directly omit $\mathit{s}$ in Problem 2, called the known-scale registration problem. However, the scale could vary, which, for instance, exists in the cross-source point cloud registration problems. In this case, the scale needs to be estimated additionally, and such problems are called unknown-scale point cloud registration.

Our work seeks to solve both the known-scale and the unknown-scale registration problems in the presence of both noise and a great number of outliers among the putative correspondences, providing a highly robust backend for real applications.

1.2. Motivation and Contributions

Motivation: The correspondence-based rotation search and point cloud registration approaches are often confronted with one difficulty: in many real-world scenes, the input correspondences are very likely to be corrupted by outliers; for example, the feature matchers (e.g., SIFT [15] and SURF [16] for 2D images and FPFH [17] for 3D point clouds) may generate mismatched or spurious results owing to noise, repetitive patterns, low-textured scenes, etc. Therefore, in real applications, robust estimation solvers should be of paramount significance.

When it comes to robust estimation, RANSAC [18] is a common robust heuristic [19], but it has a critical disadvantage: its runtime increases exponentially with the outlier ratio. Thus, RANSAC could be impractical for use when encountering extreme outlier ratios (e.g., more than 90%). Branch-and-Bound (BnB) is able to search in the rotation ($SO\left(3\right)$) or transformation ($SE\left(3\right)$) group to globally optimize the objective function, whereas its computational cost grows exponentially with the input correspondence number; hence, it may be too slow for large-sized problems.

In addition, the outlier removal method GORE [20,21], as well as the nonminimal solvers FGR [22], QUASAR [23], and TEASER/TEASER++ [24,25] have been proposed to deal with the two problems with over 90% outliers. However, for point cloud registration, these solvers can only succeed when the scale between the two point clouds is known and fixed. When the scale varies (e.g., between point clouds formed with different sources), none of these methods could tolerate more than 90% outliers, which indeed necessitates a general solver capable of solving not only the known-scale problems, but also the unknown-scale ones.

To handle rotation search problems with high outlier ratios, though GORE [21] is efficient when the problem size is small, it requires more time to remove outliers as the correspondence number increases. Hence, proposing a high-outlier-ratio-tolerant solver that can be efficient regardless of the correspondence number seems rather imperative.

Contributions: Motivated by these facts, in this paper, we propose a novel highly robust paradigm, named Inlier searching with COmpatible Structures (ICOS), for addressing rotation search and generalized point cloud registration with extreme outlier ratios, as exemplified in Figure 1.

To be specific, ICOS aims to seek the correct inliers through the random sample flow (rather than pruning outliers as performed in the previous works [20,27]), but significantly different from the RANSAC family (e.g., [18,28,29]), the sampling and filtering process of ICOS is built upon and also facilitated by the constraints derived from the compatible structures that we define based on the theory of invariance.

First, we introduced and constructed a series of compatible structures for the rotation search and generalized point cloud registration problems, with which we were able to “customize” special invariant-based constraints that can be used for effectively differentiating inliers from outliers only with simple Boolean conditions (Section 4).

Moreover, we designed sampling-based frameworks that can fit with the compatible structures to seek and preserve eligible inliers out of the putative correspondences for the two problems, in which peculiar constraint-based sampling strategies were employed to optimize time efficiency for the proposed algorithms (Section 5).

Finally, we validated ICOS in a variety of experiments over different datasets and demonstrated that: (i) ICOS is precise and robust even when over 95% of the correspondences are outliers in the rotation search, as well as both the known-scale and unknown-scale registration problems (it can be even robust against up to 99% outliers when correspondences are relatively sufficient); (ii) ICOS runs fast, yields accurate estimates, and recalls approximately 100% of the inliers; (iii) ICOS is also applicable to practical application problems over real datasets (Section 6).

2. Related Work

In this section, we provide reviews on: (i) methods for naive and robust rotation search, (ii) methods for naive and robust point cloud registration, and (iii) heuristics for robust outlier rejection.

Rotation search: The rotation search (also called “Wahba”) problem was first proposed by Wahba in [1], in order to estimate the attitude of satellites from the observation of vectors. It can be solved in closed form based on either a rotation matrix [30,31,32,33] or the quaternion-based parametrization [34,35], although these methods usually fail in the existence of outliers.

It is known to all that in the process of building vector correspondences using the feature descriptors [16,17] based on 2D or 3D keypoints, it is likely to obtain false results and mismatches, that is the so-called outliers. Thus, robust solvers are required.

The BnB framework is effective at finding the optimal rotation from the outlier-corrupted corresponding vectors, and the typical examples include the first globally optimal method searching over the rotation space [36], the L2-distance-based approach [37], and the speeded up angular-distance-based solution [38]. In addition to BnB, guaranteed outlier removal [21] is another way to estimate the rotation robustly and is generally faster than the BnB solvers. Some other robust rotation search solvers are also the subroutines for point cloud registration problems (e.g., [20,22,25]), which will be later discussed in the review on point cloud registration.

Point cloud registration: To solve the point cloud registration problem, the Iterative Closest Point (ICP) [39] is a common local-minimum approach, but it is too dependent on the initial guess of the transformation, thus prone to fail without a good initialization. Therefore, an initialization-free way to address the registration problem —to build correspondences between point clouds from 3D keypoints [40] using feature descriptors (e.g., FPFH [17], ISS [41], and FCGF [42]), has grown increasingly popular. Many correspondence-based methods have been proposed to address the registration problem in the presence of noise. For example, it can be solved using closed-form solutions [32,34] by decoupling the estimation of rotation, scale, and translation. BnB has also been applied to solve the registration problem [43] in a globally optimal way. More recently, the Lagrangian dual relaxation was developed to produce a globally optimal generalized registration solver [14] for handling point-to-point, point-to-line, and point-to-plane registration with optimality guarantees.

However, the 3D feature descriptors generally have lower accuracy and robustness compared with 2D ones [15,16], which could result in relatively higher or even extreme outlier ratios in some real-world scenes, as was discussed in [20]. The above-mentioned solutions are fairly sensitive and can be fragile once there exist outliers among the correspondences, which consequently demands the following robust registration methods.

The minimal closed-form solutions [32,34] can be fit into the RANSAC family frameworks [18,28,29] to deal with outliers, though they may be slow when the outlier ratio is high. Another idea to be robust is that: if as many outliers can be guaranteed to be removed or eliminated as possible, then what remains should be mostly inliers. GORE [20] is such a preprocessing outlier removal approach that guarantees that the correspondences removed must not be inliers. In terms of the nonminimal robust solvers, FGR [22] is the first method using Graduated Nonconvexity (GNC) to reject outliers, which estimates the rigid transformation matrix with a Gauss–Newton method. Though not globally optimal, FGR is efficient for known-scale registration. Currently, TEASER/TEASER++ [24,25] are the more robust nonminimal certifiably globally optimal solvers that can tolerate 99% outliers in known-scale situations. Another recent solution [44] utilizes RANSAC for scale and translation estimation and also adopts GNC to solve the rotation. Unfortunately, all these solvers are either incapable of estimating the scale or unable to tolerate over 90% outliers for unknown-scale point cloud registration (as will be shown in Section 6).

Robust estimators. Apart from the classic hypothesize-and-test framework RANSAC, M-estimation is useful for outlier rejection [22,45,46,47]. When the naive solver is nonminimal, the special M-estimator GNC [48] is a general-purpose estimator operated by alternating optimization and adopts robust cost functions such as Geman–McClure (GM) or Truncated Least Squares (TLS), which can handle 70–80% outliers in robotics or vision problems. In addition, invariants can be applied for robust estimation. This paper is an improved and extended version of our prior work RANSIC [49]. The main differences are as follows: (i) this paper introduces the concept of compatible structures for the first time, which can enable faster termination with extreme outlier ratios (this is shown in experiments); (ii) though both use invariants, RANSIC keeps searching for compatible three-point sets, while ICOS only seeks single correspondences based on an eligible compatible structure, so that novel robust algorithm frameworks are proposed in this paper; (iii) the method’s presentation is more extensive, and the experiments are also enriched in this paper.

3. Notation and Preliminaries

3.1. Invariants

We first define the concept of invariants by borrowing the notation and definition in [27].

Definition 1

(Invariants). In one geometric problem, assume that we obtain N putative correspondences whose indices are in set $\mathcal{C}=\{1,2,\dots ,N\}$. Let $\mathcal{S}\subset \mathcal{C}$ denote a subset of $\mathcal{C}$ with a fixed size, $\mathit{x}$ denote the vector stacking all variables, ${\mathit{y}}_{\mathcal{S}}$ denote the measurements w.r.t. subset $\mathcal{S}$, ${\mathit{\eta }}_{\mathcal{S}}$ denote the corresponding noise measurements independent of $\mathit{x}$, and ${\mathit{h}}_{\mathcal{S}}(\mathit{x},{\mathit{\eta }}_{\mathcal{S}})$ denote the mathematical model that is used to define the relation between $\mathit{x}$ and ${\mathit{y}}_{\mathcal{S}}$, given by ${\mathit{y}}_{\mathcal{S}}={\mathit{h}}_{\mathcal{S}}(\mathit{x},{\mathit{\eta }}_{\mathcal{S}})$.

If we find a function $\mathit{f}\left({\mathit{y}}_{\mathcal{S}}\right)$ where $\mathit{f}\left({\mathit{y}}_{\mathcal{S}}\right)=\mathit{f}\left({\mathit{h}}_{\mathcal{S}}(\mathit{x},{\mathit{\eta }}_{\mathcal{S}})\right)$ is always satisfied and also independent of $\mathit{x}$ no matter which $\mathcal{S}\subset \mathcal{C}$ is selected, we can consider $\mathit{f}$ as an invariant function, also abbreviated as an invariant.

3.2. Naive and Nonminimal Solvers

In our solver ICOS, the computation of the scale, rotation, and translation without outliers is realized by introducing the fast nonminimal naive solvers below.

Assume that we have M correspondences (inliers); we first need to acquire the corresponding 3D vectors, which, in rotation search, are just the original vector correspondences such that:

$${\tilde{\mathbf{m}}}_i={\mathbf{u}}_i,{\tilde{\mathbf{n}}}_i={\mathbf{v}}_i,$$

(5)

while in point cloud registration, this can be represented with the centroid-based translation-free technique [32] such that:

$${\tilde{\mathbf{m}}}_i={\mathbf{P}}_i-\overline{\mathbf{P}},{\tilde{\mathbf{n}}}_i={\mathbf{Q}}_i-\overline{\mathbf{Q}},$$

(6)

where $\overline{\mathbf{P}}$ and $\overline{\mathbf{Q}}$ are the centroids in the two frames, respectively, given by:

$$\overline{\mathbf{P}}=\frac{{\sum }_{i=1}^M{\mathbf{P}}_i}{M},\overline{\mathbf{Q}}=\frac{{\sum }_{i=1}^M{\mathbf{Q}}_i}{M}.$$

(7)

The optimal rotation can be computed in closed-form using Singular-Value Decomposition (SVD) [32] such that:

$$\begin{array}{c}\mathbf{U}\mathsf{\Sigma }{\mathbf{V}}^{\top }=SVD\left(\sum _{i=1}^M{\tilde{\mathbf{m}}}_i{\tilde{\mathbf{n}}}_i^{\top }\right),\\ \widehat{\mathit{R}}=\mathbf{V}{\mathbf{U}}^{\top }.\end{array}$$

(8)

In point cloud registration, the best scale can be computed by the weighted representation as in [25] such that:

$$\widehat{\mathit{s}}=\frac{1}{{\sum }_{i=1}^M{\omega }_i}·\sum _{i=1}^M{\omega }_i\frac{\parallel {\tilde{\mathbf{n}}}_i\parallel }{\parallel {\tilde{\mathbf{m}}}_i\parallel },$$

(9)

where ${\omega }_i=\frac{{\left|\right|{\tilde{\mathbf{m}}}_i\left|\right|}^2}{A^2}$. With the rotation and the scale obtained, the translation can be recovered using the centroids as:

$$\widehat{\mathit{t}}=\overline{\mathbf{Q}}-\widehat{\mathit{s}}\widehat{\mathit{R}}\overline{\mathbf{P}}.$$

(10)

4. Compatible Structures in the Point Cloud Registration Problem

In this section, we define the Compatible Structures (COSs) for the two problems and then fully explore the potential invariants among them to establish the constraints for the sampling process.

4.1. COS in Rotation Search

The basic structure in rotation search for constrained random sampling is the COS that consists of a pair of correspondences as introduced below.

Definition 2

(2-COS and length-based invariants). Assume we have 2 vector correspondences such that $({\mathbf{u}}_i,{\mathbf{v}}_i),i=1,2$, defined as a 2-COS. We first normalize them all to unit length such that:

$${\mathbf{u}}_1^{*}=\frac{{\mathbf{u}}_1}{\left|\right|{\mathbf{u}}_1\left|\right|},{\mathbf{v}}_1^{*}=\frac{{\mathbf{v}}_1}{\left|\right|{\mathbf{v}}_1\left|\right|},{\mathbf{u}}_2^{*}=\frac{{\mathbf{u}}_2}{\left|\right|{\mathbf{u}}_2\left|\right|},{\mathbf{v}}_2^{*}=\frac{{\mathbf{v}}_2}{\left|\right|{\mathbf{v}}_2\left|\right|}.$$

(11)

Then, we derive the following pairwise length-based invariants as:

$$I_{12}^l=\left|\left|\right|{\mathbf{u}}_1^{*}-{\mathbf{u}}_2^{*}\left|\right|-\left|\right|{\mathbf{v}}_1^{*}-{\mathbf{v}}_2^{*}\left|\right|\right|={ϵ}_{12}^{\mathit{l}},$$

(12)

where $I_{12}^{\mathit{l}}$ represents the length-based invariant w.r.t. this pair of correspondences and ${ϵ}_{12}^{\mathit{l}}$ denotes the noise measurement (perturbance) on this invariant.

Intuitively, invariant $I_{12}^l$ represents the length difference between the third sides of the two triangles formed by the corresponding vector pairs over the two frames. Without noise perturbance, $I_{12}^l$ should be strictly equal to 0 as long as the correspondences pf this pair are both inliers. However, with noise, we can still impose a constraint on $I_{12}^l$ to examine whether this correspondence pair might contain outliers.

Proposition 1

(Constraints on $I_{12}^{\mathit{l}}$). For the pairwise invariant $I_{ij}^{\mathit{l}}$ in the 2-COS defined above, we can derive that:

$$I_{12}^l\le L,$$

(13)

where L denotes the bound w.r.t. noise satisfying the inequality such that $L^{*}+\left|\right|\mathit{\epsilon }\left|\right|·\left(∥\frac{1}{{\mathbf{v}}_1}∥+∥\frac{1}{{\mathbf{v}}_2}∥\right)$ $\le L$, in which $L^{*}=∥\frac{{\mathbf{u}}_1}{\left|\right|{\mathbf{v}}_1\left|\right|}-\frac{{\mathbf{u}}_2}{\left|\right|{\mathbf{v}}_2\left|\right|}-\frac{{\mathbf{u}}_1}{\left|\right|{\mathbf{u}}_1\left|\right|}+\frac{{\mathbf{u}}_2}{\left|\right|{\mathbf{u}}_2\left|\right|}∥$.

Proof. 

Based on the triangular inequality and the fact that norm $\parallel ·\parallel$ is invariant of $\mathit{R}$, we can always have:

$$\begin{array}{c}\left|\left|\right|{\mathbf{u}}_1^{*}-{\mathbf{u}}_2^{*}\left|\right|-\left|\right|{\mathbf{v}}_1^{*}-{\mathbf{v}}_2^{*}\left|\right|\right|\\ =\left|\parallel \mathit{R}\parallel ·∥\frac{{\mathbf{u}}_1+{\mathit{R}}^{\top }{\mathit{ϵ}}_1}{\left|\right|{\mathbf{v}}_1\left|\right|}-\frac{{\mathbf{u}}_2+{\mathit{R}}^{\top }{\mathit{ϵ}}_2}{\left|\right|{\mathbf{v}}_2\left|\right|}∥-∥\frac{{\mathbf{u}}_1}{\parallel {\mathbf{u}}_1\parallel }-\frac{{\mathbf{u}}_2}{\parallel {\mathbf{u}}_2\parallel }∥\right|,\\ \le L^{*}+\parallel {\mathit{R}}^{\top }\parallel ·∥\frac{{\mathit{ϵ}}_1}{\left|\right|{\mathbf{v}}_1\left|\right|}-\frac{{\mathit{ϵ}}_2}{\left|\right|{\mathbf{v}}_2\left|\right|}∥\\ \le L^{*}+∥\frac{{\mathit{ϵ}}_1}{\left|\right|{\mathbf{v}}_1\left|\right|}∥+∥\frac{{\mathit{ϵ}}_2}{\left|\right|{\mathbf{v}}_2\left|\right|}∥=L,\end{array}$$

(14)

where the length-based inequality (13) can be proven. □

If we have obtained an eligible 2-COS, we can compute the rotation w.r.t. this pair of correspondences using Horn’s triad-based matrix transformation [34] such that:

$$\begin{array}{c}{\mathbf{O}}_1=\left[\begin{array}{ccc}{\mathbf{u}}_1& ({\mathbf{u}}_1\times {\mathbf{v}}_1)& {\mathbf{u}}_1\times ({\mathbf{u}}_1\times {\mathbf{v}}_1)\end{array}\right],\\ {\mathbf{O}}_2=\left[\begin{array}{ccc}{\mathbf{u}}_2& ({\mathbf{u}}_2\times {\mathbf{v}}_2)& {\mathbf{u}}_2\times ({\mathbf{u}}_2\times {\mathbf{v}}_2)\end{array}\right],\\ {\mathit{R}}_{12}={\mathbf{O}}_2{\mathbf{O}}_1^{\top }={\mathit{R}}_{gt}Exp\left({\mathit{\rho }}_{12}\right),\end{array}$$

(15)

where “$Exp$” maps the size-3 noise measurement (vector) ${\mathit{\rho }}_{12}$ to the rotation (for explicit derivations, please refer to Section 7.1.3 in [50]), and ${\mathit{R}}_{gt}$ denotes the ground truth rotation.

Then, we further try to build a 3-COS by adding a new correspondence with this 2-COS, based on which new constraints can be imposed.

Proposition 2

(3-COS, invariants and constraints). Assume that now we obtain a new correspondence, say the $3_{rd}$ correspondence, in addition to the 2-COS we have, as illustrated in Figure 2 we are able to construct a 3-COS with two more invariants such that:

$$I_{i3}^{\mathit{l}}=\left|\left|\right|{\mathbf{u}}_i^{*}-{\mathbf{u}}_3^{*}\left|\right|-\left|\right|{\mathbf{v}}_i^{*}-{\mathbf{v}}_3^{*}\left|\right|\right|={ϵ}_{i3}^{\mathit{l}},$$

(16)

and two additional length-based constraints are satisfied:

$$I_{i3}^{\mathit{l}}\le L,$$

(17)

where $i\in \{1,2\}$. Apart from simply constraining the length-based rigidity, we also supplement a residual-based constraint on $({\mathbf{u}}_3,{\mathbf{v}}_3)$ such that:

$$∥{\mathit{R}}_{12}{\mathbf{u}}_3-{\mathbf{v}}_3∥\le G,$$

(18)

where G is the bound for the residual error w.r.t. the $3_{rd}$ correspondence. Besides, for every single 2-COS in this 3-COS, we can compute a raw rotation, represented as ${\mathit{R}}_{ij}$ with the correspondence tuple $(i,j)\in \left\{\right(1,2),(2,3),(1,3\left)\right\}$. In this fashion, we can impose new constraints on the mutual compatibility between the raw rotations w.r.t. different correspondence tuples selected from this 3-COS such that:

$$\angle \left({\mathit{R}}_{ij},{\mathit{R}}_{ab}\right)\le F,$$

(19)

where “∠” is the geodesic distance (error) [51] between two rotations, written as:

$$\angle \left({\mathit{R}}_{ij},{\mathit{R}}_{ab}\right)=\left|arccos\left(\frac{tr\left({\mathit{R}}_{ij}^{\top }{\mathit{R}}_{ab}\right)-1}{2}\right)\right|,$$

(20)

where F is the bound of noise for the geodesic error satisfying that $2\angle \left(Exp\left(\mathit{\rho }\right),{\mathbf{I}}_3\right)\le F$ and ${\mathbf{I}}_3$ denotes the $3\times 3$ identity matrix.

Proof. 

The proof of Constraints (17) and (18) is similar to that in Proposition 1, thus omitted here. For the constraints (19), on the basis of the triangular inequality of the geodesic distance in [51] and similar to [27], we can derive:

$$\begin{array}{c}\angle ({\mathit{R}}_{ij},{\mathit{R}}_{ab})=\angle \left(\mathit{R}Exp\left({\rho }_{ij}\right),\mathit{R}Exp\left({\mathit{\rho }}_{ab}\right)\right)\\ =\left|arccos\left(\frac{tr\left(Exp{\left({\mathit{\rho }}_{ij}\right)}^{\top }{\mathit{R}}^{\top }\mathit{R}Exp\left({\mathit{\rho }}_{ab}\right)\right)-1}{2}\right)\right|\\ =\angle \left(Exp\left({\mathit{\rho }}_{ij}\right),Exp\left({\mathit{\rho }}_{ab}\right)\right)\\ \le \angle \left(Exp\left({\mathit{\rho }}_{ij}\right),{\mathbf{I}}_3\right)+\angle \left(Exp\left({\mathit{\rho }}_{ab}\right),{\mathbf{I}}_3\right)\le F.\end{array}$$

(21)

 □

Up to now, in rotation search, the constraints over the 2-COS and 3-COS have been established, which will be later converted as a series of Boolean conditions for the sampling and filtering frameworks of ICOS in Section 5.

4.2. COS in Point Cloud Registration

The basic structure in point cloud registration lies in the COS with 3 correspondences.

Definition 3

(3-COS and pairwise scale-based invariants). Assume that we obtain 3 noncollinear point correspondences $({\mathbf{P}}_i,{\mathbf{Q}}_i),i=1,2,3$, defined as one 3-COS. We can derive the following pairwise scale-based invariants:

$$I_{ij}^{\mathit{s}}=\frac{\left|\right|{\mathbf{Q}}_i-{\mathbf{Q}}_j\left|\right|}{\left|\right|{\mathbf{P}}_i-{\mathbf{P}}_j\left|\right|}={\mathit{s}}_{gt}+{\epsilon }_{ij}^{\mathit{s}},$$

(22)

where $(i,j)\in \left\{\right(1,2),(1,3),(2,3\left)\right\}$, $I_{ij}^{\mathit{s}}$ represents the scale-based invariant w.r.t. the $ith$ and the jth correspondences, ${\mathit{s}}_{gt}$ denotes the ground truth scale, and ${\epsilon }_{ij}^{\mathit{s}}$ denotes the noise measurement on the scale.

Then, we can endow each pair of scale-based invariants $I_{ij}^{\mathit{s}}$ with constraints given as follows:

Proposition 3

(Constraints on $I_{ij}^{\mathit{s}}$). For any two invariants $I_{ij}^{\mathit{s}}$ in the 3-COS, we can derive that:

$$|I_{ij}^{\mathit{s}}-I_{jk}^{\mathit{s}}|\le A\left(\frac{1}{\left|\right|{\mathbf{P}}_i-{\mathbf{P}}_j\left|\right|}+\frac{1}{\left|\right|{\mathbf{P}}_j-{\mathbf{P}}_k\left|\right|}\right)$$

(23)

where $(i,j,k)\in \left\{\right(1,2,3),(2,3,1),(3,1,2\left)\right\}$ (here, $I_{ij}^{\mathit{s}}$ and $I_{ji}^{\mathit{s}}$ denote the same invariant) and A is the noise bound, satisfying that $2\left|\right|\mathit{\epsilon }\left|\right|\le A$. In addition, when the scale is known (${\mathit{s}}_{gt}=1$), we can further have that:

$$|I_{ij}^{\mathit{s}}-1|\le A\left(\frac{1}{\left|\right|{\mathbf{P}}_i-{\mathbf{P}}_j\left|\right|}\right).$$

(24)

Proof. 

It is easy to have that:

$${\mathit{s}}_{gt}{\mathit{R}}_{gt}{\mathbf{P}}_{ij}+{\mathit{\epsilon }}_{ij}={\mathbf{Q}}_{ij},\Rightarrow \left|\right|{\mathit{s}}_{gt}{\mathit{R}}_{gt}{\mathbf{P}}_{ij}\left|\right|=\left|\right|{\mathbf{Q}}_{ij}-{\mathit{\epsilon }}_{ij}\left|\right|,$$

(25)

where ${\mathbf{P}}_{ij}={\mathbf{P}}_i-{\mathbf{P}}_j$, ${\mathbf{Q}}_{ij}={\mathbf{Q}}_i-{\mathbf{Q}}_j$, and ${\mathit{\epsilon }}_{ij}={\mathit{\epsilon }}_i-{\mathit{\epsilon }}_j$.

Using the triangular inequality, we can derive that:

$$\begin{array}{c}\left|\right|{\mathbf{Q}}_{ij}\left|\right|-\left|\right|{\mathit{\epsilon }}_{ij}\left|\right|\le \left|\right|{\mathit{s}}_{gt}{\mathit{R}}_{gt}{\mathbf{P}}_{ij}\left|\right|={\mathit{s}}_{gt}\left|\right|{\mathbf{P}}_{ij}\left|\right|\le \left|\right|{\mathbf{Q}}_{ij}\left|\right|+\left|\right|{\mathit{\epsilon }}_{ij}^{\mathit{s}}\left|\right|,\end{array}$$

(26)

so that:

$$\begin{array}{c}{\mathit{s}}_{gt}-\frac{\left|\right|{\mathit{\epsilon }}_{ij}\left|\right|}{\left|\right|{\mathbf{P}}_{ij}\left|\right|}\le \frac{\left|\right|{\mathbf{Q}}_{ij}\left|\right|}{\left|\right|{\mathbf{P}}_{ij}\left|\right|}\le {\mathit{s}}_{gt}+\frac{\left|\right|{\mathit{\epsilon }}_{ij}\left|\right|}{\left|\right|{\mathbf{P}}_{ij}\left|\right|}.\end{array}$$

(27)

Thus, Inequalities (23) and (24) can be derived as long as we have: $\left|\right|{\mathit{\epsilon }}_{ij}\left|\right|\le \left|\right|{\mathit{\epsilon }}_i\left|\right|+\left|\right|{\mathit{\epsilon }}_j\left|\right|=2\left|\right|\mathit{\epsilon }\left|\right|\le A$. □

Subsequently, we calculate the raw scale ${\mathit{s}}_{123}$ as:

$${\mathit{s}}_{123}=\frac{1}{{\sum }_1^3{\omega }_{ij}}·\sum _1^3{\omega }_{ij}\frac{\parallel {\mathbf{Q}}_{ij}\parallel }{\parallel {\mathbf{P}}_{ij}\parallel },$$

(28)

where ${\omega }_{ij}=\frac{{\parallel {\mathbf{P}}_{ij}\parallel }^2}{A^2}$ and the raw rotation ${\mathit{R}}_{123}$ using Horn’s 3-point minimal solver [34], similar to (15). With the raw scale and rotation solved, we can define the translation-based invariants as follows:

Definition 4

(Translation-based invariants). In addition to (22), we can derive the translation-based invariants:

$$I_i^{\mathit{t}}={\mathbf{Q}}_i-{\mathit{s}}_{123}{\mathit{R}}_{123}{\mathbf{P}}_i={\mathit{t}}_{gt}+{\mathit{\epsilon }}_i^{\mathit{t}},$$

(29)

where $i\in \{1,2,3\}$, $I_i^{\mathit{t}}$ represents the translation-based invariant w.r.t. the $i_{th}$ correspondences and ${\mathit{\epsilon }}_i^{\mathit{t}}$ describes its noise measurement. In known-scale cases, we directly set ${\mathit{s}}_{123}=1$.

To create constraints on any two invariants $I_i^{\mathit{t}}$ from this 3-COS, we can derive the following:

Proposition 4

(Constraints on $I_i^{\mathit{t}}$). For invariants $I_i^{\mathit{t}}$ in the 3-COS above, we can derive that:

$$\left|\right|I_i^{\mathit{t}}-I_j^{\mathit{t}}\left|\right|\le B$$

(30)

where $(i,j)\in \left\{\right(1,2),(2,3),(3,1\left)\right\}$ and B is the noise bound, satisfying that $2\left|\right|{\mathit{\epsilon }}^{\mathit{t}}\left|\right|\le B$.

Proof. 

The proof here is trivial. □

On the basis of this 3-COS, whenever we obtain a new (the $4_{th}$) correspondence, defined as a 4-COS, additional invariants can be established as follows.

Proposition 5

(4-COS, invariants and constraints). If we have the $4th$ correspondence in addition to the 3-COS we have, we can first build 3 more scale-based invariants such that:

$$I_{i4}^{\mathit{s}}=\frac{\left|\right|{\mathbf{Q}}_i-{\mathbf{Q}}_4\left|\right|}{\left|\right|{\mathbf{P}}_i-{\mathbf{P}}_4\left|\right|}={\mathit{s}}_{gt}+{ϵ}_{i4},$$

(31)

where $i\in \{1,2,3\}$, with the following constraints:

$$|I_{i4}^{\mathit{s}}-I_{j4}^{\mathit{s}}|\le A\left(\frac{1}{\left|\right|{\mathbf{P}}_i-{\mathbf{P}}_4\left|\right|}+\frac{1}{\left|\right|{\mathbf{P}}_j-{\mathbf{P}}_4\left|\right|}\right),$$

(32)

where $i,j\in \{1,2,3\},i\ne j$. Similar to Proposition 2, we also supplement a residual-based constraint on $({\mathbf{P}}_4,{\mathbf{Q}}_4)$ such that:

$$∥{\mathit{s}}_{123}{\mathit{R}}_{123}{\mathbf{P}}_4+{\mathit{t}}_{123}-{\mathbf{Q}}_4∥\le C,$$

(33)

where ${\mathit{t}}_{123}$ can be computed from the 3-COS as:

$${\mathit{t}}_{123}={\overline{I}}_{123}^{\mathit{t}}=\frac{I_1^{\mathit{t}}+I_2^{\mathit{t}}+I_3^{\mathit{t}}}{3},$$

(34)

and C is the bound for this residual error. Moreover, for each 3-COS in this 4-COS, we can compute a raw rotation, represented as ${\mathit{R}}_{ijk}$ with the correspondence tuple $(i,j,k)$. Thus, new constraints on the mutual compatibility of rotations can be established as:

$$\angle \left({\mathit{R}}_{ijk},{\mathit{R}}_{abc}\right)\le D,$$

(35)

where “∠” is also the geodesic error between two rotations, expressed as:

$$\angle \left({\mathit{R}}_{ijk},{\mathit{R}}_{abc}\right)=\left|arccos\left(\frac{tr\left({\mathit{R}}_{ijk}^{\top }{\mathit{R}}_{abc}\right)-1}{2}\right)\right|,$$

(36)

where $(i,j,k),(a,b,c)\in \left\{\right(1,2,3),(1,2,4),(1,3,4),(2,3,4\left)\right\}$ and D is the noise bound satisfying $2\angle \left(Exp\left(\mathit{\rho }\right),{\mathbf{I}}_3\right)\le D$.

Proof. 

The proof here is omitted, since the derivations of Constraints (32) and (33) is analogous to Propositions 3 and 4 and the derivation of Constraint (35) is similar to Proposition 2. □

So far, we have derived the invariant-based constraints from both the 3-COS and 4-COS for the registration problem, leading to the resulting robust solver ICOS.

5. Our Solver: ICOS

In this section, we formulate our solver ICOS for rotation search and both the known-scale and the unknown-scale point cloud registration problems.

We first provide the insights and overview of the proposed solver. As intuitively illustrated in Figure 2, we begin with finding eligible a-COS across the two point sets by roughly checking the first layer of invariant-based constraints, where $a=2$ for rotation search and $a=3$ for point cloud registration, as discussed in Section 4.

Once an a-COS satisfies the required constraints, we solve the raw solutions (rotation, scale, or translation) with it, which is for further examining whether the second layer of constraints can be satisfied. Holding one eligible a-COS, we then pick random correspondences one after another with testing both their residual-based and invariant-based constraints simultaneously, in order to construct an eligible b-COS, where $b=a+1$, on the basis of this a-COS.

The process of finding b-COS continues until we are able to obtain a sufficient number of such b-COS within reasonable times of the single-correspondence random sampling, which also serves as a hidden condition aiming to determine whether the a-COS and its corresponding b-COS can be all true inliers.

The following subsections provide more details on formulating the pseudocode of ICOS for specific problems.

5.1. ICOS for Rotation Search

For addressing the rotation search problem, we render the pseudocode of ICOS in Algorithm 1 and the description for each part as follows.

Algorithm 1: ICOS for rotation search

Lines 2–10: ICOS initiates with random sampling. However, different from ordinary random sampling procedures, we impose constraints on each random sample selected. When picking one random subset $\mathcal{R}$ of 2 correspondences, we immediately check the length-based invariant (13). If it can be satisfied, we solve the raw rotation with this 2-COS $\mathcal{R}$ and then allow it to proceed to the next step, which is operated by setting $pass=1$; if not, we start over from the sampling process.

Lines 11–25: If we manage to obtain an eligible 2-COS, say $\mathcal{R}$, we can then seek new correspondences to build a 3-COS, also by constrained sampling, where we select and test the correspondence samples one at a time. If one correspondence can satisfy Constraints (17)–(19), indicating that it can form an eligible 3-COS with $\mathcal{R}$, we add it to the inlier set and make $count$ increase by 1. Once we collect enough 3-COS ($count\ge X$), we stop sampling and return the X + 2 inliers in total.

During this process, we supplement an important subroutine: checkSampling, in order to boost time efficiency, which is designed to timely break from the sampling if the obtained 2-COS ($\mathcal{R}$) does not seem to be inliers, and this operation could be realized by regularly checking the number of 4-COS formed according to the iteration number, as demonstrated in Algorithm 2. To be specific, as long as the correspondences in set $\mathcal{R}$ are both inliers, at least one inlier correspondence ought to be sampled after a sufficient number of iterations that can be computed from (37) by setting a high confidence; conversely, if the latter condition cannot be satisfied, we have every reason to suppose that set $\mathcal{R}$ contains at least one outlier.

Algorithm 2:checkSampling (subroutine of ICOS)

Lines 26–28: We first compute the rotation with the X+2 inliers and then consider all correspondences whose residual errors satisfy $r_i\le 5.2\sigma$ as the ultimate inliers. Finally, we can solve the optimal rotation with all the inliers found among the putative correspondences using the nonminimal estimator (8).

5.2. ICOS for Known-Scale Registration

The pseudocode of ICOS for point cloud registration with a known scale ($\mathit{s}=1$) is given in Algorithm 3.

Lines 2–14: We design a decoupled sampling method for the known-scale registration. First, we select a random subset $\mathcal{R}$ of 2 correspondences, rather than 3, and check if their scale-based invariant $I_{12}^{\mathit{s}}$ is close enough to 1 using constraint (24). If it is, we then proceed to select random samples for the remaining one correspondence, during which we try forming an eligible 3-COS that satisfies the constraints (24) based on $\mathcal{R}$; if not, we go back to the sampling process to seek $\mathcal{R}$.

Lines 15–32: Once an eligible 3-COS (set $\mathcal{O}$) is obtained, we check their constraints (23). If satisfied, we then solve the raw rotation with $\mathcal{O}$ and determine whether the constraints (30) are fulfilled in $\mathcal{O}$. If yes, we then move on to sampling once again, for building one 4-COS based on set $\mathcal{O}$. We take random correspondences one at a time and feed them sequentially to $\mathcal{O}$ to check Constraints (32)–(35). After collecting enough correspondences (X) that can construct qualified 4-COS with $\mathcal{O}$, we jump out and return the inliers.

Lines 33–35: Similar to Algorithm 1, we check the residual error w.r.t. each correspondence to gather the whole inlier set, which is then used to compute optimal solutions with the nonminimal solvers (9) and (10).

Algorithm 3: ICOS for known-scale registration

5.3. ICOS for Unknown-Scale Registration

When the scale is unknown, we design a different framework for ICOS, and its pseudocode is shown in Algorithm 4.

Algorithm 4 employs a methodology similar to lines 15–35 of Algorithm 3. The differences lie in that: (i) we are unable to impose the constraints (24) on the 3-COS using the decoupled sampling method as operated in Lines 3–14, since the scale is not fixed, which may result in more runtime than the known-scale problems generally; (ii) the raw scale and the scale-based invariants are not set to 1, but need to be computed as in (22) and (31) instead.

Algorithm 4: ICOS for unknown-scale registration

5.4. Discussion on the Maximum Iteration Numbers

Note that in Algorithms 1 and 2, a series of maximum iteration numbers is involved during the sampling process, so now we discuss the proper choice of their values.

Generally, the maximum iteration we set can be given by the following formulation:

$$maxItr\ge x·\frac{log(1-p)}{log\left(1-{(1-outlierratio)}^n\right)},$$

(37)

where x is the number of eligible inlier subsets desired, p is the confidence for the fact that all the correspondences selected are inliers (typically we choose $p>0.98$), usually the outlier ratio should be within 0–99% (typically, 99% is chosen unless we can know in advance that the outlier ratio is below a certain level), and n is the number of correspondences we select for each random sample (subset).

Therefore, for $maxIt{r}_1$, we adopted $n=2$, choosing two correspondences at one time, while for $maxIt{r}_2$, $maxIt{r}_3$ and $maxIt{r}_4$, we set all $n=1$ for single-correspondence sampling.

5.5. Discussion on the Performance of ICOS

Since the main body of our solver ICOS consists of simply judging a series of Boolean conditions (true or false) according to the constraints over the invariants w.r.t. different variables, it can be fast to implement. Besides, ICOS abandons the strategy of making an estimate for every random sample selected (as operated in RANSAC) and smartly solves the transformation only when the scale-based constraints (23) and (24) allow it to do so. Furthermore, the residual errors w.r.t. all the correspondences only require being solved once (just for finding the whole inlier set), saving much time for a realistic implementation, especially when the point cloud correspondences are in huge numbers, which is not uncommon in practice.

6. Experiments

In this section, we conduct a series of experiments based on multiple datasets to evaluate the proposed solver ICOS for rotation search and generalized point cloud registration, also in comparison with the existing state-of-the-art solvers. All the experiments were implemented in MATLAB on a laptop with an i7-7700HQ CPU and 16 GB RAM, and no parallelism programming was ever used. The explicit parameter setup for ICOS is given in

Table 1. Parameter setup of ICOS for the rotation search and point cloud registration problems.

Algorithm 1	Algorithm 3	Algorithm 4
$X=2/3(N=100)$	$X=4$	$X=4$
$X=4(N=500)$	$A=4.5\sigma$	$A=4.5\sigma$
$X=5(N=1000)$	$B=5.0\sigma$	$B=5.0\sigma$
$L=2.5\sigma$	$C=6.0\sigma$	$C=6.0\sigma$
$G=4\sigma$	$D=10.5\sigma$	$D=10.5\sigma$
$F=10.5\sigma$	$maxIt{r}_1=40000$	$maxIt{r}_3=X·400$
$maxIt{r}_1=40000$	$maxIt{r}_2=400$	$maxIt{r}_4=400$
$maxIt{r}_3=X·400$	$maxIt{r}_3=X·400$
$maxIt{r}_4=400$	$maxIt{r}_4=400$

.

6.1. Benchmarking of Rotation Search on Synthetic Data

Experimental setup: We tested our solver ICOS for rotation search over synthetic datasets, where our environmental setup was similar to that in [23]. We first randomly generated $N=\{100,500,1000\}$ vectors $\mathcal{U}={\left\{{\mathbf{u}}_i\right\}}_{i=1}^N$ with unit-norm, and rotated $\mathcal{U}$ with a random rotation $\mathit{R}\in SO\left(3\right)$ to obtain a new vector set $\mathcal{V}={\left\{{\mathbf{v}}_i\right\}}_{i=1}^N$ in another coordinate frame. We then added random zero-mean Gaussian noise with $\sigma =0.01$ and $\mu =0$ to all vectors in $\mathcal{V}$. After that, a portion of the vectors in $\mathcal{V}$ (from 0% up to 99%) were substituted by randomly generated unit-norm vectors to generate outliers among the correspondences. All the experimental results were obtained over 50 Monte Carlo runs in the same environment.

To quantitatively represent the errors of rotation, we adopted the geodesic errors such that:

$$E_{\mathit{R}}=\angle (\widehat{\mathit{R}},{\mathit{R}}_{gt})·\frac{{180}^{\circ }}{\pi },$$

(38)

where “∠” is the geodesic error (21) [51] and the subscript “$gt$” denotes the ground truth value.

For benchmarking, we tested our solver ICOS (Algorithm 1) against FGR [22], GORE [21], RANSAC, BnB [38], and RANSIC [49]. For FGR, we merely solved the rotation rather than the whole transformation using the Gauss–Newton method. For RANSAC, we adopted Horn’s minimal method [34] to solve the rotation with the confidence set to 0.995, where we designed two stop conditions: (i) RANSAC with at most 100 iterations, named RANSAC (100), and (ii) RANSAC with at most 1000 iterations, named RANSAC (1000). When $N=1000$, we excluded BnB as it took tens of minutes per run. QUASAR [23] was also not adopted due to its very long runtime.

Results:Figure 3 displays the benchmarking results on both accuracy and runtime. It is not hard to observe that: (i) both FGR and RANSAC (100) failed at 90% and RANSAC (1000) broke at 98%, while RANSIC and our ICOS were robust against 95% with $N=100$, 98% with $N=500$, and 99% with $N=1000$; (ii) ICOS was (one of) the most accurate solvers for all times since it constantly demonstrated the lowest rotation errors; (iii) with $N=\{500,1000\}$, ICOS was faster than the other solvers excluding RANSIC when the outlier ratio was below 95%, and with $N=100$, it had a similar speed as GORE when the outlier ratio was no greater than 60%; (iv) though ICOS was slightly slower than RANSIC at 90–98% outliers, it became more time efficient at 99%.

6.2. Benchmarking of Point Cloud Registration on Real Data

Experimental setup: We evaluated ICOS against other state-of-the-art methods over the “bunny” and the “Armadillo” point clouds from the Stanford 3D Scanning Repository [52], and the environmental setup was inspired by [24,25]. We first randomly downsampled the point cloud to 1000 points and resized it to be placed in a cube of ${[-0.5,0.5]}^3$ meters, which can be regarded as the initial point set $\mathcal{P}={\left\{{\mathbf{P}}_i\right\}}_{i=1}^{1000}$. Then, we generated a random rigid transformation: ($\mathit{s},\mathit{R},\mathit{t}$), in which scale $\mathit{s}\in [1,5)$, rotation $\mathit{R}\in SO\left(3\right)$, and translation $\left|\right|\mathit{t}\left|\right|\le 3$ m, to transform point set $\mathcal{P}$. We added random Gaussian noise with $\sigma =0.01$ and $\mu =0$ to the transformed point set, as the point set $\mathcal{Q}={\left\{{\mathbf{Q}}_i\right\}}_{i=1}^{1000}$. To create outliers in clutter, a portion of the points in $\mathcal{Q}$ were substituted by random points inside a sphere of diameter $\mathit{s}\sqrt{3}$ m according to the outlier ratio (from 0% up to 99%), as exemplified in Figure 4. All the experimental results were obtained over 50 Monte Carlo runs in the same environment.

The estimation errors in all experiments were computed as:

$$\begin{array}{c}{E}_{\mathit{s}}=\left|\widehat{\mathit{s}}-{\mathit{s}}_{gt}\right|,\\ E_{\mathit{t}}=\parallel \widehat{\mathit{t}}-{\mathit{t}}_{gt}\parallel ,\end{array}$$

(39)

and $E_{\mathit{R}}$ is the same as (38), where subscripts “$gt$” indicate the ground truth.

Known-scale problems: We adopted FGR, GORE, RANSAC, TEASER, RANSIC, and ICOS for the known-scale point cloud registration problems. As for RANSAC, we used Horn’s minimal (3-point) method [34] to solve the rigid transformation, and the confidence was set to 0.995. There were two stop conditions for RANSAC: (i) RANSAC with at most 1000 iterations, named RANSAC (1000), and (ii) RANSAC with at most 1 min of runtime, named RANSAC (1 min). In terms of TEASER, we adopted the GNC heuristic version as in [25], which uses GNC-TLS to solve the large robust SDP problem, rather than using convex relaxation, but differently: (i) we did not apply parallelism programming in order to ensure the fairness in the runtime evaluation for all the tested solvers; (ii) TEASER was completely implemented in MATLAB, not in C++, using the fast maximum clique solver [53].

Unknown-scale problems: For unknown-scale registration, we only compared ICOS with RANSAC and RANSIC because: (i) FGR and GORE were not designed for scale estimation originally; (ii) the scale estimator Adaptive Voting in TEASER was too slow for handling 1000 correspondences (requiring tens of minutes per run) and was also unable to tolerate over 90% outliers, thus no need to be compared.

Results: The main benchmarking results are shown in Figure 4 (over the “bunny” point cloud) and Figure 5 (over the “Armadillo” point cloud). We can observe that: (i) in the known-scale registration problem, GORE, TEASER, RANSIC, and our ICOS were all robust against 99% outliers, but GORE was hardly accurate; (ii) RANSAC (1000) failed at 90%, while FGR and RANSAC (1 min) failed at over 95%, not as robust as ICOS; (iii) in the unknown-scale registration problems, ICOS could still address 99% outliers highly robustly; (iv) ICOS was the fastest solver when the outlier ratio was below 98% (note that though FGR seemed faster at over 96%, it already became no longer robust) and was constantly faster than RANSIC when the outlier ratio was high (e.g., 90–99%).

6.3. Evaluation on the Inlier Recall Ratio

We tested the capability of ICOS for recalling correct inliers from the outlier-corrupted putative correspondence set in the rotation search and known-scale and unknown-scale registration with the same experimental setup as in Section 6.1 and Section 6.2, setting $N=1000$ for all the problems, and the results are shown in Figure 6. The recall ratio is defined as the ratio of the number of true inliers found by ICOS to the ground truth number of inliers among the original correspondences. We can observe that ICOS was able to recall approximately 100% (no less than 99%) of the inliers from the correspondences with outlier ratio ranging from 0% to 99%, which underlies its high estimation accuracy against noise in the benchmarking above.

6.4. Benchmarking on Scale Estimation in Registration

We benchmarked ICOS for scale estimation alone, compared with the Adaptive Voting in TEASER/TEASER++ and the 1-point RANSAC in [44]. Following the setup in Section 6.2, we further downsampled the “bunny” to only 100 points because the solver Adaptive Voting would run in tens of minutes with 1000 correspondences. In each run, the scale was randomly generated within $(1,5)$ m. Figure 7 shows the scale errors and the runtime w.r.t. an increasing outlier ratio. We can see that both Adaptive Voting and 1-point RANSAC broke at 90% outliers, while ICOS could stably tolerate 90% outliers and was faster than the other two competitors. In fact, ICOS could be robust against up to 99% outliers, as shown in Section 6.2.

6.5. Qualitative Registration Results over Real Datasets

In addition to the quantitative point cloud registration results rendered above, we provide qualitative results of the registration problems over multiple real point clouds, including the “bunny” and “Armadillo” from [52], the “Mario” and “Squirrel” from the SHOT dataset (http://vision.deis.unibo.it/list-all-categories/78-cvlab/80-shot, accessed on 30 July 2021), and the “city” and “castle” from the RGB-D Scans dataset [26], for concrete evaluation.

The experiments were set up as follows: (i) we transformed a certain point cloud with a random transformation (with both a known scale and unknown scale); (ii) we adopted the FPFH 3D feature descriptor [54] to match and build putative correspondences between the transformed point cloud (colored in magenta) and the initial one (colored in blue), where outliers are bound to exist; (iii) we applied ICOS to solve the transformation and then used it to reproject the transformed point cloud back to its initial place. The qualitative results are shown in Figure 8. After reprojection, the better the magenta point cloud overlaps with the blue one, the better the registration can be.

From Figure 8, we see that ICOS yielded rather satisfactory robust registration results.

6.6. Additional Benchmarking on High Noise

We conducted more experiments on rotation search and unknown-scale point cloud registration with high noise. As the “bunny” point cloud is in a ${[-0.5,0.5]}^3$ box, adding noise with $\sigma =0.1$ is extreme, as shown in Figure 9, where the shape of the “bunny” is hardly seen. We tested ICOS against other solvers with $N=100$ (rotation search) and with $N=1000$ (registration). The results are shown in Figure 9 (in registration, the translation errors were similar to the rotation errors, thus omitted).

We can observe that ICOS was still robust against at least 80% outliers in both of the tested problems with such high noise, while remaining time efficient, outperforming other competitors.

6.7. Application 1: Image Stitching

To validate ICOS in the real-world application of rotation search, we adopted ICOS to solve the image stitching problem over the LunchRoom image sequence from the PASSTA dataset [55], with an experimental setup similar to [23]. We first used the 2D feature descriptor SURF [16] to detect and match the 2D keypoints across a pair of images with overlapping scenes. Then, we applied the inverse of the camera intrinsic matrix ${\mathbf{K}}^{-1}$ to create the vector correspondences $\mathcal{U}={\left\{{\mathbf{u}}_i\right\}}_{i=1}^N$ and $\mathcal{V}={\left\{{\mathbf{v}}_i\right\}}_{i=1}^N$ over the two frames. Eventually, we used Algorithm 1 (with $\sigma =0.02$) to solve the rotation $\widehat{\mathit{R}}$ between the two images and then obtained the homography matrix using matrix transformation: $\mathbf{H}=\mathbf{K}\widehat{\mathit{R}}{\mathbf{K}}^{-1}$ to stitch them together. The correspondences and experimental results are shown in Figure 10. We found that ICOS always found the correct inliers among the correspondences and stitched the images successfully.

6.8. Application 2: 3D Object Localization

We tested ICOS over RGB-D scenes datasets [56] for tackling the 3D object localization (pose estimation) problems, as operated in [24,25]. We first extracted the 3D point cloud of the target object (cereal box, cap, and table in this case) from the scene according to the labels and imposed a random rigid transformation (with both a known scale and unknown scale), as well as random noise with $\sigma =0.01$ and $\mu =0$ on the target object. We utilized FPFH to build putative correspondences between the scene and the transformed object. We found that the correspondences constructed by FPFH contained over 80% outliers in all the scenes tested; even worse, the outlier ratios even exceeded 90% in most scenes.

For each object-scene, we applied ICOS, RANSAC (1000), and RANSAC (1 min) to estimate the transformation (relative pose) of the object w.r.t. the scene and then reprojected the object back to the scene with the transformation estimated by each solver, respectively. The qualitative results are demonstrated in Figure 11 and Figure 12, and the quantitative results are specified in

Table 2. Quantitative object localization results on cereal box.

Scene No.	ICOS	RANSAC (1000)	RANSAC (1 min)
Scene-02, $\mathit{s}=1$	$E_{\mathit{R}}=$0.196	$E_{\mathit{R}}=154.820$	$E_{\mathit{R}}=0.727$
$N=597$, 93.80%	$E_{\mathit{t}}=\mathbf{0.004}$	$E_{\mathit{t}}=1.120$	$E_{\mathit{t}}=0.013$
	$t=\mathbf{0.071}$	$t=1.559$	$t=34.308$
Scene-05, $\mathit{s}=1$	$E_{\mathit{R}}=\mathbf{0.202}$	$E_{\mathit{R}}=61.278$	$E_{\mathit{R}}=0.991$
$N=606$, 90.92%	$E_{\mathit{t}}=\mathbf{0.003}$	$E_{\mathit{t}}=1.092$	$E_{\mathit{t}}=0.024$
	$t=\mathbf{0.057}$	$t=1.578$	$t=11.110$
Scene-07, $\mathit{s}=1$	$E_{\mathit{R}}=\mathbf{0.188}$	$E_{\mathit{R}}=0.449$	$E_{\mathit{R}}=1.736$
$N=249$, 81.53%	$E_{\mathit{t}}=\mathbf{0.005}$	$E_{\mathit{t}}=0.013$	$E_{\mathit{t}}=0.056$
	$t=\mathbf{0.055}$	$t=0.579$	$t=0.582$
Scene-09, $\mathit{s}=1$	$E_{\mathit{R}}=\mathbf{0.144}$	$E_{\mathit{R}}=3.319$	$E_{\mathit{R}}=0.546$
$N=284$, 90.16%	$E_{\mathit{t}}=\mathbf{0.003}$	$E_{\mathit{t}}=0.048$	$E_{\mathit{t}}=0.012$
	$t=\mathbf{0.066}$	$t=1.663$	$t=21.465$
	$E_{\mathit{R}}=\mathbf{0.046}$	$E_{\mathit{R}}=63.040$	$E_{\mathit{R}}=0.257$
Scene-02, $\mathit{s}\in (1,5)$	$E_{\mathit{t}}=\mathbf{0.004}$	$E_{\mathit{t}}=5.389$	$E_{\mathit{t}}=0.233$
$N=618$, 94.82%	$E_{\mathit{s}}=\mathbf{0.001}$	$E_{\mathit{s}}=4.754$	$E_{\mathit{s}}=0.004$
	$t=\mathbf{0.039}$	$t=1.621$	$t=59.095$
	$E_{\mathit{R}}=\mathbf{0.030}$	$E_{\mathit{R}}=152.878$	$E_{\mathit{R}}=48.347$
Scene-05, $\mathit{s}\in (1,5)$	$E_{\mathit{t}}=\mathbf{0.003}$	$E_{\mathit{t}}=5.094$	$E_{\mathit{t}}=5.093$
$N=555$, 95.14%	$E_{\mathit{s}}=\mathbf{0.001}$	$E_{\mathit{s}}=3.669$	$E_{\mathit{s}}=3.667$
	$t=\mathbf{0.185}$	$t=1.453$	$t=60.001$
	$E_{\mathit{R}}=\mathbf{0.057}$	$E_{\mathit{R}}=123.266$	$E_{\mathit{R}}=122.522$
Scene-07, $\mathit{s}\in (1,5)$	$E_{\mathit{t}}=\mathbf{0.004}$	$E_{\mathit{t}}=8.723$	$E_{\mathit{t}}=8.721$
$N=185$, 92.43%	$E_{\mathit{s}}=\mathbf{0.001}$	$E_{\mathit{s}}=4.475$	$E_{\mathit{s}}=4.475$
	$t=\mathbf{0.066}$	$t=0.535$	$t=6.393$
	$E_{\mathit{R}}=\mathbf{0.067}$	$E_{\mathit{R}}=3.633$	$E_{\mathit{R}}=143.444$
Scene-09, $\mathit{s}\in (1,5)$	$E_{\mathit{t}}=\mathbf{0.005}$	$E_{\mathit{t}}=0.241$	$E_{\mathit{t}}=4.081$
$N=548$, 95.26%	$E_{\mathit{s}}=\mathbf{0.002}$	$E_{\mathit{s}}=0.004$	$E_{\mathit{s}}=3.247$
	$t=\mathbf{0.142}$	$t=1.429$	$t=60.001$

and

Table 3. Quantitative object localization results on cap and table.

Scene No.	ICOS	RANSAC (1000)	RANSAC (1 min)
Scene-01, $\mathit{s}=1$	$E_{\mathit{R}}=$0.151	$E_{\mathit{R}}=65.867$	$E_{\mathit{R}}=0.662$
$N=341$, 90.91%	$E_{\mathit{t}}=\mathbf{0.005}$	$E_{\mathit{t}}=1.309$	$E_{\mathit{t}}=0.014$
	$t=\mathbf{0.060}$	$t=0.903$	$t=6.245$
Scene-03, $\mathit{s}=1$	$E_{\mathit{R}}=\mathbf{0.337}$	$E_{\mathit{R}}=128.182$	$E_{\mathit{R}}=0.924$
$N=316$, 94.62%	$E_{\mathit{t}}=\mathbf{0.008}$	$E_{\mathit{t}}=1.278$	$E_{\mathit{t}}=0.2018$
	$t=\mathbf{0.198}$	$t=0.851$	$t=27.688$
Scene-09, $\mathit{s}=1$	$E_{\mathit{R}}=\mathbf{0.058}$	$E_{\mathit{R}}=0.910$	$E_{\mathit{R}}=0.397$
$N=579$, 86.18%	$E_{\mathit{t}}=\mathbf{0.001}$	$E_{\mathit{t}}=0.022$	$E_{\mathit{t}}=0.010$
	$t=\mathbf{0.043}$	$t=1.514$	$t=3.013$
Scene-12, $\mathit{s}=1$	$E_{\mathit{R}}=\mathbf{0.068}$	$E_{\mathit{R}}=165.928$	$E_{\mathit{R}}=0.191$
$N=639$, 89.98%	$E_{\mathit{t}}=\mathbf{0.001}$	$E_{\mathit{t}}=1.747$	$E_{\mathit{t}}=0.004$
	$t=\mathbf{0.047}$	$t=1.645$	$t=8.537$
	$E_{\mathit{R}}=\mathbf{0.037}$	$E_{\mathit{R}}=101.892$	$E_{\mathit{R}}=151.913$
Scene-01, $\mathit{s}\in (1,5)$	$E_{\mathit{t}}=\mathbf{0.005}$	$E_{\mathit{t}}=5.779$	$E_{\mathit{t}}=5.782$
$N=579$, 94.99%	$E_{\mathit{s}}=\mathbf{0.002}$	$E_{\mathit{s}}=3.428$	$E_{\mathit{s}}=3.429$
	$t=\mathbf{0.069}$	$t=1.508$	$t=60.001$
	$E_{\mathit{R}}=\mathbf{0.139}$	$E_{\mathit{R}}=60.219$	$E_{\mathit{R}}=176.511$
Scene-03, $\mathit{s}\in (1,5)$	$E_{\mathit{t}}=\mathbf{0.010}$	$E_{\mathit{t}}=4.718$	$E_{\mathit{t}}=4.720$
$N=345$, 91.88%	$E_{\mathit{s}}=\mathbf{0.009}$	$E_{\mathit{s}}=3.552$	$E_{\mathit{s}}=3.553$
	$t=\mathbf{0.060}$	$t=0.936$	$t=9.052$
	$E_{\mathit{R}}=\mathbf{0.045}$	$E_{\mathit{R}}=0.065$	$E_{\mathit{R}}=0.096$
Scene-09, $\mathit{s}\in (1,5)$	$E_{\mathit{t}}=\mathbf{0.001}$	$E_{\mathit{t}}=0.004$	$E_{\mathit{t}}=0.007$
$N=619$, 80.94%	$E_{\mathit{s}}=\mathbf{0.001}$	$E_{\mathit{s}}=\mathbf{0.001}$	$E_{\mathit{s}}=0.002$
	$t=\mathbf{0.041}$	$t=1.264$	$t=1.237$
	$E_{\mathit{R}}=\mathbf{0.008}$	$E_{\mathit{R}}=128.166$	$E_{\mathit{R}}=0.079$
Scene-12, $\mathit{s}\in (1,5)$	$E_{\mathit{t}}=\mathbf{0.001}$	$E_{\mathit{t}}=7.151$	$E_{\mathit{t}}=0.005$
$N=631$, 89.54%	$E_{\mathit{s}}=\mathbf{0.001}$	$E_{\mathit{s}}=3.417$	$E_{\mathit{s}}=\mathbf{0.001}$
	$t=\mathbf{0.052}$	$t=1.613$	$t=7.560$

, in which the unit for $E_{\mathit{R}}$ is degrees (${}^{\circ }$) and the unit for $E_{\mathit{t}}$ is meters, computed as in (39). In each table, the best results are labeled in bold font.

From the results, ICOS was able to estimate the object poses robustly and accurately even with over 95% outliers among the 548 correspondences (e.g., the last scene in Table 2); however, the two RANSAC solvers failed in nearly half of the cases. Thus, in addition to its superior performance in standard benchmarking, ICOS manifested strong practicality for real problems.

7. Conclusions

In this paper, a novel, fast, and highly robust paradigm, ICOS, applicable to the correspondence-based rotation search and both the known-scale and unknown-scale point cloud registration problems was presented. We defined special compatible structures for the two problems above to generate invariants with mathematical constraints. Then, we rendered a sequence of well-designed invariant-constrained random sampling and inlier-seeking frameworks for these problems, which can eliminate outliers by means of fast Boolean conditions and search for qualified a-COS and b-COS as the inliers, giving birth to our solver ICOS. Generally, ICOS can automatically stop sampling and eventually return nearly 100% of the inliers from the putative correspondences.

We benchmarked ICOS against other state-of-the-art solvers in multiple experiments. We showed that: (i) ICOS is the most state-of-the-art, or at least one of the most stat-of-the-art, solvers for the two problems, since it was robust against over 95% outliers (at most up to 99%), had the best estimation accuracy, and ran fastest in most cases; (ii) ICOS demonstrated a similar robustness (robust against 99% outliers) and accuracy regardless of the scale situation, suitable for generalized point cloud registration; (iii) ICOS is practical for real-world applications. The MATLAB source code of ICOS can be found at: https://github.com/LeiSun-98/ICOS, (accessed on 1 August 2021).