不同ICP方法简单对比

WoooChi

已于 2024-08-25 09:25:12 修改

阅读量1k

点赞数 13

分类专栏： 3D 文章标签：算法 3d

于 2024-08-24 08:00:00 首次发布

本文链接：https://blog.csdn.net/m0_50910915/article/details/140274440

版权

3D 专栏收录该内容

18 篇文章 22 订阅

订阅专栏

不同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 University

ICP.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 IRLS
		float 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 symmetric
	for (int j = 1; j < 6; j++)
		for (int k = 0; k < j; k++)
			A[j][k] = A[k][j];

	// Eigen-decomposition and inverse
	float 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 system
	eigmult<float,6>(A, einv, b);

	// Extract rotation and translation
	vec 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);
}
...