不同ICP方法简单对比

本文主要是介绍不同ICP方法简单对比，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

不同ICP方法比较

论文
代码
- trimesh中的实现
测试
- PCL 中的ICP对比
- trimesh2 中的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$ 垂直于法向量之和 $n p + n q$ 。这是对称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);
}
...