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

2024-05-15 04:15:27

问题引入

迭代最近点（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 \}$ ,注意这里的 $P$ 和 $Q$ 的坐标分别对应移动前和移动后的坐标系（即坐标原点始终为相机光心，这里我们有移动前、移动后两个坐标系），并且我们通过相关算法筛选和调整了点云存储的顺序，使得 $P$ 和 $Q$ 中的点一一对应，如 $\left (P _{99},Q _{99} \right )$ 在三维空间中对应同一个点。

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

$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}$

常用的求解 $R$ 和 $t$ 的方法有：

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，我们来简单看一下这个例子。

首先，加载了原始的点云图

对点云进行旋转30度、向xyz分别平移5、5、10后得到变换后的点云图

由于每个点在三位矩阵中的下标并没有发生变化，只是每个点的坐标发生了变化，满足我们之前所要求的点云已经配准前提。调用ICP函数

tform = pcregistericp(ptCloudTformed,ptCloud,'Extrapolate',true);

输出结果

ICP算法详解——我见过最清晰的解释相关推荐

PnP算法详解（超详细公式推导）
PnP算法详解 PnP概述 PnP数学模型 PnP求解方法 DLT直接线性变换法 EPnP EPnP的特点步骤理论推倒 1.控制点及齐次重心坐标系 2.控制点的选择 3.计算控制点在相机坐标系下的 ...
回溯算法详解之全排列、N皇后问题
回溯算法详解回溯算法框架.解决一个回溯问题,实际上就是一个决策树的遍历过程.你只需要思考 3 个问题: 1.路径:也就是已经做出的选择. 2.选择列表:也就是你当前可以做的选择. 3.结束条件:也就 ...
md5与des算法有何不同_Python算法详解：为什么说算法是程序的灵魂？
算法是程序的灵魂,只有掌握了算法,才能轻松地驾驭程序开发.软件开发工作不是按部就班,而是选择一种最合理的算法去实现项目功能.算法能够引导开发者在面对一个项目功能时用什么思路去实现,有了这个思路后,编程 ...
20200118：（leetcode）最长回文子串（中心扩展算法详解及思考）
最长回文子串(中心扩展算法详解及思考) 题目中心扩展算法详解代码实现题目给定一个字符串 s,找到 s 中最长的回文子串.你可以假设 s 的最大长度为 1000. 示例 1: 输入: " ...
LDA主题模型（算法详解）
LDA主题模型(算法详解) http://blog.csdn.net/weixin_41090915/article/details/79058768?%3E 一.LDA主题模型简介 LDA(Late ...
kmeans聚类算法matlab_KMeans聚类算法详解
"如果把人工智能比作一块大蛋糕,监督学习只是上面的一层奶油". 日常生活中,从人脸识别.语音识别到搜索引擎,我们看到越来越多人工智能领域的算法逐渐走向落地.尽管全球每日新增数据量以 ...
java制作海报一：java使用Graphics2D 在图片上写字，文字换行算法详解
文章目录前言一.直接上代码 1. 写字方法 2. 换行算法二. 叙述换行算法前言代码都上传到GitHub了,这里仅仅是贴出来主要部分,GitHub传送门:https://github.com ...
图解机器学习算法(13) | 聚类算法详解（机器学习通关指南·完结）
作者:韩信子@ShowMeAI 教程地址:https://www.showmeai.tech/tutorials/34 本文地址:https://www.showmeai.tech/article-d ...
离线强化学习(Offline RL)系列3: (算法篇) Onestep 算法详解与实现
[更新记录] 论文信息: David Brandfonbrener, William F. Whitney, Rajesh Ranganath, Joan Bruna: "Offline R ...
【强化学习】Sarsa算法详解以及用于二维空间探索【Python实现】
Sarsa算法 Sarsa算法,是基于Q-Learning算法.改动其实很小. 本文工作基于之前的Q-Learning的项目,如果有疑问可以看下面两个问题: [强化学习]Q-Learning算法详解以 ...

最新文章

热门文章