ICP算法推导

最新推荐文章于 2023-07-22 16:43:13 发布

骑羊儿放狼

最新推荐文章于 2023-07-22 16:43:13 发布

阅读量341

点赞数

文章标签：算法 matlab 开发语言

原文链接：https://blog.csdn.net/qq_41685265/article/details/107140349

版权

本文详细介绍了迭代最近点（ICP）算法，这是一种用于点云匹配的技术。在假设通过RGB-D相机获取两组点云数据后，文章探讨了如何在存在噪声和错误匹配情况下，通过最小化目标函数来计算相机的旋转和平移。文中提到了使用SVD进行非线性优化，并提供了一个MATLAB示例来说明ICP算法的应用。

摘要由CSDN通过智能技术生成

ICP算法详解——我见过最清晰的解释

迭代最近点（Iterative Closest Point, 下简称ICP）算法是一种点云匹配算法。

假设我们通过RGB-D相机得到了第一组点云 $P = \left \{p _{1} ,p _{2} ,p _{3} ,\cdots ,p _{n} \right \}$ ，相机经过位姿变换（旋转加平移）后又拍摄了第二组点云 $Q = \left \{q _{1} ,q _{2} ,q _{3} ,\cdots ,q _{n} \right \}$ ,注意这里的和的坐标分别对应移动前和移动后的坐标系（即坐标原点始终为相机光心，这里我们有移动前、移动后两个坐标系），并且我们通过相关算法筛选和调整了点云存储的顺序，使得和中的点一一对应，如 $\left (P _{99},Q _{99} \right )$ 在三维空间中对应同一个点。

现在我们要解决的问题是：计算相机的旋转和平移，在没有误差的情况下，从坐标系转换到的公式为：

$q_{i} = Rp_{i} + t$

但由于噪声及错误匹配（如 $\left (P _{99},Q _{99} \right )$ 其实并不对应空间中同一点，但特征匹配算法错误地认为二者是同一点）的存在，上式不总是成立，所以我们要最小化的目标函数为

$\frac{1}{2}\sum_{i = 1}^{n}\left \| q_{i} - Rp_{i} - t \right \|^{2}$

常用的求解和的方法有：

SVD
非线性优化

非线性优化的描述比较繁琐，下面只介绍SVD方法。为了后面能使用SVD，我们需要在数学上做一点小小的变形。首先定义两组点云的质心（center of mass）为 $\mu _{p} = \frac{1}{n} \sum_{i = 1}^{n}p_{i}$ , $\mu _{q} = \frac{1}{n} \sum_{i = 1}^{n}q_{i}$ ,并作出如下处理：

MATLAB Example

MATLAB R2018a提供了ICP例程，需要安装Computer Vision System Toolbox，我们来简单看一下这个例子。

%Load point cloud data.
ptCloud = pcread('teapot.ply');
pcshow(ptCloud); 
title('Teapot');
%Create a transform object with 30 degree rotation along z -axis and translation [5,5,10].
A = [cos(pi/6) sin(pi/6) 0 0; ...
    -sin(pi/6) cos(pi/6) 0 0; ...
            0         0  1 0; ...
            5         5 10 1];
tform1 = affine3d(A);
%Transform the point cloud.
ptCloudTformed = pctransform(ptCloud,tform1);

pcshow(ptCloudTformed);
title('Transformed Teapot');
%Apply the rigid registration.
tform = pcregistericp(ptCloudTformed,ptCloud,'Extrapolate',true);
%Compare the result with the true transformation.
disp(tform1.T);
tform2 = invert(tform);
disp(tform2.T);