[笔记]ICP算法(迭代最近点)——LOAM系列代码前序
最近在学习LOAM一系列的算法,看到激光SLAM前端匹配主要是基于ICP算法变体,所以学习一下。下面是个人愚见,期待大佬指正。
ICP(迭代最近点)算法
ICP(Iterative Closest Point 迭代最近点)算法是一种点集对点集配准方法。如下图所示,红色线 和 蓝色线 是两个点集,该算法就是计算怎么把红色线的点集平移旋转,使 红色线 和 蓝色线 尽量重叠。
一、原理
1、数学描述
用数学语言描述如下,即ICP算法的实质是基于最小二乘法的最优匹配,它重复进行“确定对应关系的点集→计算最优刚体变换”的过程,直到某个表示正确匹配的收敛准则得到满足。
假设我们通过LiDAR(激光雷达)得到了第一组点云 X = { x 1 , x 2 , . . . , x n } \boldsymbol X\boldsymbol=\mathbf{\{x_{1,}x_2,...,x_n\}} X={x1,x2,...,xn},LiDAR经过位姿变换(旋转加平移)后又得到了第二组点云 P = { p 1 , p 2 , . . . , p n } \boldsymbol P\boldsymbol=\mathbf{\{p_{1,}p_2,...,p_n\}} P={p1,p2,...,pn} 。注意这里的 X \boldsymbol X X和 P \boldsymbol P P的坐标分别对应移动前和移动后的坐标系(即坐标原点始终为LiDAR自身中心的右手坐标系,这里我们有移动前、移动后两个坐标系),并且我们通过相关算法筛选和调整了点云存储的顺序,使得 X , P \boldsymbol X,\boldsymbol P X,P中的点一一对应,如 ( x 11 , p 11 ) \left(x_{11},p_{11}\right) (x11,p11)在三维空间中对应同一个点。
现在我们要解决的问题是:计算LiDAR的旋转 R \boldsymbol R R和平移 t \boldsymbol t t,在没有误差的情况下,从 X \boldsymbol X X坐标系转换到 P \boldsymbol P P的公式为
p i = R x i + t {\boldsymbol p}_{\mathbf i}\boldsymbol={\boldsymbol R}_{{\mathbf x}_{\mathbf i}}\boldsymbol+\boldsymbol t pi=Rxi+t
但由于噪声及错误匹配(如
(
x
11
,
p
11
)
\left(x_{11},p_{11}\right)
(x11,p11)其实并不对应空间中同一点,但特征匹配算法错误地认为二者是同一点)的存在,上式不总是成立,所以我们要最小化的目标函数为
E
(
R
,
t
)
=
1
n
∑
i
=
1
n
∥
p
i
−
R
x
i
−
t
∥
2
E\left(R,t\right)=\frac{\mathbf1}{\mathbf n}\sum_{i=1}^n\left\|p_i-R_{x_i}-t\right\|^2
E(R,t)=n1i=1∑n∥pi−Rxi−t∥2
常用的求解
R
\boldsymbol R
R和
t
\boldsymbol t
t的方法有:
- SVD
- 非线性优化
非线性优化的描述比较繁琐,这次先介绍SVD方法。
2、SVD求解 R \boldsymbol R R、 t \boldsymbol t t方法
首先计算两个点云
X
\boldsymbol X
X和
P
\boldsymbol P
P的质心(center of mass),分别为
μ
x
\mu_x
μx和
μ
p
\mu_p
μp
μ
x
=
1
n
∑
i
=
1
n
x
i
\mu_x=\frac1n\sum_{i=1}^nx_i
μx=n1i=1∑nxi
μ
p
=
1
n
∑
i
=
1
n
p
i
\mu_p=\frac1n\sum_{i=1}^np_i
μp=n1i=1∑npi
并做如下处理:
1
n
∑
i
=
1
n
∥
p
i
−
R
x
i
−
t
∥
2
=
1
n
∑
i
=
1
n
∥
p
i
−
R
x
i
−
t
−
(
μ
p
−
R
μ
x
)
+
(
μ
p
−
R
μ
x
)
∥
2
=
1
n
∑
i
=
1
n
∥
(
p
i
−
μ
p
−
R
(
x
i
−
μ
x
)
)
+
(
μ
p
−
R
μ
x
−
t
)
∥
2
=
1
n
∑
i
=
1
n
(
∥
p
i
−
μ
p
−
R
(
x
i
−
μ
x
)
∥
2
+
∥
μ
p
−
R
μ
x
−
t
∥
2
+
2
(
p
i
−
μ
p
−
R
(
x
i
−
μ
x
)
)
T
(
μ
p
−
R
μ
x
−
t
)
)
\frac1n\sum_{i=1}^n\left\|p_i-R_{x_i}-t\right\|^2\\=\frac1n\sum_{i=1}^n\left\|p_i-R_{x_i}-t-\left(\mu_p-R\mu_x\right)+\left(\mu_p-R\mu_x\right)\right\|^2\\=\frac1n\sum_{i=1}^n\left\|\left(p_i-\mu_p-R\left(x_i-\mu_x\right)\right)+\left(\mu_p-R\mu_x-t\right)\right\|^2\\=\frac1n\sum_{i=1}^n\left(\left\|p_i-\mu_p-R\left(x_i-\mu_x\right)\right\|^2+\left\|\mu_p-R\mu_x-t\right\|^2+2\left(p_i-\mu_p-R\left(x_i-\mu_x\right)\right)^T\left(\mu_p-R\mu_x-t\right)\right)
n1i=1∑n∥pi−Rxi−t∥2=n1i=1∑n∥pi−Rxi−t−(μp−Rμx)+(μp−Rμx)∥2=n1i=1∑n∥(pi−μp−R(xi−μx))+(μp−Rμx−t)∥2=n1i=1∑n(∥pi−μp−R(xi−μx)∥2+∥μp−Rμx−t∥2+2(pi−μp−R(xi−μx))T(μp−Rμx−t))
然后最后一项处理一下
∑
i
=
1
n
(
p
i
−
μ
p
−
R
(
x
i
−
μ
x
)
)
T
(
μ
p
−
R
μ
x
−
t
)
=
(
μ
p
−
R
μ
x
−
t
)
T
∑
i
=
1
n
(
p
i
−
μ
p
−
R
(
x
i
−
μ
x
)
)
=
(
μ
p
−
R
μ
x
−
t
)
T
(
n
p
i
−
n
μ
p
−
R
(
n
μ
x
−
n
μ
x
)
)
=
0
\sum_{i=1}^n\left(p_i-\mu_p-R\left(x_i-\mu_x\right)\right)^T\left(\mu_p-R\mu_x-t\right)\\=\left(\mu_p-R\mu_x-t\right)^T\sum_{i=1}^n\left(p_i-\mu_p-R\left(x_i-\mu_x\right)\right)\\=\left(\mu_p-R\mu_x-t\right)^T\left(np_i-n\mu_p-R\left(n\mu_x-n\mu_x\right)\right)\\=0
∑i=1n(pi−μp−R(xi−μx))T(μp−Rμx−t)=(μp−Rμx−t)T∑i=1n(pi−μp−R(xi−μx))=(μp−Rμx−t)T(npi−nμp−R(nμx−nμx))=0
再然后在两个点云中分别减去对应的质心(Subtract the corresponding center of mass from every point in the two point sets before calculating the transformation),设为 x i ′ = x i − μ x x'_i=x_i-\mu_x xi′=xi−μx和 p i ′ = p i − μ p p'_i=p_i-\mu_p pi′=pi−μp,目标函数则可以简化为 1 n ∑ i = 1 n ( ∥ p i ′ − R x i ′ ∥ 2 + ∥ μ p − R μ x − t ∥ 2 ) \frac1n\sum_{i=1}^n\left(\left\|p'_i-\boldsymbol Rx'_i\right\|^2+\left\|\mu_p-\boldsymbol R\mu_x-t\right\|^2\right) n1i=1∑n(∥pi′−Rxi′∥2+∥μp−Rμx−t∥2)
设 R ∗ \boldsymbol R^\boldsymbol\ast R∗和 t ∗ \boldsymbol t^\boldsymbol\ast t∗为最优解,可将最优化问题分为两步:
- R ∗ = a r g m i n R 1 n ∑ i = 1 n ∥ p i ′ − R x i ′ ∥ 2 \boldsymbol R^\boldsymbol\ast\boldsymbol=\underset{\mathbf R}{\mathbf a\mathbf r\mathbf g\mathbf m\mathbf i\mathbf n}\frac1n\sum_{i=1}^n\left\|p'_i-\boldsymbol Rx'_i\right\|^2 R∗=Rargminn1∑i=1n∥pi′−Rxi′∥2
- t ∗ = μ p − R μ x \boldsymbol t^\boldsymbol\ast\boldsymbol=\mu_p-\boldsymbol R\mu_x t∗=μp−Rμx
重点在于步骤1,将它展开得:
R
∗
=
a
r
g
m
i
n
R
1
n
∑
i
=
1
n
(
p
i
′
T
p
i
′
+
x
i
′
T
R
T
R
x
i
′
−
2
p
i
′
T
R
x
i
′
)
=
a
r
g
m
i
n
R
1
n
∑
i
=
1
n
(
p
i
′
T
p
i
′
+
I
−
2
p
i
′
T
R
x
i
′
)
=
a
r
g
m
i
n
R
1
n
∑
i
=
1
n
(
−
p
i
′
T
R
x
i
′
)
\boldsymbol R^\ast=\underset{\boldsymbol R}{argmin}\frac1n\sum_{i=1}^n\left(p_i^{'T}p'_i+x_i^{'T}\boldsymbol R^T\boldsymbol Rx'_i-2p_i^{'T}\boldsymbol Rx'_i\right)\\=\underset{\boldsymbol R}{argmin}\frac1n\sum_{i=1}^n\left(p_i^{'T}p'_i+\boldsymbol I-2p_i^{'T}\boldsymbol Rx'_i\right)\\=\underset{\boldsymbol R}{argmin}\frac1n\sum_{i=1}^n\left(-p_i^{'T}\boldsymbol Rx'_i\right)
R∗=Rargminn1i=1∑n(pi′Tpi′+xi′TRTRxi′−2pi′TRxi′)=Rargminn1i=1∑n(pi′Tpi′+I−2pi′TRxi′)=Rargminn1i=1∑n(−pi′TRxi′)
令
W
=
∑
i
=
1
n
p
i
′
x
i
′
T
\boldsymbol W\boldsymbol=\sum_{i=1}^np'_ix_i^{'T}
W=∑i=1npi′xi′T,通过SVD分解:
W
=
U
Σ
V
T
\boldsymbol W\boldsymbol=\boldsymbol U\boldsymbol\Sigma\boldsymbol V^{\mathbf T}
W=UΣVT
当
W
\boldsymbol W
W满秩时有
Σ
=
[
σ
1
σ
2
σ
3
]
\boldsymbol\Sigma\boldsymbol=\begin{bmatrix}\sigma_1&&\\&{\mathrm\sigma}_2&\\&&{\mathrm\sigma}_3\end{bmatrix}
Σ=⎣⎡σ1σ2σ3⎦⎤
且对应唯一的
U
,
V
\boldsymbol U,\boldsymbol V
U,V组合,对应地
R
∗
=
U
V
T
\boldsymbol R^\ast=\boldsymbol U\boldsymbol V^{\mathbf T}
R∗=UVT
t
∗
=
μ
p
−
R
μ
x
\boldsymbol t^\ast=\mu_p-\boldsymbol R\mu_x
t∗=μp−Rμx就是我们要找的最优解(至于为什么
R
∗
\boldsymbol R^\ast
R∗具有以上形式请参见此链接)
3、非线性优化方法求解 R \boldsymbol R R、 t \boldsymbol t t(TODO)
二、ICP算法分析
1、ICP算法优点
- 可以获得非常精确的配准效果
- 不必对处理的点集进行分割和特征提取
- 在较好的初值情况下,可以得到很好的算法收敛性
2、ICP算法的不足之处
- 在搜索对应点的过程中,计算量非常大,这是传统ICP算法的瓶颈
- 标准ICP算法中寻找对应点时,认为欧氏距离最近的点就是对应点。这种假设有不合理之处,会产生一定数量的错误对应点。
3、ICP算法改进方面
针对标准ICP算法的不足之处,许多研究者提出ICP算法的各种改进版本,主要涉及如下所示的6个方面。
标准ICP算法中,选用点集中所有的点来计算对应点,通常用于配准的点集元素数量都是非常巨大的,通过这些点集来计算,所消耗的时间很长。在后来的研究中,提出了各种方法来选择配准元素,这些方法的主要目的都是为了减小点集元素的数目,即如何选用最少的点来表征原始点集的全部特征信息。在点集选取时可以:
- 选取所有点;
- 均匀采样(Uniform sub-sampling )
- 随机采样(Random sampling)
- 按特征采样(Feature based Sampling ) 备注:LOAM系列主要采用此方法降低ICP运算复杂度
- 法向空间均匀采样(Normal-space sampling)
按特征采样的最近邻点(LOAM用此方法)
基于特征的采样使用一些具有明显特征的点集来进行配准,大量减少了对应点的数目。
最近邻点(Closet Point):
法方向最近邻点(Normal Shooting):
投影法(Projection):
三、例程
MATLAB2018例程
MATLAB R2018a提供了ICP例程,需要安装Computer Vision System Toolbox。
调用ICP函数
tform = pcregistericp(ptCloudTformed,ptCloud,'Extrapolate',true);
python例程
import numpy as np
def best_fit_transform(A, B):
'''
Calculates the least-squares best-fit transform between corresponding 3D points A->B
Input:
A: Nx3 numpy array of corresponding 3D points
B: Nx3 numpy array of corresponding 3D points
Returns:
T: 4x4 homogeneous transformation matrix
R: 3x3 rotation matrix
t: 3x1 column vector
'''
assert len(A) == len(B)
# translate points to their centroids
centroid_A = np.mean(A, axis=0)
centroid_B = np.mean(B, axis=0)
AA = A - centroid_A
BB = B - centroid_B
# rotation matrix
W = np.dot(BB.T, AA)
U, s, VT = np.linalg.svd(W)
R = np.dot(U, VT)
# special reflection case
if np.linalg.det(R) < 0:
VT[2,:] *= -1
R = np.dot(U, VT)
# translation
t = centroid_B.T - np.dot(R,centroid_A.T)
# homogeneous transformation
T = np.identity(4)
T[0:3, 0:3] = R
T[0:3, 3] = t
return T, R, t
def nearest_neighbor(src, dst):
'''
Find the nearest (Euclidean) neighbor in dst for each point in src
Input:
src: Nx3 array of points
dst: Nx3 array of points
Output:
distances: Euclidean distances (errors) of the nearest neighbor
indecies: dst indecies of the nearest neighbor
'''
indecies = np.zeros(src.shape[0], dtype=np.int)
distances = np.zeros(src.shape[0])
for i, s in enumerate(src):
min_dist = np.inf
for j, d in enumerate(dst):
dist = np.linalg.norm(s-d)
if dist < min_dist:
min_dist = dist
indecies[i] = j
distances[i] = dist
return distances, indecies
def icp(A, B, init_pose=None, max_iterations=50, tolerance=0.001):
'''
The Iterative Closest Point method
Input:
A: Nx3 numpy array of source 3D points
B: Nx3 numpy array of destination 3D point
init_pose: 4x4 homogeneous transformation
max_iterations: exit algorithm after max_iterations
tolerance: convergence criteria
Output:
T: final homogeneous transformation
distances: Euclidean distances (errors) of the nearest neighbor
'''
# make points homogeneous, copy them so as to maintain the originals
src = np.ones((4,A.shape[0]))
dst = np.ones((4,B.shape[0]))
src[0:3,:] = np.copy(A.T)
dst[0:3,:] = np.copy(B.T)
# apply the initial pose estimation
if init_pose is not None:
src = np.dot(init_pose, src)
prev_error = 0
for i in range(max_iterations):
# find the nearest neighbours between the current source and destination points
distances, indices = nearest_neighbor(src[0:3,:].T, dst[0:3,:].T)
# compute the transformation between the current source and nearest destination points
T,_,_ = best_fit_transform(src[0:3,:].T, dst[0:3,indices].T)
# update the current source
# refer to "Introduction to Robotics" Chapter2 P28. Spatial description and transformations
src = np.dot(T, src)
# check error
mean_error = np.sum(distances) / distances.size
if abs(prev_error-mean_error) < tolerance:
break
prev_error = mean_error
# calculcate final tranformation
T,_,_ = best_fit_transform(A, src[0:3,:].T)
return T, distances
if __name__ == "__main__":
A = np.random.randint(0,101,(20,3)) # 20 points for test
rotz = lambda theta: np.array([[np.cos(theta),-np.sin(theta),0],
[np.sin(theta),np.cos(theta),0],
[0,0,1]])
trans = np.array([2.12,-0.2,1.3])
B = A.dot(rotz(np.pi/4).T) + trans
T, distances = icp(A, B)
np.set_printoptions(precision=3,suppress=True)
print T
上面代码创建一个源点集A(在0-100的整数范围内随机生成20个3维数据点),然后将A绕Z轴旋转45°并沿XYZ轴分别移动一小段距离,得到点集B。结果如下,可以看出ICP算法正确的计算出了变换矩阵。
需要注意几点:
- 首先需要明确公式里的变换是T(X→P), 即通过旋转和平移把点集P变换到X。我们这里求的变换是T(A→B),要搞清楚对应关系。
- 本例只用了20个点进行测试,ICP算法在求最近邻点的过程中需要计算20×20次距离并比较大小。如果点的数目巨大,那算法的效率将会非常低。
- 两个点集的初始相对位置对算法的收敛性有一定影响,最好在“足够近”的条件下进行ICP配准。
C++例程
计算点云中心点
//计算点云P的中心点mean
void CalculateMeanPoint3D(vector<Point3D> &P, Point3D &mean)
{
vector<Point3D>::iterator it;
mean.x = 0;
mean.y = 0;
mean.z = 0;
for(it=P.begin(); it!=P.end(); it++){
mean.x += it->x;
mean.y += it->y;
mean.z += it->z;
}
mean.x = mean.x/P.size();
mean.y = mean.y/P.size();
mean.z = mean.z/P.size();
}
利用控制点求初始旋转矩阵
在确定对应关系时,所使用的几何特征是空间中位置最近的点。这里,我们甚至不需要两个点集中的所有点。可以指用从某一点集中选取一部分点,一般称这些点为控制点(Control Points)。这时,配准问题转化为:
这里,pi,qi为最近匹配点。
在Geomagic Studio中利用三个点就可以进行两个模型的“手动注册”(感觉这里翻译的不好,Registration,应该为“手动匹配”)。
我们将手动选择的三个点导出,作为实验初始的控制点:
对于每一对矩阵Ai,计算矩阵B:
double B[16];
for(int i=0;i<16;i++)
B[i]=0;
for(itp=P.begin(),itq=Q.begin();itp!=P.end();itp++,itq++ ){
double divpq[3]={itp->x,itp->y,itp->z};
double addpq[3]={itp->x,itp->y,itp->z};
double q[3]={itq->x,itq->y,itq->z};
MatrixDiv(divpq,q,3,1);
MatrixAdd(addpq,q,3,1);
double A[16];
for(int i=0;i<16;i++)
A[i]=0;
for(int i=0;i<3;i++){
A[i+1]=divpq[i];
A[i*4+4]=divpq[i];
A[i+13]=addpq[i];
}
double AT[16],AMul[16];
MatrixTran(A,AT,4,4);
MatrixMul(A,AT,AMul,4,4,4,4);
MatrixAdd(B,AMul,4,4);
}
//使用奇异值分解计算B的特征值和特征向量
double eigen, qr[4];
MatrixEigen(B, &eigen, qr, 4);
//计算n阶正定阵m的特征值分解:eigen为特征值,q为特征向量
void MatrixEigen(double *m, double *eigen, double *q, int n)
{
double *vec, *eig;
vec = new double[n*n];
eig = new double[n];
CvMat _m = cvMat(n, n, CV_64F, m);
CvMat _vec = cvMat(n, n, CV_64F, vec);
CvMat _eig = cvMat(n, 1, CV_64F, eig);
//使用OpenCV开源库中的矩阵操作求解矩阵特征值和特征向量
cvEigenVV(&_m, &_vec, &_eig);
*eigen = eig[0];
for(int i=0; i<n; i++)
q[i] = vec[i];
delete[] vec;
delete[] eig;
}
//计算旋转矩阵
void CalculateRotation(double *q, double *R)
{
R[0] = q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3];
R[1] = 2.0 * (q[1]*q[2] - q[0]*q[3]);
R[2] = 2.0 * (q[1]*q[3] + q[0]*q[2]);
R[3] = 2.0 * (q[1]*q[2] + q[0]*q[3]);
R[4] = q[0]*q[0] - q[1]*q[1] + q[2]*q[2] - q[3]*q[3];
R[5] = 2.0 * (q[2]*q[3] - q[0]*q[1]);
R[6] = 2.0 * (q[1]*q[3] - q[0]*q[2]);
R[7] = 2.0 * (q[2]*q[3] + q[0]*q[1]);
R[8] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
}
平移矩阵计算
//通过特征向量计算旋转矩阵R1和平移矩阵T1
CalculateRotation(qr, R1);
double mean_Q[3]={_mean_Q.x,_mean_Q.y,_mean_Q.z};
double mean_P[3]={_mean_P.x,_mean_P.y,_mean_P.z};
double meanT[3]={0,0,0};
int nt=0;
for(itp=P.begin(),itq=Q.begin();itp!=P.end();itp++,itq++ ){
double tmpP[3]={itp->x,itp->y,itp->z};
double tmpQ[3]={itq->x,itq->y,itq->z};
double tmpMul[3];
MatrixMul(R1, mean_P, tmpMul, 3, 3, 3, 1);
MatrixDiv(tmpQ,tmpMul,3,1);
MatrixAdd(meanT,tmpQ,3,1);
nt++;
}
for(int i=0; i<3; i++)
T1[i] = meanT[i]/(double)(nt);
迭代最近点
//计算误差和d
d = 0.0;
if(round==1){
FindClosestPointSet(data,P,Q);
}
int pcount=0;
for(itp = P.begin(),itq=Q.begin();itp!=P.end(); itp++, itq++){
double sum = (itp->x - itq->x)*(itp->x - itq->x) + (itp->y - itq->y)*(itp->y - itq->y)
+ (itp->z - itq->z)*(itp->z - itq->z);
d += sum;
pcount++;
}
d=d/(double)(pcount);
参考资料:
[1]高翔,视觉SLAM十四讲7.9 3D-3D: ICP
[2]Wolfram Burgard, et al. Introduction to Mobile Robotics: Iterative Closest Point Algorithm
[3]ICP迭代最近点算法综述
[4]知乎答主:灯灯
[5]博主:XXX已失联