ICP 算法步骤

The Iterative Closest Point (ICP) is an algorithm employed to match two surface representations, such as points clouds or polygon meshes.

Method 1

Optimal transformation:

$min_{R,t}sum_{i}||Rp_i+t-q_i||_2^2$

R represented by Euler angles (3 degrees of freedom)

$Rp_i+t-q_i=left( egin{array}{cccccc} 0&p_i^z&-p_i^y&1&0&0\ -p_i^z&0&p_i^x&0&1&0\ p_i^y&-p_i^x&0&0&0&1 end{array} ight) x-left( egin{array}{c} p_i^x-q_i^x\ p_i^y-q_i^y\ p_i^z-q_i^z end{array} ight) =A_ix-b_i$

$A=[A_1,A_2,cdots,A_n]^T\,\,\,\,\,b=[b_1,b_2,cdots,b_n]^T$

Method 2

References:

1. Exercise 2: 3D Surface Registration

2. Build Your Own 3D Scanner: 3D Photography for Beginners