点云配准icp算法推导

一、引言
ICP算法可以用于匹配两个点云，得到两个点云之间的位姿转换关系。本文主要介绍已知对应点的ICP匹配算法。

二、ICP算法
首先计算两个点云的质心：

然后将原始点云减去对应的质心，得到去除质心的点云：

计算新产生的两个点云形成的W矩阵，并SVD分解：

那么，两个原始点云的转换关系R和t如下所示：

三、推导过程
对于两组点云：

求解R和t，使得下式最小：

上式进行变换：

其中，цx 和 цp分别为X点云和P点云的质心。
转换为去除质心的形式：

展开得到

由于后面两项都有

我们可以令该项等于0，然后式子转化为

该式子仅与R有关，可以寻找可以最小化上式的R，然后根据=0求解t。

上式进一步转化，得到：

其中，X’ 和 P‘ 是原始点云减去对应质心的点点云，x’和p’是其中的点。

展开得到：

其中旋转矩阵R因为是正交矩阵，所以RTR=1，因此求上式的最小值等价于求最后一项的最小值：

即该式的最大值：

进行转化，用矩阵的迹进行表示：

这里可以用迹表示的原因是3X1矩阵乘以3X3矩阵，再乘以1X3矩阵，得到的是一个常量。常量迹即等于自身

其中，

对H进行SVD分解得到：

设定一个变量X为

得到：

此时得到的XH是一个正定对称矩阵

根据定理：

假设矩阵A为正定对称矩阵，则对于任意的正交矩阵B，都有

类比到我们的推导这里，我们准备求解可以让Trace(RH) 最大的R值，

我们发现，当R=VUT的时候，RH的结果是一个正定对称矩阵

而对于任意一个正定对称矩阵来说，根据定理可以知道，我们再乘以任何一个正交矩阵（旋转矩阵是正交矩阵），都会导致Trace值下降

所以说明，此时的R=VUT取值可以让Trace(RH)取得最大值。

因此：

参考：https://blog.csdn.net/weixin_35695879/article/details/103125575

点云配准icp算法推导相关推荐

一分钟详解点云配准ICP方法
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者:丁洪凯链接:https://zhuanlan.zhihu.com/p/107218828 本文 ...
python 点云配准_点云配准(Registration)算法——以PCL为例
本文为PCL官方教程的Registration模块的中文简介版. An Overview of Pairwise Registration 点云配准包括以下步骤: from a set of poin ...
PCL之点云配准--ICP
代码展示: #include <string> #include <iostream> #include <pcl/io/ply_io.h> #include &l ...
激光雷达点云配准算法
最近做了一些关于激光雷达的实验,并了解了一些雷达点云配准的算法在这里给大家分享一下,也算是记录一下学习的过程,留下一点自己的理解. 1.为什么要点云配准因为雷达采集到的点云信息需要进行数据融合,得到 ...
ICP算法实现（C++）
用C++实现的ICP(Iterative Closest Point,迭代最近点)算法,借助了PCL库实现点云基本变换.KD-tree以及可视化部分,核心迭代部分没有调用PCL的api.代码在KD-t ...
点云配准2：icp算法在PCL1.10.0上的实现+源码解析
目录本文最后实现的配准实例点云配准系列准备程序结构主程序 1.为什么要降采样 2.体素降采样原理 3.点云更新 icp 配准前的参数设置 icp配准算法内部对应点对确定(determine ...
多视图点云配准算法综述
作者:杨佳琪,张世坤,范世超等转载自:华中科技大学学报(自然科学版) 编辑:东岸因为@一点人工一点智能原文:多视图点云配准算法综述摘要:以多视图点云配准为研究对象,对近二十余年的多视图点 ...
点云配准5：4pcs算法在pcl上的实现
目录配准结果点云配准系列准备完整项目文件参数设定及说明数据参数代码结果 Bunny hippo 算法缺点参考及感谢完配准结果偶尔效果比较好,白色是目标点云0°的Bunny,紫 ...
点云配准（二）— python open3d ICP方法
上一节中介绍了点云配准的基础知识.本节将采用python open3d来进行点云配准. open3d安装和点云配准介绍,请参考: Open3d读写ply点云文件_Coding的叶子的博客-CSDN博客 ...
python点云快速配准_分享一个V-SLAM中点云配准算法改进的方法
近年来,随着自主导航的广泛应用,视觉同时定位与地图构建(Visual simultaneous localization and mapping, V-SLAM)通过自身携带的视觉传感器对周围环境进行 ...

点云配准icp算法推导

点云配准icp算法推导相关推荐

最新文章

热门文章