ICP_SVD的公式推导

最新推荐文章于 2023-05-22 17:40:57 发布
最新推荐文章于 2023-05-22 17:40:57 发布
阅读量395 收藏 2
点赞数 1
文章标签： slam
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/xiaomaotian/article/details/121285470
版权

ICP_SVD

设目标点云的点集为 : X = {x1,x2......,xn}

设当前点云的点集为 : Y = {y1,y2......,yn}

所构建的目标方程为:
minE(R,t) = min\frac{1}{N} \sum_{i=1}^{N}{||x_i - R_{yi}y_i-t||^2}

对后面的方程进行化简

首先减去上 X Y 的平均值 为了等式相等所以也得加上


E(R,t) = \frac{1}{N}\sum_{i=1}^{N}{||x_i - Ry_i - t- u_x - Ru_y + u_x + Ru_y ||^2}

将 R t隔离


= \frac{1}{N}\sum_{i=1}^{N}{||x_i - u_x - R(y_i - u_y) + (u_x - Ru_y - t) ||^2}

将二范数展开


= \frac{1}{N}\sum_{i=1}^{N}{(||x_i - u_x - R(y_i - u_y) ||^2 + ||(u_x - Ru_y - t) ||^2 + 2 * (x_i - u_x - R(y_i - u_y))^T *(u_x - Ru_y - t))}


\frac{1}{N}\sum_{i=1}^{N} (x_i - u_x - R(y_i - u_y))^T *(u_x - Ru_y - t)

上面的式子为0

所以
= \frac{1}{N}\sum_{i=1}^{N}{(||x_i - u_x - R(y_i - u_y) ||^2 + ||(u_x - Ru_y - t) ||^2 )}

令只与旋转相关的式子
E_1(R,t) = \frac{1}{N}\sum_{i=1}^{N}{||x_i - u_x - R(y_i - u_y) ||^2 }

令只与平移相关的式子
E_2(R,t)= \frac{1}{N}\sum_{i=1}^{N}{||u_x - Ru_y - t||^2 }

那么，对于任意的R，都可以找到一个t 使得上式子等于 0

所以可以先求旋转在求平移

对于求解旋转有如下：
E_1(R,t) = \frac{1}{N}\sum_{i=1}^{N}{||x_i - u_x - R(y_i - u_y) ||^2 }
= \frac{1}{N}\sum_{i=1}^{N}{||x'_i - Ry'_i ||^2 }

二范数展开
= \frac{1}{N}\sum_{i=1}^{N}{(x'{_i}^{T}x'{_i} + y'{_i}^{T}R^TRy'_i - 2x'{_i}^{T}Ry'_i)}

由于旋转矩阵的正交性质 所以上式前两项和 R 无关 所以求取下式的最大值
= \frac{1}{N}\sum_{i=1}^{N}{x'{_i}^{T}Ry'_i}

展开之后就需要用到 迹 由于常数的 迹 等于本身
= \sum_{i=1}^{N}{x'{_i}^{T}Ry'_i} = \sum_{i=1}^{N}Trace(Ry'_ix'{_i}^{T}) = Trace(RH)

H为确定的，问题转换为找到和是的R使得Trace(RH)为最大值

定理: 若有正定矩阵 AAT ，则对于任意正交矩阵 R 有 Trace(AAT) >= Trace(BAAT)

意义，如果能找到R 是的 Trace(RH) 为 Trace(AAT)形式，则R就是我们要找的矩阵 (这里可以理解 后面在对这个R进行变换相当于 做成一些变换矩阵 依旧没有当前的值大)

证明 :
tr(BAA^T) = tr(A^TBA) = \sum_{i} a_{i}^{T}(Ba_i)

R = VU^T

ai为 A的列向量 根据柯西不等式，有
a_{i}^{T}(Ba_i) \leq \sqrt{((a_{i}^{T}a_i)(a_{i}^{T}B^TBa_i))} = a_{i}^{T}a_i

因此


tr(BAA^T) <= tr(AA^T)

当前目的: 找到 R ，把Trace(RH)转换为 Trace(AAT)

方法：

对H进行SVD分解


H = U\sigma V

取 R 为
R = VU^T

那么R为我们需要的值

xiaomaotian
关注
  • 1
    点赞
  • 踩
  • 2
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
svd.zip_SVD_svd C语言_svd c_svd实现
09-24
svd的简单实现。用于学习。用c语言实现的。
欧拉角，旋转向量，四元数与旋转矩阵
weixin_43955293的博客
08-30 2853
1.坐标系介绍与旋转角 1.坐标系分为左手坐标系和右手坐标系，一般来说，右手坐标系更加常用一些，区别在于： 四指从X轴弯曲到Y轴，若右手大拇指指向Z轴方向，则为右手坐标系。若左手大拇指指向Z轴方向，则为左手坐标系。 2.在三维旋转中，物体的旋转次序也是十分重要的，例如，（a,b,c）在不同的旋转顺序下会有不同 的结果。 欧拉角的旋转顺序有很多，例如：Z-X-Y,X-Y-Z,X-Y-X,Z-X-Y。 且任意旋转可以分解成以下三个轴上的转角： 绕物体的Z轴旋转，得到偏航角yaw;绕旋转之后的Y轴旋转，得到
三维重建笔记_相机标定(相机矩阵求解)基本概念汇总
惊鸿一博
12-25 9878
目录 1. 数学知识 1.1. 单应性矩阵 1.2. SVD(奇异值分解) 1.3.矩阵范数 ||A|| 2. 相机知识 2.1.相机模型 2.2. 相机矩阵 2.3 相机标定算法--同时标定内参外参 3. 对极几何 3.1多视几何 基本矩阵 F（Fundamental matrix） 本质矩阵 E (Essential matrix) 单位矩阵 I (Ident...
融合定位学习--ICP-SVD实现
weixin_44081378的博客
05-22 221
点云滤波中，frame_filter设置成1.3，体素滤波器是一种下采样的滤波器，采用体素格中接近中心点的点替代体素内的所有点云，调大可以降低里程计耗时，但是会随时一定的精度，调小则相反。修改 /src/lidar_localization/config/front_end/config.yaml 中的data_path。函数功能是进行点云的配准，通过nearestKSearch方法找到最邻近关键点，这里只用找一个点，第二项参数设置成1即可。indices 和 distances分别存储目标点索引和距离。
手写ICP-SVD
dushib的博客
02-10 561
ICP简介 Iterative Closest Point迭代最近点算法； 分别在待匹配的目标点云P和源点云Q中，按照一定的约束条件，找到最邻近点（pi，qi），然后计算出最优匹配参数R和t，使得误差函数最小。误差函数为E(R,t)为 其中R和t分别为两帧点云的旋转与平移矩阵；通过不断的迭代求解出两帧点云相对平移与旋转 流程如下： ICP求解过程 1、构造误差函数 2、化简 将误差函数拆解成两部分，一部分是只与旋转有关，另一部分与旋转和平移有关，所以可以先求旋转再求平移 .
已知旋转矩阵求角度_旋转群SO(3)
weixin_40002692的博客
12-04 707
第一小节： 的定义在三维 空间里，定义一组旋转群组 ，表示能在合成算子的作用下，能围绕原点旋转。旋转能保持被旋转向量的长度和相对矢量方向，同时是线性的。在机器人学中，这种旋转十分重要，它能标志刚体在3D空间中的旋转：刚体精确地要求在运动时保持刚体内部的距离、角度和相对方向，否则，如果不能保持的话，就不能能称为刚体。现在，我...
SVD.rar_SVD_SVD 气象_fortran svd_svd fortran
09-24
用于气象行业的奇异值分析程序，用fortran语言编写
svd.rar_SVD_interesting_svd pdf
09-24
It is about SVD and interesting article for mathematical solution
SVD.rar_SVD_SVD的MATLAB程序_svd matlab
09-19
搜集的SVD的一些小程序。matlab编写。
ICP（Iterative Closest Point）算法实现源码
07-14
通过C++实现了ICP算法的点云匹配过程，内容包括KdTree搜索算法和SVD算法的实现源码，希望给大家带来参考。
4. 机器人正运动学---理解变换矩阵
hitgavin的博客
11-24 1万+
这篇文章主要介绍了旋转矩阵的三个性质以及我们应该如何理解它。运用旋转矩阵的性质可以帮助我们理解一些姿态变换的算法。
旋转变换（一）旋转矩阵
热门推荐
Frank的专栏
03-27 34万+
本文主要介绍了计算机图形学中的旋转的概念和矩阵的描述方式，包括二维和三维旋转矩阵的推导过程
罗德里格斯（Rodrigues）旋转向量转旋转矩阵
Du_Shuang的博客
11-06 6536
转载于：https://blog.csdn.net/qq_22235957/article/details/80461290 旋转向量中V的单位向量代表方向，模代表角度，通过罗德里格斯方程可以将旋转向量转换成旋转矩阵。 罗德里格斯旋转方程是从角度和向量计算出相应的旋转矩阵，这个旋转方程在很多方面有重要的应用，这里简要概述一下方程的推导过程。 主要参考资料是维基百科，其实基本上就是翻译一下，自己走一遍这个推导过程，这里把链接贴出来。 维基百科-罗德里格斯方程 推导过程
旋转矩阵
jinking01的专栏
05-15 2458
旋转坐标轴还是旋转坐标？有什么区别？相信很多人都有过类似的问题，这里我来简单尝试着理解一下。 旋转矩阵的相关知识请查看维基百科旋转矩阵 需要注意的是 Roll Pitch Yaw 的定义： 这里的旋转矩阵是需要左乘的，而且以逆时针为正。 关于左乘和右乘: R是一个旋转矩阵，X是一个三维列向量[x,y,z]'。 RX就是把X旋转。 (RX)'=X'*R',看R'就是一个右乘的旋转...
机器学习(29)之奇异值分解SVD原理与应用详解
机器学习算法与Python学习
12-01 1万+
微信公众号 关键字全网搜索最新排名 【机器学习算法】：排名第一 【机器学习】：排名第一 【Python】：排名第三 【算法】：排名第四 前言 奇异值分解(Singular Value Decomposition，简称SVD)是在机器学习领域广泛应用的算法，它不光可以用于降维算法中的特征分解，还可以用于推荐系统，以及自然语言处理等领域，是很多机器学习算法的基石。本文就对SVD的原理做一
SVD 实例计算与理解
THE XING
12-27 3366
网上各种思想解释SVD的太多了。这篇博文想从一个实际的矩阵出发，阐述SVD究竟干了什么。 1.初始理论部分 SVD的基本公式： A=Um×mΣm×nVn×nT(1)A=U_{m\times m} \Sigma_{m\times n} V_{n\times n}^T \tag{1}A=Um×m​Σm×n​Vn×nT​(1) 注意Um×mU_{m\times m}Um×m​, Vn×nV_{n\t...
算法学习报告-01 SVD（奇异值分解）
weixin_42035443的博客
04-25 394
算法学习报告 SVD（奇异值分解）
二-旋转矩阵
one_2_one的博客
08-27 1394
空间中三维坐标旋转一般有三种方式：旋转矩阵、欧拉角和四元数 为什么BVH文件需要用欧拉角表示，因为欧拉角只用3个角度就可以表示，而旋转矩阵需要用一个包含九个元素的矩阵，浪费空间，当需要变成3D位置坐标时候，需要简单的转换就可以将欧拉角变成旋转矩阵。注意：旋转矩阵是通过欧拉角计算得到的。 先介绍旋转矩阵： 旋转矩阵的目的：旋转矩阵反映了一个坐标系中的坐标在另一个坐标系中表示的转换关系（A坐标系...
cv2.DECOMP_SVD
最新发布
09-14
cv2.DECOMP_SVD 是 OpenCV 中的一个解压缩标志，用于指定在使用 SVD（奇异值分解）方法进行矩阵分解时的特定选项。SVD 是一种常用的矩阵分解方法，可以将矩阵分解为三个矩阵的乘积：U、S 和 V^T（转置后的 V）。其中，U 和 V 是正交矩阵，S 是对角矩阵。 cv2.DECOMP_SVD 可以与函数 cv2.SVDecomp() 一起使用，用于指定 SVD 分解的方式。在这个特定标志下，函数将尝试计算完整的 SVD 分解，并且返回所有三个矩阵 U、S 和 V^T。 这是一个示例代码片段，展示了如何使用 cv2.DECOMP_SVD 进行矩阵分解： ```python import cv2 import numpy as np # 生成一个矩阵 A = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]], dtype=np.float32) # 进行 SVD 分解 U, S, V = cv2.SVDecomp(A, flags=cv2.DECOMP_SVD) # 打印分解结果 print("U:", U) print("S:", S) print("V^T:", V) ``` 注意：cv2.SVDecomp() 函数会返回 U、S 和 V^T，而不是 U、S 和 V。这是因为 OpenCV 中的 SVD 函数返回转置后的 V 矩阵。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值