ICP匹配方法

两组三维点集的最小二乘拟合

给定两个三维点集：

{xi}={x1,x2,…,xN}{pi}={p1,p2,…,pN}\{x_i\} = \{x_1, x_2, \dots, x_{N}\} \\ \{p_i\} = \{p_1, p_2, \dots, p_{N}\} {xi}={x1,x2,…,xN}{pi}={p1,p2,…,pN}

点集{xi}\{x_i\}{xi}和点集{pi}\{p_i\}{pi}的集合转换关系如下所示：

pi=Rxi+T+Nip_i = Rx_i + T + N_i pi=Rxi+T+Ni

其中，RRR是一个3×33 \times 33×3的旋转矩阵，TTT是一个三维的平移向量，NiN_iNi是噪声项。

我们可以通过最小化下式，求得旋转矩阵RRR和平移向量TTT。

∑2=∑i=1N∣∣pi−(Rxi+T)∣∣2\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \sum^2 = \sum\limits_{i=1}^N{||p_i - (Rx_i + T)||}^2 ∑2=i=1∑N∣∣pi−(Rxi+T)∣∣2

为了实现最小二乘过程的RRR、TTT变量解耦，做去中心化处理，假设R^\hat{R}R^和T^\hat{T}T^是我们的最小二乘解,，定义以下变量。

xi′=xi−1N∑i=1Nxipi′=pi−1N∑i=1Np1x'_i = x_i - \frac{1}{N}\sum_{i=1}^N x_i \\ p'_i = p_i - \frac{1}{N}\sum_{i=1}^N p_1 \\ xi′=xi−N1i=1∑Nxipi′=pi−N1i=1∑Np1

此时我们有
∑2=∑i=1N∣∣pi′−Rxi′∣∣2\ \ \ \ \\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \sum^2 = \sum\limits_{i=1}^N{||p'_i - Rx'_i||}^2 ∑2=i=1∑N∣∣pi′−Rxi′∣∣2

首先我们可以求得最小二乘解R^\hat{R}R^,然后由下式求得T^\hat{T}T^

T^=p′−R^p\hat{T} = p' - \hat{R}p T^=p′−R^p

∑i=1N∣∣pi′−Rxi′∣∣2=∑i=1N(pi′−Rxi′)t(pi′−Rxi′)=∑i=1N(pi′tpi′+xi′txi′−2pi′tRxi)\sum_{i=1}^N{||p'_i - Rx'_i||}^2 = \sum_{i=1}^N (p'_i - Rx'_i)^t(p'_i - R x'_i) \\ =\sum_{i=1}^N({p'_i}^tp'_i + {x'_i}^tx'_i - 2{p'_i}^tRx_i) i=1∑N∣∣pi′−Rxi′∣∣2=i=1∑N(pi′−Rxi′)t(pi′−Rxi′)=i=1∑N(pi′tpi′+xi′txi′−2pi′tRxi)

所以最小化

F=∑i=1Npi′tRxi=Trace(∑i=1NRxi′pi′t)=Trace(RH)F = \sum_{i=1}^N {p'_i}^tRx_i \\ =Trace(\sum_{i=1}^N Rx'_i{p'_i}^t) = Trace(RH) F=i=1∑Npi′tRxi=Trace(i=1∑NRxi′pi′t)=Trace(RH)

由定理：
对于任意的对称正定矩阵AATAA^TAAT和任意的正交矩阵BBB，都有

Trace(AAT)≥Trace(BAAT)Trace(AA^T) \geq Trace(BAA^T) Trace(AAT)≥Trace(BAAT)

对HHH进行SVDSVDSVD分解得到H=UMVTH = UMV^TH=UMVT，令X=VUTX = VU^TX=VUT，则有，
XH=VMVTXH = VMV^T XH=VMVT

所以，

Trace(XH)≥Trace(BXH)Trace(XH) \geq Trace(BXH) Trace(XH)≥Trace(BXH)
因此，如果XXX矩阵的行列式为1，则T^=X\hat{T} = XT^=X

激光雷达SLAM激光的前端配准算法相关推荐

激光SLAM的前端配准算法
四.激光的前端配准算法 Tracking , 激光slam里为帧间匹配算法帧间匹配算法 ---- map和scan匹配,不一定是前后两帧. 1.ICP匹配方法 2.PL-ICP匹配方法 3.基于优 ...
激光SLAM——第四节（激光SLAM的前端配准方法）
@[目录] ICP变种方法 – 第四章激光的前端配准算法(ICP) 4.1.2.3.4的ICP方法,一个比一个精度高,计算量也大. 点云匹配算法是为了匹配两帧点云数据,从而得到传感器(激光雷达或摄像 ...
激光SLAM从理论到实践学习——第四节（激光SLAM的前端配准方法）
激光的前端配准算法(帧间匹配算法) 前端配准在视觉内叫做Tracking或者帧间匹配,对激光SLAM是有非常大的影响的帧间匹配不一定说的是前后两帧进行匹配,也可以是任意帧之间进匹配是一个Map-& ...
激光SLAM入门笔记(五):前端配准 II
前端配准 II 1.爬山法(拟梯度法) 1.1 示意图 1.2 基本思想 1.3 得分函数定义 1.4 算法流程 1.5 伪代码 2.高斯牛顿优化方法 2.1 示意图 2.2 数学描述 2.3 优化方 ...
3D激光雷达SLAM算法学习01——3D激光SLAM整体框架
#1.3D激光雷达系列主要内容结合个人毕业设计和日后发展方向,开始学习3D激光雷达的相关感知算法,预计SLAM(定位和建图)开始,本篇给出简单的框架和绪论,后边会按照图优化/滤波原理.传感器基础.数 ...
SLAM | 激光SLAM中开源算法对比
点击上方"AI算法修炼营",选择加星标或"置顶" 标题以下,全是干货前面的话好久没有更新SLAM系列的文章了,前面我们讲到了激光SLAM技术.基于激光雷达的 ...
激光雷达点云配准算法
最近做了一些关于激光雷达的实验,并了解了一些雷达点云配准的算法在这里给大家分享一下,也算是记录一下学习的过程,留下一点自己的理解. 1.为什么要点云配准因为雷达采集到的点云信息需要进行数据融合,得到 ...
python点云快速配准_分享一个V-SLAM中点云配准算法改进的方法
近年来,随着自主导航的广泛应用,视觉同时定位与地图构建(Visual simultaneous localization and mapping, V-SLAM)通过自身携带的视觉传感器对周围环境进行 ...
经典激光雷达SLAM系统：LeGO-LOAM
作者 | 密斯特李编辑 | 汽车人原文链接:https://zhuanlan.zhihu.com/p/511968459 点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨 ...

激光雷达SLAM激光的前端配准算法

激光雷达SLAM激光的前端配准算法

ICP匹配方法

两组三维点集的最小二乘拟合

激光雷达SLAM激光的前端配准算法相关推荐

最新文章

热门文章