参考资料：

《VICP: Velocity Updating Iterative Closest Point Algorithm》–Seungpyo Hong Heedong Ko Jinwook Kim
《视觉SLAM十四讲从理论到实践》–高翔

简介

追踪机器人的位姿是移动机器人领域的关键问题，ICP算法通过迭代3D空间中最近的点来求解位姿，而VICP算法在ICP算法的基础上引入了速度的更新,目的是解决运动畸变。

ICP算法

假设 X = { x i } X=\{x_i\} X={xi}是第一帧扫描中所得到的数据点， Y = { y i } Y=\{y_i\} Y={yi}是第二次扫描中所得到的数据点，且两帧中的数据已经进行了配对。那么优化中目标函数可以被表示为
min ⁡ R , t J = 1 2 ∑ i = 1 n ∥ ( p i − ( R p i ′ + t ) ) ∥ 2 2 \min _{R, t} J=\frac{1}{2} \sum_{i=1}^{n}\left\|\left( p _{i}-\left( R p _{i}{ }^{\prime}+ t \right)\right)\right\|_{2}^{2} R,tminJ=21i=1∑n∥(pi−(Rpi′+t))∥22
定义质心
p = 1 n ∑ i = 1 n ( p i ) , p ′ = 1 n ∑ i = 1 n ( p i ′ ) p =\frac{1}{n} \sum_{i=1}^{n}\left( p _{i}\right), \quad p ^{\prime}=\frac{1}{n} \sum_{i=1}^{n}\left( p _{i}^{\prime}\right) p=n1i=1∑n(pi),p′=n1i=1∑n(pi′)
对函数进行一下变形，加一个 ( − p + R p ′ + p − R p ′ ) (-p+R p^{\prime}+p-R p^{\prime}) (−p+Rp′+p−Rp′)这个部分
1 2 ∑ i = 1 n ∥ p i − ( R p i ′ + t ) ∥ 2 = 1 2 ∑ i = 1 n ∥ p i − R p i ′ − t − p + R p ′ + p − R p ′ ∥ 2 = 1 2 ∑ i = 1 n ∥ ( p i − p − R ( p i ′ − p ′ ) ) + ( p − R p ′ − t ) ∥ 2 = 1 2 ∑ i = 1 n ( ∥ p i − p − R ( p i ′ − p ′ ) ∥ 2 + ∥ p − R p ′ − t ∥ 2 + 2 ( p i − p − R ( p i ′ − p ′ ) ) T ( p − R p ′ − t ) ) \begin{aligned} \frac{1}{2} \sum_{i=1}^{n}\left\| p _{i}-\left( R p _{i}^{\prime}+ t \right)\right\|^{2}=& \frac{1}{2} \sum_{i=1}^{n}\left\| p _{i}- R p _{i}{ }^{\prime}- t - p + R p ^{\prime}+ p - R p ^{\prime}\right\|^{2} \\ =& \frac{1}{2} \sum_{i=1}^{n}\left\|\left( p _{i}- p - R \left( p _{i}^{\prime}- p ^{\prime}\right)\right)+\left( p - R p ^{\prime}- t \right)\right\|^{2} \\ =& \frac{1}{2} \sum_{i=1}^{n}\left(\left\| p _{i}- p - R \left( p _{i}^{\prime}- p ^{\prime}\right)\right\|^{2}+\left\| p - R p ^{\prime}- t \right\|^{2}+\right. \left.2\left( p _{i}- p - R \left( p _{i}^{\prime}- p ^{\prime}\right)\right)^{T}\left( p - R p ^{\prime}- t \right)\right) \end{aligned} 21i=1∑n∥pi−(Rpi′+t)∥2===21i=1∑n∥pi−Rpi′−t−p+Rp′+p−Rp′∥221i=1∑n∥(pi−p−R(pi′−p′))+(p−Rp′−t)∥221i=1∑n(∥pi−p−R(pi′−p′)∥2+∥p−Rp′−t∥2+2(pi−p−R(pi′−p′))T(p−Rp′−t))
这样凑得关键目的是 ( p i − p − R ( p i ′ − p ′ ) ) \left( p _{i}- p - R \left( p _{i}{ }^{\prime}- p ^{\prime}\right)\right) (pi−p−R(pi′−p′))在求和后是等于0的,如下
∑ i = 1 n 2 ( p i − p − R ( p i ′ − p ′ ) ) T ( p − R p ′ − t ) ) = 2 ( ∑ i = 1 n p i − n p ⏟ = 0 − R ( ∑ i = 1 n p i − n p ) ⏟ = 0 ) T ( p − R p ′ − t ) = 0 \begin{aligned} & \sum_{i=1}^{n} 2\left(p_{i}-p-R\left(p_{i}^{\prime}-p^{\prime} )\right)^T\left(p-R p^{\prime}-t\right)\right) \\ =&2(\underbrace{\sum_{i=1}^{n} p_{i}-n p}_{=0}-R\underbrace{\left(\sum_{i=1}^{n} p_{i}-n p\right)}_{=0})^T\left(p-R p^{\prime}-t\right)\\ =& 0 \end{aligned} ==i=1∑n2(pi−p−R(pi′−p′))T(p−Rp′−t))2(=0 i=1∑npi−np−R=0 (i=1∑npi−np))T(p−Rp′−t)0
所以，此时优化问题可以变为
min ⁡ R , t J = 1 2 ∑ i = 1 n ∥ p i − p − R ( p i ′ − p ′ ) ∥ 2 + ∥ p − R p ′ − t ∥ 2 \min _{R, t} J=\frac{1}{2} \sum_{i=1}^{n}\left\| p _{i}- p - R \left( p _{i}^{\prime}- p ^{\prime}\right)\right\|^{2}+\left\| p - R p ^{\prime}- t \right\|^{2} R,tminJ=21i=1∑n∥pi−p−R(pi′−p′)∥2+∥p−Rp′−t∥2
观察函数可以发现，两个式子中，前一个式子含有优化变量R，后一个式子中含有R,t，所以我们可以先优化前一个式子，得到R，再令 t = p − R p ′ t=p-Rp^{\prime} t=p−Rp′，使得后一个式子为最小值0。
即，此时优化问题变为
min ⁡ R , t J = 1 2 ∑ i = 1 n ∥ p i − p − R ( p i ′ − p ′ ) ∥ 2 \min _{R, t} J=\frac{1}{2} \sum_{i=1}^{n}\left\| p _{i}- p - R \left( p _{i}^{\prime}- p ^{\prime}\right)\right\|^{2} R,tminJ=21i=1∑n∥pi−p−R(pi′−p′)∥2
为了使描述简单，定义去质心坐标， q i = p i − p , q i ′ = p i ′ − p ′ q _{i}= p _{i}- p , \quad q _{i}^{\prime}= p _{i}^{\prime}- p ^{\prime} qi=pi−p,qi′=pi′−p′，将上式子展开，有
1 2 ∑ i = 1 n ∥ q i − R q i ′ ∥ 2 = 1 2 ∑ i = 1 n q i T q i + q i ′ T R T R q i ′ − 2 q i T R q i ′ \frac{1}{2} \sum_{i=1}^{n}\left\| q _{i}- R q _{i}^{\prime}\right\|^{2}=\frac{1}{2} \sum_{i=1}^{n} q _{i}^{T} q _{i}+ q _{i}^{\prime T} R ^{T} R q _{i}^{\prime}-2 q _{i}^{T} R q _{i}^{\prime} 21i=1∑n∥qi−Rqi′∥2=21i=1∑nqiTqi+qi′TRTRqi′−2qiTRqi′
第一项和优化变量无关，利用旋转矩阵的正交性， R T R = I R^TR=I RTR=I，所以第二项也和优化变量无关，所以，优化问题可以转变为
∑ i = 1 n − q i T R q i ′ = ∑ i = 1 n − tr ⁡ ( R q i ′ q i T ) = − tr ⁡ ( R ∑ i = 1 n q i ′ q i T ) \sum_{i=1}^{n}- q _{i}^{T} R q _{i}^{\prime}=\sum_{i=1}^{n}-\operatorname{tr}\left( R q _{i}^{\prime} q _{i}^{T}\right)=-\operatorname{tr}\left( R \sum_{i=1}^{n} q _{i}^{\prime} q _{i}^{T}\right) i=1∑n−qiTRqi′=i=1∑n−tr(Rqi′qiT)=−tr(Ri=1∑nqi′qiT)
这里用到的小技巧是一个行向量与一个列向量的乘积等于行向量与列向量乘积的迹。
后续证明较复杂，这里给出结论，定义
W = ∑ i = 1 n q i q i ′ T W =\sum_{i=1}^{n} q _{i} q _{i}^{\prime T} W=i=1∑nqiqi′T
现在的问题即为
m i n ( − t r ( R W ) ) = m a x t r ( R W ) min(-tr(RW))=max\ tr(RW) min(−tr(RW))=max tr(RW)
先给出一个定理: 假设矩阵A为正定对称矩阵，则对于任意的正交矩阵B，都有： t r ( A ) ≥ t r ( B A ) tr(A) \ge tr(BA) tr(A)≥tr(BA)
对W做SVD分解以后有
W = U Λ V T X = V U T —–正交矩阵 X W = V U T U Λ V T = V Λ V T ——正定对称 \begin{aligned} &W=U \Lambda V^{T} \quad X=V U^{T} \text { -----正交矩阵 }\\ &X W=V U^{T} U \Lambda V^{T}=V \Lambda V^{T}\text { ------正定对称 } \end{aligned} W=UΛVTX=VUT —–正交矩阵 XW=VUTUΛVT=VΛVT ——正定对称
利用上面的定理： t r ( X W ) ≥ t r ( B X W ) tr(XW) \ge tr(BXW) tr(XW)≥tr(BXW)
tips ：这里的关键在于B是任意的正交矩阵，X也是正交矩阵，所以BX可以是任意正交矩阵。可以从刚体变换的角度理解，任意一次旋转变换都可以拆分为一个固定的旋转变换X和一个特定的旋转变换B。

所以，优化的结果为
R = U V T R=UV^T R=UVT

ICP算法伪代码

VICP

问题提出

由于激光雷达在测距的过程中采取旋转扫描的方式获取距离，所以测量到的每一个点并不是同时采集到的，如果在测量的过程中激光雷达(机器人)发生了移动的话，所测到的数据就会发生扫描失真。如下图所示，一个逆时针旋转的激光雷达在测距时发生了向前运动，那么偏左侧的数据将会测量到更短的距离，从而导致测量失真。

推导过程

将问题形式化，用 X i X^i Xi表示 t i t_i ti 时刻采集到的的数据，同样的 X i − 1 X^{i-1} Xi−1表示 t i − 1 t_{i-1} ti−1时刻采集到的数据，两个数据之间的时间间隔为 Δ t \Delta t Δt，每一次扫描 X i X^i Xi都有自己的变换矩阵 T i T_i Ti。
X i X^i Xi这次扫描的坐标系相对于 X i − 1 X^{i-1} Xi−1的坐标系的位姿变换矩阵可以表示为 T i − 1 − 1 T i T^{-1}_{i-1}T_{i} Ti−1−1Ti，所以可以得到下面这样一个各次扫描之间刚体变换的递推式
x k i − 1 = T i − 1 − 1 T i x k i , k = 1 , ⋯ , n x_{k}^{i-1}=T_{i-1}^{-1} T_{i} x_{k}^{i} \quad, k=1, \cdots, n xki−1=Ti−1−1Tixki,k=1,⋯,n
这里有点没看懂，我一直觉得是这样的 x k i = T i − 1 − 1 T i x k i − 1 , k = 1 , ⋯ , n x_{k}^{i}=T_{i-1}^{-1} T_{i} x_{k}^{i-1} \quad, k=1, \cdots, n xki=Ti−1−1Tixki−1,k=1,⋯,n

两次扫描过程中速度可以用下面这个式子计算：
V i = T i − 1 T ˙ i ≈ 1 Δ t log ⁡ T i − 1 − 1 T i V_{i}=T_{i}^{-1} \dot{T}_{i} \approx \frac{1}{\Delta t} \log T_{i-1}^{-1} T_{i} Vi=Ti−1T˙i≈Δt1logTi−1−1Ti
这个式子用到了李群和李代数之间的转换关系，是时候祭出14讲中的这张图了。对其次矩阵T取对数以后会得到 [ ρ ϕ ] \left[\begin{array}{l}\rho \\ \phi\end{array}\right] [ρϕ]，也就是沿三个轴的平移量和旋转量，除以时间就得到了沿三个轴的速度和角速度的近似。

有了各帧数据之间的速度以后，我们开始处理同样一帧数据之间各个点由于激光雷达运动而产生的畸变。
下图显示了激光雷达的简化模型，假设在一帧激光雷达的数据中有n个点，每两个点之间的时间差定义为 Δ t s = Δ t / n \Delta t_s=\Delta t/n Δts=Δt/n，那么第 j j j个点 ( j = 0 , 1 , 2... , n − 1 ) (j=0,1,2...,n-1) (j=0,1,2...,n−1)的位姿为
T ( t i + j Δ t s ) = T i e j Δ t s V i T(t_i+j\Delta t_s)=T_ie^{j\Delta t_sV_i} T(ti+jΔts)=TiejΔtsVi
这里实际上是假设在在激光雷达在打每一个点的过程中是匀速运动的，这个速度用两帧数据之间的速度， j Δ t s V i j\Delta t_s V_i jΔtsVi就是打第 j j j个点时距离第一个点激光雷达所移动的距离和角度，然后用一个指数映射将齐转换为位姿变换矩阵。之所以将齐乘在 T i T_i Ti右侧是因为这个变换是相对于连体坐标系(第一个点的位姿)的变换，运用口诀：右乘连体左乘基。

采用这种方法对数据进行修正，每个点的修正后坐标为
x ˉ ⋆ = { e j Δ V i P j ∣ j = 0 , . . . , n − 1 } \bar x^{\star}=\{e^{j\Delta Vi}P_j |j=0,...,n-1\} xˉ⋆={ejΔViPj∣j=0,...,n−1}
就是每个点的坐标在匀速运动的假设下每个点补偿一个自己时间段内的刚体变换。

Tips

文章中的V都是广义v，实际上是由沿三个轴的速度和沿三个轴的角速度所组成的6维向量，也就是 se ⁡ ( 3 ) \operatorname{se}(3) se(3)的导数。

这里的 j j j是代表第 j j j个点，不是虚数单位，第一次看的时候自动脑补了欧拉公式 e j θ e^{j\theta} ejθ，看半天没看懂。

向前修正与向后修正

上面的式子是基于向前修正，就是把后续测量到的点做一个补偿修正，另外一个方法是做一个后向修正，就是把先采集到的数据做一个预先补偿，好处是可以减少延时?(没看懂怎样减少)。

向后修正的补偿公式为
X ˉ i = { e ( n − j ) Δ t s ( − V i ) x j ∣ j = 0 , … , n } \bar{X}^{i}=\left\{e^{(n-j) \Delta t_{s}\left(-V_{i}\right)} x_{j} \mid j=0, \ldots, n\right\} Xˉi={e(n−j)Δts(−Vi)xj∣j=0,…,n}

VICP算法伪代码

Tips

两帧数据之间的位姿变换矩阵调用ICP算法计算，每一帧数据的补偿中用到的速度实际是调用上帧和上上帧数据计算得到的。

用for循环迭代计算位姿变换矩阵，可以减少计算量。

SLAM--VICP(Velocity Updating Iterative Closest Point Algorithm)学习笔记相关推荐

Shor’s Algorithm 学习笔记
Shor'sAlgorithm 以下是我在学习Quantum Algorithm时整理的演示PPT,是我对这个算法的一些个人理解希望可以帮到你.
Grover’s Algorithm 学习笔记
Grover's Algorithm 以下是我在学习Quantum Algorithm时整理的演示PPT,是我对这个算法的一些个人理解希望可以帮到你.
Algorithm学习笔记 --- 迷宫问题
版权声明:学习交流为主,未经博主同意禁止转载,禁止用于商用. https://blog.csdn.net/u012965373/article/details/26376987 </div> ...
迭代最近点算法 Iterative Closest Points
研究生课程系列文章参见索引<在信科的那些课> 基本原理假定已给两个数据集P.Q, ,给出两个点集的空间变换f使他们能进行空间匹配.这里的问题是,f为一未知函数,而且两点集中的点数不一定相 ...
迭代最近点（Iterative Closest Point, ICP）算法
1. 定义 ICP(Iterative Closest Point,迭代最近点)算法是一种迭代计算方法,主要用于计算机视觉中深度图像的精确拼合,通过不断迭代最小化源数据与目标数据对应点来实现精确地拼合 ...
Iterative Shrinkage Thresholding Algorithm
Iterative Shrinkage Thresholding Algorithm(ISTA) ISTA 对于一个基本的线性逆问题: y=Ax+w(1){y}={A} {x}+{w}\tag{1} ...
SLAM学习笔记（二十）LIO-SAM流程及代码详解（最全）
写在前面关于安装配置,博客LIO_SAM实测运行,论文学习及代码注释[附对应google driver数据] 我觉得已经写的比较完善了.但是我觉得在注释方面,这位博主写的还不够完善,因此在学习以后, ...
视觉SLAM学习笔记
中英文对照表中文英文计算机视觉 Computer Vision 人工智能 Artificial Intelligence 单目相机 Monocular 双目相机 Stereo 深度相机 RGB- ...
（学习笔记）激光SLAM的发展和应用趋势
目录一.SLAM的定义(Simultaneous Localization And Mapping) 二.SLAM的框架三.激光SLAM 1.传感器: 2.地图类型: 3.帧间匹配算法: 4.回环 ...

SLAM--VICP(Velocity Updating Iterative Closest Point Algorithm)学习笔记

参考资料：

简介

ICP算法

ICP算法伪代码

VICP

问题提出

推导过程

Tips

向前修正与向后修正

VICP算法伪代码

Tips

SLAM--VICP(Velocity Updating Iterative Closest Point Algorithm)学习笔记相关推荐

最新文章

热门文章