不同ICP方法简单对比

接触到一种新的ICP方法，测试看一下效果。

---

论文

A Symmetric Objective Function for ICP

The Iterative Closest Point (ICP) algorithm, commonly used for alignment of 3D models, has previously been defined using either a point-topoint or point-to-plane objective. Alternatively, researchers have proposed computationally-expensive methods that directly minimize the distance function between surfaces. We introduce a new symmetrized objective function that achieves the simplicity and computational efficiency of point-to-plane optimization, while yielding improved convergence speed and a wider convergence basin. In addition, we present a linearization of the objective that is exact in the case of exact correspondences. We experimentally demonstrate the improved speed and convergence basin of the symmetric objective, on both smooth models and challenging cases involving noise and partial overlap。

迭代最近点( Iterative Closest Point，ICP )算法，通常用于3D模型的对齐，以前使用点到点或点到平面的目标来定义。有研究人员提出了计算代价昂贵的方法，直接最小化曲面之间的距离函数。我们引入了一个新的对称化目标函数，它实现了点到平面优化的简单性和计算效率，产生更快的收敛速度和更宽的收敛域。此外，我们给出了在精确对应的情况下精确的目标的线性化。我们通过实验证明了对称目标在光滑模型和涉及噪声和粒子的挑战性情况下的改进速度和收敛域。

对于从圆弧上采样的任意点 $p$ 和 $q$，它们之间的向量$p-q$垂直于法向量之和 $np+nq$ 。这是对称ICP公式所利用的基本性质。

有兴趣移步原文，这里不做展开了。

---

代码

trimesh中的实现

/*
Szymon Rusinkiewicz
Princeton UniversityICP.cc
Iterative Closest Point alignment using covariance-weighted sampling,
adaptive outlier rejection, and symmetric point-to-plane minimization.
*/...
// Do symmetric point-to-plane alignment, returning alignxf
// as well as eigenvectors and inverse eigenvalues
static void align_symm(const vector<PtPair> &pairs, float scale,const point &centroid1, const point &centroid2,float median_dist, xform &alignxf)
{float huber_thresh = HUBER_THRESH_MULT * scale * median_dist;size_t npairs = pairs.size();float A[6][6] = { { 0 } }, b[6] = { 0 };for (size_t i = 0; i < npairs; i++) {vec p1 = scale * (pairs[i].p1 - centroid1);vec p2 = scale * (pairs[i].p2 - centroid2);vec n = pairs[i].n1 + pairs[i].n2;vec p = p1 + p2;vec c = p CROSS n;vec d = p1 - p2;float x[6] = { c[0], c[1], c[2], n[0], n[1], n[2] };float dn = d DOT n;// Huber weights, used for IRLSfloat wt = huber_thresh / max(fabs(dn), huber_thresh);for (int j = 0; j < 6; j++) {b[j] += wt * dn * x[j];for (int k = j; k < 6; k++)A[j][k] += wt * x[j] * x[k];}}// Make matrix symmetricfor (int j = 1; j < 6; j++)for (int k = 0; k < j; k++)A[j][k] = A[k][j];// Eigen-decomposition and inversefloat eval[6], einv[6];eigdc<float,6>(A, eval);for (int i = 0; i < 6; i++)einv[i] = 1.0f / (eval[i] + REGULARIZATION * eval[5]);// Solve systemeigmult<float,6>(A, einv, b);// Extract rotation and translationvec rot(b[0], b[1], b[2]), trans(b[3], b[4], b[5]);float rotangle = atan(len(rot));trans *= cos(rotangle);trans *= 1.0f / scale;xform R = xform::rot(rotangle, rot);alignxf = xform::trans(centroid1) *R * xform::trans(trans) * R *xform::trans(-centroid2);
}
...

trimesh2中的代码就是原作者自己实现的，我愿称之为优雅，实现也比较简单，重点在目标函数的构建和求解。

---

测试

PCL 中的ICP对比

对两个点云配准

未配准的bunny

P2P

P2L

SYM

trimesh2 中的ICP对比

对两个mesh配准

这是两个不同视角的bunny mesh，在原始位姿上我加了一点扰动，使其初始位姿更差，两个mesh的分界更明显，在后面的配准效果对比中，我没有输出量化指标，可以通过可视化定性比较，重点关注两个mesh的边界

未配准的bunny

P2P

P2L

SYM

two plane

---

总的来说，SYM-ICP效果更优，尤其针对法线比较准确的数据，能够得到很好的结果，相对来说（尤其相对于P2P-ICP）收敛更快，不容易陷入局部最优(但也存在），即其文中所说的更快的收敛速度和更广的收敛域。