Algorithm 4 multi feature point matching algorithm fusing local geometric constraints
Input: F′ = {f′1, f′2, ⋯ ,f′m}: current RGB-D image feature point set; MIN_MATCH_SIZE: minimum match points
Output: C = {(fi′, fim)|fi ∈ F′,fim ∈ F}
1:	M = Φ, C = Φ ⊲ Set the initial value
2:	for i = 1 → n do
3:	Use hierarchical clustering to retrieve features, search the best match fim for f′iin feature point set F
4:	M = M ∪ {(f′i, fim)}
5:	end for
6:	Arbitrarily select matching point pair from M,ca = (f′a, fam) ∈ M
7:	C = C U {ca}, M = M/{ca} ⊲ remove ca from M
8:	if |M| == 0 then
9:	if |C| > MIN_MATCH_SIZE then
10:	Call Algorithm 5 to pose estimate
11:	end if
12:	end if
13:	while |M|! = 0 do
14:	Arbitrarily select matching point pair from M,cb ∈ M
15:	dxy = ‖px − py‖ ,dS = ‖p′x − p′x‖
16:	if |dS − dxy| ≤ Δd then
17:	C = C ∪ {ca}, M = M/{ca}
18:	end if
19:	if |C| > MIN_MATCH_SIZE then
20:	Call the Algorithm 5 to pose estimate
21:	end if
22:	end while