坐标与坐标系

这里所考虑的3维坐标系由原点、3个相互垂直、尺度一致的坐标轴的单位向量确定。
存在唯一不变的世界坐标系(oxw,oyw,ozw)、(exw,eyw,ezw)(ox_w,oy_w,oz_w)、(ex_w,ey_w,ez_w)(oxw,oyw,ozw)、(exw,eyw,ezw)。点的位置由世界坐标系下的坐标(xw,yw,zw)(x_w,y_w,z_w)(xw,yw,zw)确定，位置为(xw∗exw,yw∗eyw,zw∗ezw)(x_w*ex_w,y_w*ey_w,z_w*ez_w)(xw∗exw,yw∗eyw,zw∗ezw)。
其他坐标系由世界坐标系进行平移、旋转得到。
平移容易理解，即(oxl−oxw,oyl−oyw,ozl−ozw)(ox_l-ox_w,oy_l-oy_w,oz_l-oz_w)(oxl−oxw,oyl−oyw,ozl−ozw)。
旋转的表达就比较多了。

点的旋转

vvv绕着一个方向uuu旋转θ\thetaθ角度，变成了vθv_{\theta}vθ。
vθ=vcos⁡θ+(u×v)sin⁡θ+u(u⋅v)(1−cos⁡θ)\boldsymbol{v}_{\theta}=\boldsymbol{v} \cos \theta+(\boldsymbol{u} \times \boldsymbol{v}) \sin \theta+\boldsymbol{u}(\boldsymbol{u} \cdot \boldsymbol{v})(1-\cos \theta)vθ=vcosθ+(u×v)sinθ+u(u⋅v)(1−cosθ)

有没有一个坐标系oxyzioxyz_ioxyzi，旋转后的点坐标就是vvv？是存在这样的坐标系的。这样就存在点旋转与坐标系旋转（当前坐标系旋转到oxyzroxyz_roxyzr）的“对偶”分析。

寻找与点旋转“对偶”的坐标系旋转

exw、eyw、ezwex_w、ey_w、ez_wexw、eyw、ezw旋转后的向量为exr、eyr、ezrex_r、ey_r、ez_rexr、eyr、ezr，则在以exr、eyr、ezrex_r、ey_r、ez_rexr、eyr、ezr为基向量的坐标系中，旋转后的点坐标是vvv。
两组坐标基向量存在如下关系，RRR的旋转矩阵：
(exr,eyr，ezr)=(exw,eyw，ezw)R(ex_r,ey_r，ez_r)=(ex_w,ey_w，ez_w)R(exr,eyr，ezr)=(exw,eyw，ezw)R
根据上式，基向量的特征，易证RRR是正交矩阵，行列式为+1。
记一个点在两个坐标系下的坐标分别是(xw,yw,zw)T、(xr,yr,zr)T(x_w,y_w,z_w)^T、(x_r,y_r,z_r)^T(xw,yw,zw)T、(xr,yr,zr)T，则
(exw,eyw，ezw)∗(xw,yw,zw)T=(exr,eyr，ezr)∗(xr,yr,zr)T=(exw,eyw，ezw)∗R(xr,yr,zr)T(ex_w,ey_w，ez_w)*(x_w,y_w,z_w)^T=(ex_r,ey_r，ez_r)*(x_r,y_r,z_r)^T\\ =(ex_w,ey_w，ez_w)*R(x_r,y_r,z_r)^T(exw,eyw，ezw)∗(xw,yw,zw)T=(exr,eyr，ezr)∗(xr,yr,zr)T=(exw,eyw，ezw)∗R(xr,yr,zr)T
所以，
(xw,yw,zw)T=R(xr,yr,zr)T(x_w,y_w,z_w)^T=R(x_r,y_r,z_r)^T(xw,yw,zw)T=R(xr,yr,zr)T
vθ=Rvv_\theta=Rvvθ=Rv

旋转矩阵的欧拉角表示

用 z-y’-x’'的欧拉角表示，其中三个旋转角度也叫作yaw（航向角），pitch（俯仰角）和roll（横滚角）。

绕z轴旋转，使得y'轴在另一个坐标的zy平面;
绕y'轴旋转，使得z''轴在另一个坐标的zy平面。且x''与另一个坐标系的x轴重合;
绕x''轴旋转，使得y''、z''与另一个坐标系重合

R(ψ,θ,ϕ)=Rz(ψ)Ry(θ)Rx(ϕ)\mathbf{R}(\psi, \theta, \phi)=\mathbf{R}_{z}(\psi) \mathbf{R}_{y}(\theta) \mathbf{R}_{x}(\phi)R(ψ,θ,ϕ)=Rz(ψ)Ry(θ)Rx(ϕ)

点旋转与旋转矩阵的转换

点旋转的表达是(u,θ)(u,\theta)(u,θ)，旋转矩阵的表达是RRR，表征了两个坐标系的转换。
根据点旋转的描述，来构造RRR：
需要一个中间的坐标系oxyz′oxyz'oxyz′来过渡，z′z'z′与旋转轴重合。这样，就是绕z'轴旋转θ\thetaθ，坐标变换关系易得；
oxyzoxyzoxyz和oxyz′oxyz'oxyz′的左边变化关系，也易得。

根据RRR来构造(u,θ)(u,\theta)(u,θ)：o-x-x'、o-y-y'、o-z-z'的中垂面的交线就是旋转轴。旋转角度易得。

旋转的李代数表示

知识1：点旋转，也叫罗戈里格斯旋转
知识2：矩阵的指数运算
exp⁡(ϕ∧)=∑n=0∞1n!(ϕ∧)n\exp(\mathbf{\phi}^\wedge) = \sum\limits_{n = 0}^\infty {\frac{1}{{n!}}{ (\mathbf{\phi}^{\wedge})^n}}exp(ϕ∧)=n=0∑∞n!1(ϕ∧)n
知识3：反对称矩阵的性质
ϕϕT=ϕ∧ϕ∧+∥ϕ∥2I3×3ϕ∧ϕ∧ϕ∧=−ϕ∧ϕ×v=ϕ∧v\mathbf{\phi} \mathbf{\phi}^T = \mathbf{\phi}^{\wedge} \mathbf{\phi}^{\wedge} + \| \mathbf{\phi} \|^2 \mathbf{I}_{3 \times 3}\\ \mathbf{\phi}^{\wedge} \mathbf{\phi}^{\wedge} \mathbf{\phi}^{\wedge} = - \mathbf{\phi}^{\wedge}\\ \mathbf{\phi}×v = \mathbf{\phi}^{\wedge}vϕϕT=ϕ∧ϕ∧+∥ϕ∥2I3×3ϕ∧ϕ∧ϕ∧=−ϕ∧ϕ×v=ϕ∧v

点旋转(u,θ)(u, \theta)(u,θ)
exp⁡(ϕ∧)=exp⁡(θu∧)=∑n=0∞1n!(θu∧)n=I+θu∧+12!θ2u∧u∧+13!θ3u∧u∧u∧+14!θ4(u∧)4+...=uuT−u∧u∧+θu∧+12!θu∧u∧−13!θ3u∧+14!θ4(u∧)4+...=uuT+(θ−13!θ3+15!θ5−...)u∧−(1−12!θ2+14!θ4−...)u∧u∧=u∧u∧+I+sin⁡θu∧−cos⁡θu∧u∧=(1−cos⁡θ)u∧u∧+I+sin⁡θu∧=cos⁡θI+(1−cos⁡θ)uuT+sin⁡θu∧\begin{align*} \exp \left( {{\mathbf{\phi} ^ \wedge }} \right) &= \exp \left( {\theta {\mathbf{u}^ \wedge }} \right) = \sum\limits_{n = 0}^\infty {\frac{1}{{n!}}{{\left( {\theta {\mathbf{u}^ \wedge }} \right)}^n}} \\ &= \mathbf{I} + \theta {\mathbf{u}^ \wedge } + \frac{1}{{2!}}{\theta ^2}{\mathbf{u}^ \wedge }{\mathbf{u}^ \wedge } + \frac{1}{{3!}}{\theta ^3}{\mathbf{u}^ \wedge }{\mathbf{u}^ \wedge }{\mathbf{u}^ \wedge } + \frac{1}{{4!}}{\theta ^4}{\left( {{\mathbf{u}^ \wedge }} \right)^4} + ...\\ &= \mathbf{u} {\mathbf{u}^T} - {\mathbf{u}^ \wedge }{\mathbf{u}^ \wedge } + \theta {\mathbf{u}^ \wedge } + \frac{1}{{2!}}\theta {\mathbf{u}^ \wedge }{\mathbf{u}^ \wedge } - \frac{1}{{3!}}{\theta ^3}{\mathbf{u}^ \wedge } + \frac{1}{{4!}}{\theta ^4}{\left( {{\mathbf{u}^ \wedge }} \right)^4} + ...\\ &= \mathbf{u}{\mathbf{u}^T} + \left( {\theta - \frac{1}{{3!}}{\theta ^3} + \frac{1}{{5!}}{\theta ^5} - ...} \right){\mathbf{u}^ \wedge } - \left( {1 - \frac{1}{{2!}}{\theta ^2} + \frac{1}{{4!}}{\theta ^4} - ...} \right){\mathbf{u}^ \wedge }{\mathbf{u}^ \wedge }\\ &= {\mathbf{u}^ \wedge }{\mathbf{u}^ \wedge } + \mathbf{I} + \sin \theta {\mathbf{u}^ \wedge } - \cos \theta {\mathbf{u}^ \wedge }{\mathbf{u}^ \wedge }\\ &= (1 - \cos \theta ){\mathbf{u}^ \wedge }{\mathbf{u}^ \wedge } + I + \sin \theta {\mathbf{u}^ \wedge }\\ &= \cos \theta \mathbf{I} + (1 - \cos \theta )\mathbf{u}{\mathbf{u}^T} + \sin \theta {\mathbf{u}^ \wedge } \\ \end{align*}exp(ϕ∧)=exp(θu∧)=n=0∑∞n!1(θu∧)n=I+θu∧+2!1θ2u∧u∧+3!1θ3u∧u∧u∧+4!1θ4(u∧)4+...=uuT−u∧u∧+θu∧+2!1θu∧u∧−3!1θ3u∧+4!1θ4(u∧)4+...=uuT+(θ−3!1θ3+5!1θ5−...)u∧−(1−2!1θ2+4!1θ4−...)u∧u∧=u∧u∧+I+sinθu∧−cosθu∧u∧=(1−cosθ)u∧u∧+I+sinθu∧=cosθI+(1−cosθ)uuT+sinθu∧

vθ=vcos⁡θ+(u×v)sin⁡θ+u(u⋅v)(1−cos⁡θ)=exp⁡(θu∧)∗v=Rv\boldsymbol{v}_{\theta}=\boldsymbol{v} \cos \theta+(\boldsymbol{u} \times \boldsymbol{v}) \sin \theta+\boldsymbol{u}(\boldsymbol{u} \cdot \boldsymbol{v})(1-\cos \theta)=\exp (\theta{\mathbf{u}^ \wedge })*v=Rvvθ=vcosθ+(u×v)sinθ+u(u⋅v)(1−cosθ)=exp(θu∧)∗v=Rv

李代数表示的优势在于求导优化。可以通过指数展开，舍弃高次方项，推导出简洁的线性公式。
核心是用李代数形式的旋转来表示这次迭代优化要得到的微量旋转，R∗exp(θu∧)R*exp(\theta u^{\wedge})R∗exp(θu∧)、exp(θu∧)∗Rexp(\theta u^{\wedge})*Rexp(θu∧)∗R

旋转描述

旋转矩阵

每个旋转有唯一的旋转矩阵。
默认，x->y、y->z、z->x的旋转为正角度。逆时针旋转为正、右手螺旋

这里，第2个是z->x旋转，但是x坐标在z坐标前面，所以，旋转矩阵要调整下。

欧拉角

每个旋转可以有无数个欧拉角。因为旋转2π2\pi2π就会回到原来的位置。
把旋转分解成沿三个轴转动的量：滚转角－俯仰角－偏航角(roll-pitch-yaw)。它的好处是十分的直观，且只有三个参数描述。缺点是会碰到著名的万向锁问题：在俯仰为±90∘时，表达某个姿态的形式不唯一。此外，它也不易于插值和迭代。

万向锁，表达某个姿态的形式不唯一。因为第二次旋转90°后，第一次旋转、和第三次旋转是旋转轴一样的。丢失了一个维度的自由度。我觉得迭代优化时，这个问题也可以接受呀。

欧拉角计算复杂，所以不易于插值和迭代。记录这样一个变换，至少需要三个角的sine和cosine值，也就是一共存储6个单位数据。

罗格里格斯旋转

求一个向量绕任意轴旋转 [公式] 后的向量是多少。我们可以使用罗德里格公式解得。

公式–>旋转矩阵

四元数

知乎链接

李代数

李代数
矩阵指数函数
e

上面模型是R∗θR*\thetaR∗θ。还有一种建模是θ∗R\theta * Rθ∗R。

【slam】位姿变化相关推荐

一起做激光SLAM[六]isam于SLAM位姿因子图优化的使用
本节目标:学习gtsam与isam在二位位姿pose2和三维位姿pose3上的使用,并将isam用于位姿的因子图优化. 预期效果:将ICP匹配带来的瞬间位移变成对之前累积误差的消除.蓝色ICP无图优化 ...
SLAM优化位姿时，误差函数的雅可比矩阵的推导。
转载请说明出处: http://blog.csdn.net/zhubaohua_bupt/article/details/74011005 视觉SLAM的核心是求取相机的位姿并建图. 在优化位姿时,其 ...
SLAM系统性能评估：绝对轨迹误差(ATE)和相对位姿误差(RPE)
绝对轨迹误差直接计算相机位姿的真实值与SLAM系统的估计值之间的差,程序首先根据位姿的时间戳将真实值和估计值进行对齐, 然后计算每对位姿之间的差值, 并最终以图表的形式输出, 该标准非常适合于评估视 ...
SLAM中的位姿与轨迹评价指标:APE、RPE、ATE、RTE
本篇文章原文为<SLAM中的位姿与轨迹评价指标:APE.RPE.ATE.RTE> 0.背景前两天学弟在看EVO的一个有关于Metrics的文档的时候,对于APE和RPE的计算不太清楚.问 ...
一起做激光SLAM：ICP匹配用于闭环检测
编辑丨古月居目标利用ICP进行闭环检测,完成闭环. 预期效果:通过闭环检测完成起止闭环,下图为加入闭环前后. rosbag数据: https://pan.baidu.com/s/1o-noUxgV ...
据我所知，这是第一个完整实现运动分割、动态目标追踪等的「开源」动态SLAM系统！...
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达今天给大家分享一篇最新文章,VDO-SLAM :一种动态目标感知的视觉SLAM系统,原文名称 VDO- ...
李飞飞团队最新论文：基于anchor关键点的类别级物体6D位姿跟踪
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达简介作者提出了一种基于RGB-D的深度学习方法6PACK,能够实时的跟踪已知类别物体.通过学习用少量 ...
对比激光SLAM与视觉SLAM：谁会成为未来主流趋势？
来源:智车科技摘要:SLAM(同步定位与地图构建),是指运动物体根据传感器的信息,一边计算自身位置,一边构建环境地图的过程,解决机器人等在未知环境下运动时的定位与地图构建问题. 目前,SLAM 的主 ...
视觉SLAM总结——SVO中关键知识点总结
视觉SLAM总结--SVO中关键知识点总结视觉SLAM总结--SVO中关键知识点总结 1. 为什么叫半直接法视觉里程计? 2. SVO的优缺点是什么? 3. SVO的整体框架是怎样的? 4. 分别介 ...

【slam】位姿变化