## 5 Our Solver: ICOS
### ICOS for Known-Scale Registration  
The pseudocode of ICOS for point cloud registration with a known scale (\(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}^{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).  
```
Input :correspondences \(\mathcal{N}=\{(\mathbf{P}_{i},\mathbf{Q}_{i})\}_{i=1}^{N}\); minimum number of 4-COS \(X\); Output :optimal \((\hat{\mathbf{R}},\hat{\mathbf{t}})\); inlier set \(\mathcal{N}^{In}\);
1 whiletruedo
2\(\mathcal{N}^{In}\leftarrow\emptyset\), \(count\gets 0\);
3 for\(itr_{1}=1:maxInt_{1}\)do
4 Randomly select \(\mathcal{R}=\{(\mathbf{P}_{i},\mathbf{Q}_{i})\}_{i=1}^{2}\subset\mathcal{N}\);
5 if\(\mathcal{R}\) satisfies constraint (24)then
6 for\(itr_{2}=1:maxInt_{2}\)do
7 Randomly select \((\mathbf{P}_{3},\mathbf{Q}_{3})\in\mathcal{N}\), and construct a 3-COS set \(\mathcal{O}\) with \(\mathcal{R}\);
8 if\(\mathcal{O}\) satisfies the constraints (24)then
9 break
10
11 end if
12 break
13
14 end if
15
16 end for
17
18 if\(\mathcal{O}\) satisfies Constraints (23)-(24)then
19 Solve \(s_{123},R_{123}\) with \(\mathcal{O}\) using (28) and (15);
20 if\(\mathcal{O}\) satisfies the constraints (30)then
21 for\(itr_{3}=1:maxInt_{3}\)do
22 CheckSampling;
23 Randomly select \((\mathbf{P}_{k},\mathbf{Q}_{k})\in\mathcal{N}\), and construct a 4-COS with \(\mathcal{O}\);
24 ifthis4-COS satisfies (32)-(35)then
25\(count\gets count+1\);
26\(\mathcal{N}^{In}\leftarrow\mathcal{N}^{In}\cup\{(\mathbf{P}_{k},\mathbf{Q}_{k })\}\);
27
28 end if
29 if\(count\geq X\)then
30\(\mathcal{N}^{In}\leftarrow\mathcal{N}^{In}\cup\mathcal{O}\);
31 break
32
33 end if
34
35 end if
36
37 end if
38
39 end for
40
41 end for
42 Solve \((\hat{\mathbf{R}},\hat{\mathbf{t}})\) with set \(\mathcal{N}^{In}\) using (8) and (10);
43 Compute the residual errors \(r_{i}\) (\(i=1,2,\ldots,N\)) for all the correspondences in \(\mathcal{N}\); if\(r_{i}\leq 5.2\sigma\), add it to \(\mathcal{N}^{In}\);
44 Solve optimal \((\hat{\mathbf{R}},\hat{\mathbf{t}})\) with \(\mathcal{N}^{In}\) using (8) and (10);
45 return optimal \((\hat{\mathbf{R}},\hat{\mathbf{t}})\) and the inlier set \(\mathcal{N}^{In}\);
```  
**Algorithm 3**ICOS for known-scale registration