视觉SLAM十四讲-第三讲-三维空间刚体运动共31页
《视觉SLAM十四讲》笔记（ch7）
《视觉SLAM⼗四讲》笔记（ch7）ch7 视觉⾥程计1本章⽬标：1.理解图像特征点的意义，并掌握在单副图像中提取出特征点及多副图像中匹配特征点的⽅法2.理解对极⼏何的原理，利⽤对极⼏何的约束，恢复出图像之间的摄像机的三维运动3.理解PNP问题，以及利⽤已知三维结构与图像的对应关系求解摄像机的三维运动4.理解ICP问题，以及利⽤点云的匹配关系求解摄像机的三维运动5.理解如何通过三⾓化获得⼆维图像上对应点的三维结构本章⽬的：基于特征点法的vo，将介绍什么是特征点，如何提取和匹配特征点，以及如何根据配对的特征点估计相机运动和场景结构，从⽽实现⼀个基本的两帧间视觉⾥程计。
特征点：⾓点、SIFT(尺度不变特征变换，Scale-Invariant Feature Transform)、SURF、、ORB（后三个是⼈⼯设计的特征点，具有更多的优点）特征点的组成：1.关键点：指特征点在图像⾥的位置2.描述⼦：通常是⼀个向量，按照某种⼈为设计的⽅式，描述了该关键点周围像素的信息。
相似的特征应该有相似的描述⼦（即当两个特征点的描述⼦在向量空间上的距离相近，认为这两个特征点是⼀样的）以ORB特征为代表介绍提取特征的整个过程：ORB特征：OrientedFAST关键点+BRIEF关键⼦提取ORB特征的步骤：1.提取FAST⾓点：找出图像中的“⾓点”，计算特征点的主⽅向，为后续BRIEF描述⼦增加了旋转不变特性FAST⾓点：主要检测局部像素灰度变化明显的地⽅特点：速度快缺点：1).FAST特征点数量很⼤且不确定，但是我们希望对图像提取固定数量的特征2).FAST⾓点不具有⽅向信息，并且存在尺度问题解决⽅式：1).指定要提取的⾓点数量N，对原始FAST⾓点分别计算Harris响应值，然后选取前N个具有最⼤响应值的⾓点作为最终的⾓点集合2).添加尺度和旋转的描述 尺度不变性的实现：构建图像⾦字塔，并在⾦字塔的每⼀层上检测⾓点（⾦字塔：指对图像进⾏不同层次的降采样，以获得不同分辨率的图像）特征旋转的实现：灰度质⼼法（质⼼：指以图像块灰度值作为权重的中⼼）2.计算BRIEF描述⼦：对前⼀步提取出的特征点周围图像区域进⾏扫描特点：使⽤随机选点的⽐较，速度⾮常快，由于使⽤了⼆进制表达，存储起来也⼗分⽅便，适⽤于实时的图像匹配在不同图像之间进⾏特征匹配的⽅法：1.暴⼒匹配：浮点类型的描述⼦，使⽤欧式距离度量⼆进制类型的描述⼦（⽐如本例中的BRIEF描述⼦），使⽤汉明距离度量缺点：当特征点数量很⼤时，暴⼒匹配法的运算量会变得很⼤2.快速近似最近邻（FLANN）：适合匹配特征点数量极多的情况实践部分：1.OpenCV的图像特征提取、计算和匹配的过程：演⽰如何提取ORB特征并进⾏匹配代码： 1 #include <iostream>2 #include <opencv2/core/core.hpp>3 #include <opencv2/features2d/features2d.hpp>4 #include <opencv2/highgui/highgui.hpp>56using namespace std;7using namespace cv;89int main(int argc,char** argv)10 {11if(argc!=3)12 {13 cout<<"usage:feature_extraction img1 img2"<<endl;14return1;15 }1617//读取图像18 Mat img_1=imread(argv[1],CV_LOAD_IMAGE_COLOR);19 Mat img_2=imread(argv[2],CV_LOAD_IMAGE_COLOR);2021//初始化22 vector<KeyPoint> keypoints_1,keypoints_2;//关键点,指特征点在图像⾥的位置23 Mat descriptors_1,descriptors_2;//描述⼦,通常是向量24 Ptr<ORB> orb=ORB::create(500,1.2f,8,31,0,2,ORB::HARRIS_SCORE,31,20);2526//第⼀步：检测OrientFAST⾓点位置27 orb->detect(img_1,keypoints_1);28 orb->detect(img_2,keypoints_2);2930//第2步：根据⾓点位置计算BRIEF描述⼦31 orb->compute(img_1,keypoints_1,descriptors_1);32 orb->compute(img_2,keypoints_2,descriptors_2);3334 Mat outimg1;35 drawKeypoints(img_1,keypoints_1,outimg1,Scalar::all(-1),DrawMatchesFlags::DEFAULT);36 imshow("1.png的ORB特征点",outimg1);37 Mat outimg2;38 drawKeypoints(img_2,keypoints_2,outimg2,Scalar::all(-1),DrawMatchesFlags::DEFAULT);39 imshow("2.png的ORB特征点",outimg2);4041//第3步：对两幅图像中的BRIEF描述⼦进⾏匹配，使⽤Hamming距离42 vector<DMatch> matches;43//特征匹配的⽅法：暴⼒匹配44 BFMatcher matcher(NORM_HAMMING);45 matcher.match(descriptors_1,descriptors_2,matches);46// for(auto it=matches.begin();it!=matches.end();++it)47// {48// cout<<*it<<" ";49// }50// cout<<endl;5152//第4步：匹配点对筛选53 distance是min_dist5455double min_dist=10000,max_dist=0;5657//找出所有匹配之间的最⼩距离和最⼤距离，即最相似的和最不相似的和最不相似的两组点之间的距离58for(int i=0;i<descriptors_1.rows;++i)59 {60double dist=matches[i].distance;61// cout<<dist<<endl;62if(dist<min_dist) min_dist=dist;63if(dist>max_dist) max_dist=dist;64 }6566 printf("--Max dist:%f\n",max_dist);67 printf("--Min dist:%f\n",min_dist);6869//当描述⼦之间的距离⼤于两倍的最⼩距离时，即认为匹配有误70//但有时候最⼩距离会⾮常⼩，设置⼀个经验值作为下限71 vector<DMatch> good_matches;72for(int i=0;i<descriptors_1.rows;++i)73 {74if(matches[i].distance<=max(2*min_dist,30.0))75 {76 good_matches.push_back(matches[i]);77 }78 }7980//第5步：绘制匹配结果81 Mat img_match;82 Mat img_goodmatch;83 drawMatches(img_1,keypoints_1,img_2,keypoints_2,matches,img_match);84 drawMatches(img_1,keypoints_1,img_2,keypoints_2,good_matches,img_goodmatch);85 imshow("所有匹配点对",img_match);86 imshow("优化后匹配点对",img_goodmatch);87 waitKey(0);8889return0;90 }实验结果：1.png中提取到的特征点2.png中提取到的特征点匹配结果： 所有点对匹配结果 优化后的匹配点对结果（筛选依据是Hamming距离⼩于最⼩距离的两倍）结果分析：尽管在这个例⼦中利⽤⼯程经验优化筛选出正确的匹配，但并不能保证在所有其他图像中得到的匹配都是正确的，所以，在后⾯的运动估计中，还要使⽤去除误匹配的算法。
机器人操作的数学导论——刚体运动(1——3)讲解
二、三维空间中的旋转运动
2.2 旋转的指数坐标
定理2.3 指数变换是SO（3）上的满射变换 对给定的R∈SO(3),存在w∈R3,||w||=1及θ ∈R，使R=exp((w)^θ )
定理2.4 任意姿态R∈SO(3)等效于绕固定轴w∈R3,θ ∈[0,2π ] 该法并不唯一，当R=I时，W（θ取0）有无穷多中。
定理2.6 从se(3)到SE（3）的指数变换
三、三维空间中的刚体运动
3.2 刚体运动的指数坐标和运动旋量
描述的不是点在不同坐标系间的变换，而是点 由初始位置p(0)∈R3到经如下刚体转动后的位置坐标间的变换
上式中p(θ),p(0)均在同一坐标系中表示。类似，若gab(0)表示刚 体相对于A系的起始位姿，，那么现对于A系的最终位姿为：
对于一运动旋量来说，指数变换反映的是刚体的相对运动， 每一个刚体变换都可写为某个运动旋量的指数。
三、三维空间中的刚体运动
3.2 刚体运动的指数坐标和运动旋量
定理2.7 建立在SE（3）的指数变换是满射变换
3.3 旋量：运动旋量的几何表示
se(3)中的元素 称为运动旋量
三、三维空间中的刚体运动
3.3 旋量：运动旋量的几何表示
0
式中(a)^
a3
a3 a2
0
a1
a2 a1 0
后面常用符号â来代替(a)^
引理2.1 对给定的R∈SO(3)和v,w∈R3,则存在下列性质 R(v×w)=(Rv) ×(Rw) (两矢量叉积的旋转=旋转的叉积） R(w)^RT=(Байду номын сангаасw)^
定理2.2 旋转运动是刚体变换 旋转矩阵R∈SO(3)是一个刚体变换
视觉slam介绍PPT课件
2021
第一讲 视觉SLAM概述
• 相机的本质
• 以二维投影形式记录了三维世界的信息 • 此过程丢掉了一个维度：距离
• 各类相机主要区别：有没有深度信息
• 单目：没有深度，必须通过移动相机产生深度 Moving View Stereo • 双目：通过视差计算深度 Stereo • RGBD：通过物理方法测量深度
• 主流方法：SIFT/SURF/ORB （OpenCV features2d模块）
2021
3.1.1 ORB特征
• 例子：ORB特征
• 关键点：Oriented FAST • 描述：BRIEF
• FAST
• 连续N个点的灰度有明显差异
• Oriented FAST • 在FAST基础上计算旋转
• BRIEF • BRIEF-128：在特征点附近的128次像素比较
整理之：
2021
第二讲 相机模型
• 成像平面到像素坐标
代入 得
2021
第二讲 相机模型
展开形式 矩阵形式
传统习惯
左侧是齐次坐标 中间矩阵称为内参数 右侧是非齐次坐标 内参通常在相机生产之后就已固定
2021
第二讲 相机模型
• 除内参外，相机坐标系与世界坐标系还相差一个变换：
• 这里 R, t 或 T 称为外参 • 外参是SLAM估计的目标
2021
实践 点云拼接
2021
第三讲 视觉里程计
Chapter 3: Visual Odometry
2021
3.1 特征点法
• 经典SLAM模型中以位姿——路标（Landmark）来描述SLAM过程 • 路标是三维空间中固定不变的点，能够在特定位姿下观测到
视觉SLAM十四讲 第六讲 非线性优化
注 意 这 里 是 对 平 方 展 开 的
雅可比 海塞
• 若只保留一阶梯度：
增量的方向： （通常还需要计算步长）
• 称为最速下降法（Steepest Method）
18 6.2 非线性最小二乘
• 若保留二阶梯度：
• 则得到（令上式关于
的导数为零）：
• 该方法又称为牛顿法
19 6.2 非线性最小二乘
• 解： 将得到极值点或鞍点，比较这些解即可
这里
，f 为任意函数
• 当 f 复杂时：
• df/dx难求，或df/dx=0很难解
• 使用迭代方式求解
16 6.2 非线性最小二乘
• 迭代方式：
问题：如何确定这个增量？
17 6.2 非线性最小二乘
• 确定增量的方法（即梯度下降策略）：一阶的或二阶的 • 泰勒展开：
2462非线性最小二乘?lm相比于gn能够保证增量方程的正定性?即认为近似只在一定范围内成立如果近似不好则缩小范围?从增量方程上来看可以看成一阶和二阶的混合?参数控制着两边的权重25????62非线性最小二乘?小结?非线性优化是个很大的主题研究者们为之奋斗多年?主要方法
视觉SLAM十四讲
从理论到实践
• 目前没有非凸问题的通用最优值的寻找办法 • 问题凸时，二阶方法通常一两步就能收敛
27
5.3 实践：CERES
28 5.3 实践：CERES
• Google Ceres Solver
• 通用最小二乘问题求解库
• 最一般的形式（带边界的核函数最小二乘）
• f 在Ceres中称为代价函数（Cost Function），x称为参数块（Parameter Block）
7 6.1状态估计问题
【视觉SLAM十四讲】直接法视觉里程计
2 LK 光流(5分，约3小时)2.1光流文献综述(1分)我们课上演示了Lucas-Kanade 稀疏光流，用OpenCV 函数实现了光流法追踪特征点。
实际上，光流法有很长时间的研究历史，直到现在人们还在尝试用Deeplearning 等方法对光流进行改进[1,2]。
本题将指导你完成基于Gauss-Newton 的金字塔光流法。
首先，请阅读文献[3]（paper 目录下提供了pdf ），回答下列问题。
问题：1. 按此文的分类，光流法可分为哪几类？答：作者在文中对光流法按照两种不同的方法进行分类。
➢ 按照估计的是参数的叠加增量还是增量Warp 将光流法分为叠加(additional)和组合(compositional)算法➢ 按照Warp 更新规则可以将光流法分为前向（forward ）和逆向/反向(inverse)两种算法综上：可以分4类，分别是 FA(Forward Additional), FC(Forward Composition), (Inverse Additional) 和 IC(Inverse Compositional)。
2. 在compositional 中，为什么有时候需要做原始图像的wrap ？该wrap 有何物理意义？答: 与Lucas-Kanade 算法中那样简单地将迭代的更新 p ∆添加到当前估计的参数p 不同，组合（compositional ）算法中，对扭曲();W x p ∆的增量更新必须由Warp 的当前估计组成() ;Wx p 。
这两个 warp 的组合可能更复杂。
因此，我们对 warp 集合有两个要求: 1）warp 集合必须包含 identity warp 。
2）warp 集合必须在组合运算中闭合。
需要在当前位姿估计之前引入增量式 warp （incremental warp ）以建立半群约束要求（the semi-group requirement ）。
【视觉SLAM十四讲课后作业答案】第三讲：李群与李代数
第一节课习题屋卡2017年12月19日一、验证向量叉乘的李代数性质我们说向量和叉乘运算构成了李代数，现在请你验证它。
书中对李代数的定义为：李代数由⼀个集合V，⼀个数域 F 和⼀个⼀元运算[,] 组成。
如果它们满⼀以下⼀条性质，称(V,F,[,]) 为⼀个李代数，记作g二、推导SE(3) 的指数映射课上给出了SO(3) 的指数映射推导，但对于SE(3)，仅介绍了结论，没有给出详细推导。
请你完成SE(3) 指数映射部分，有关左雅可⼀的详细推导。
三、伴随在SO(3) 和SE(3) 上，有⼀个东西称为伴随（Adjoint）。
下⼀请你证明SO(3) 伴随的性质。
对于SO(3)，有：Rexp(p∧)RT = exp((Rp)∧).此时称Ad(R) = R。
解：四、轨迹的描绘我们通常会记录机器⼀的运动轨迹，来观察它的运动是否符合预期。
⼀部分数据集都会提供标准轨迹以供参考，如kitti、TUM-RGBD 等。
这些⼀件会有各⼀的格式，但⼀先你要理解它的内容。
记世界坐标系为W，机器⼀坐标系为C，那么机器⼀的运动可以⼀TWC 或TCW 来描述。
现在，我们希望画出机器⼀在世界当中的运动轨迹，请回答以下问题：1.事实上，TWC 的平移部分即构成了机器⼀的轨迹。
它的物理意义是什么？为何画出TWC 的平移部分就得到了机器⼀的轨迹？解：wc T 的平移部分构成机器人的轨迹，它的物理意义是机器人在现实三维空间中的坐标点。
wc T 包含平移和旋转部分 ，平移指的是机器人在空间中的运动位置，而旋转部分是指机器人在每个位置的方向状态。
2. 我为你准备了⼀个轨迹⼀件（code/trajectory.txt ）。
该⼀件的每⼀⼀由若⼀个数据组成，格式为[t,tx,ty,tz,qx,qy,qz,qw],其中 t 为时间，tx,ty,tz 为 TWC 的平移部分，qx,qy,qz,qw 是四元数表⼀的 TWC 的旋转部分，qw 为四元数实部。
同时，我为你提供了画图程序 draw_trajectory.cpp ⼀件。
视觉SLAM综述
通过基于光束平差法 ( Bundle Adjustment，BA) 的图优化理论估计相机的 位姿和地图点坐标，大大 提高了 SLAM 的精度。
目前，基于关键帧的VSLAM成为主流算法。最具代表性的有 两种: 一种是 LSD-SLAM [6] ，属于半稠密的单目的 VSLAM; 一 种是 ORB-SLAM [7,8] ，属于稀疏的 VSLAM。
3、稠密VSLAM
为了降低稠密地图的计算量，Engel J[12,13]提出了半稠 密地图（semi-direct visual odometry，SVO），通过结 合关键点的思想和直接配准的方法，大大降低了计算复 杂度。目前最先进的SVO就是Engel J等提出的LSDSLAM[13]，已经能在CPU上实现了实时性。
什么是SLAM？
SLAM问题可以描述为: 机器人在未知环境中从一个未知 位置开始移动,在移动过程中根据位置估计和地图进行 自身定位,同时在自身定位的基础上建造增量式地图， 实现机器人的自主定位和导航。
（1）定位(localization)：机器人必须知道自己在环境中位 置。 （2）建图(mapping)：机器人必须记录环境中特征的位置 （如果知道自己的位置）
后端图优化流程
2.3 后端优化
2.4 建图
在经典的VSLAM模型中，所谓的建图，即找到所有路标 点，这些路标点的集合即是最终的地图，一旦确定了路 标点的位置，即可以说完成了建图。地图类型分为栅格 地图，拓扑地图，特征地图和混合地图。根据建图需求 不同，可以将建图用途分为两类：
定位：把地图下来，机器人下次开机后仍然能在地图上 定位，不需要再次建模，此类地图，稀疏地图即可完成。
1.2 地图的稠密程度
视觉slam十四讲课后习题ch3-7
视觉slam⼗四讲课后习题ch3-7题⽬回顾:设有⼩萝⼘⼀号和⼩萝⼘⼆号位于世界坐标系中，⼩萝⼘⼀号的位姿为：q1=[0.35,0.2,0.3,0.1],t2=[0.3,0.1,0.1]^T (q的第⼀项为实部。
请你把q归⼀化后在进⾏计算)。
这⾥的q和t的表达的是Tcw，也就是世界到相机的变换关系。
⼩萝⼘⼆号的位姿为q2=[-0.5,0.4,-0.1,0.2],t=[-0.1,0.5,0.3]^T.现在，⼩萝⼘⼀号看到某个点在⾃⾝的坐标系下，坐标为p=[0.5,0,0.2]^T ,求该向量在⼩萝⼘⼆号坐标系下的坐标，请编程实现此事。
解:pw:某个点在世界坐标系下的坐标T_1w :表⽰世界坐标系到⼩萝⼘⼀号坐标系的变换关系T_2w:表⽰世界坐标系到⼩萝⼘⼆号坐标系的变换关系P2 :表⽰该点在⼩萝⼘⼆号坐标系下的坐标（即为所求）单位四元数到旋转矩阵R的变化关系可参考书上55页。
之后变换矩阵T=[R t][0 1]由变换关系可列出下⾯的式⼦:p = T_1w * Pw 可解出来pwp2=T_2W*pW 带⼊上式解出来的Pw即可求出来p2具体代码实现如下:1 #include<iostream>2 #include<Eigen/Core>34//包含⼏何模块5 #include<Eigen/Geometry>6using namespace std;78int main(int argc,char **argv)9 {10/*变量定义*/11 Eigen::Quaterniond Q1(0.2,0.3,0.1,0.35); //四元数的表⽰(w ,x,y,z)12 Eigen::Quaterniond Q2(0.4,-0.1,0.2,-0.5);13 Eigen::Vector3d t1(0.3,0.1,0.1);14 Eigen::Vector3d t2(-0.1,0.5,0.3);15 Eigen::Vector3d p(0.5,0,0.2); //在⼀号⼩萝⼘下的坐标16 Eigen::Vector3d pw ; //世界坐标17 Eigen::Vector3d p2; //求在⼆号⼩萝⼘的坐标 p21819/*欧⽒变换矩阵使⽤Eigen::Isometry */20 Eigen::Isometry3d T_1w = Eigen::Isometry3d::Identity();21 Eigen::Isometry3d T_2w = Eigen::Isometry3d::Identity();2223/*归⼀化*/24 Q1.normalize();25 Q2.normalize();2627/*输出归⼀化参数*/28// cout<<"Q1 is "<<Q1.x()<<endl<<Q1.y()<< endl <<Q1.z()<< endl<<Q1.w()<<endl;29// cout<<"Q2 is "<<Q2.x()<<endl<<Q2.y()<< endl <<Q2.z()<< endl<<Q2.w()<<endl;3031 cout<<"after normalize； "<< endl << Q2.coeffs()<<endl;3233/*设置变换矩阵的参数*/34 T_1w.rotate(Q1);35 T_1w.pretranslate(t1);36 T_2w.rotate(Q2);37 T_2w.pretranslate(t2);3839/* p = T1w * pw 求解pw*/40 pw = T_1w.inverse() * p;4142/* p2 = T_2w * pw 求解p2*/43 p2 = T_2w * pw;4445/*输出在⼩萝⼘⼆号下的该点坐标*/46 cout<<"该点在⼩萝⼘⼆号下的坐标为: "<<p2.transpose()<<endl;4748return0;49 }欢迎⼤家关注我的微信公众号「佛系师兄」，⾥⾯有关于 Ceres 以及 OpenCV 库的⼀些⽂章。
视觉基础介绍
⎡ xw ⎤ ⎡ xc ⎤ ⎢ y ⎥ = R⎢ y ⎥ + t ⎢ w⎥ ⎢ c⎥ ⎢zw ⎥ ⎢ zc ⎥ ⎣ ⎦ ⎣ ⎦
齐次坐标形式
⎡ xc ⎤ ⎢y ⎥ ⎢ c⎥ = ⎡ R ⎢ z c ⎥ ⎢ 0T ⎣ 3 ⎢ ⎥ ⎣1 ⎦
⎡ xw ⎤ t ⎤ ⎢ yw ⎥ ⎢ ⎥ 1⎥ ⎢ xw ⎥ ⎦ ⎢ ⎥ ⎣1 ⎦
比例 被叫作 P 关于 P1 , P 2 在这条直 线上的射影参数. 如果 c 2 = 0 , 则射影参数为 ∞ .
c1 c2
交比
Xw Zw
[u, v ] [x, y ]
Xc
O w
Yw
世界坐标系
x u
Zc
[X u , Yu ]
[X d , Yd ]
O
v
O1
图像坐标系
y
Yc
分别描述畸变前后的坐标关系
摄像机坐标系
摄像机光学成像过程的四个步骤
1、刚体变换公式
世界坐标系 刚体变换 摄像机坐标系 透视投影 理想图像坐标系 畸变校正 真实图像坐标系 数字化图像 数字化图像坐标系
xn x1 x0 ≠ 0, = m 1 ,..., = mn. x0 x0
则 ( x 1 ,..., x n , x 0 ) 被叫作这个点的齐次 坐标.
相对于齐次坐标 , ( m 1 ,..., m n ) 被称作非 齐次坐标. 不全为0的数 x 1 ,..., x n 组成的坐标
( x1 ,..., x n , 0 )
无穷远平面的方程则为:
x0 = 0
射影参数
对于 n 维空间中的任意一条直线, 如果 P1 , P 2 是它上的任意两个取定的点, 则它 上的任意一个点 P 可以由 P1 , P 2 线性生 成:
刚体在三维空间的旋转（关于旋转矩阵、DCM、旋转向量、四元数、欧拉角）
刚体在三维空间的旋转（关于旋转矩阵、DCM、旋转向量、四元数、欧拉角）最近学习了一些关于三维空间旋转相关的知识，借此梳理一下备忘。
三维空间的旋转(3D Rotation)是一个很神奇的东东：如果对某个刚体在三维空间进行任意次的旋转，只要旋转中心保持不变，无论多少次的旋转都可以用绕三维空间中某一个轴的一次旋转来表示。
表示三维空间的旋转有多种互相等价的方式，常见的有旋转矩阵、DCM、旋转向量、四元数、欧拉角等。
本篇文章主要梳理一下这些表示方式及相互转换的方法。
1. 欧拉角(Euler Angle)最直观的表示方式是绕刚体自身的X、Y、Z三个轴分别进行旋转某个角度，这就是所谓的欧拉角(Euler Angle)表示方式。
Euler Angle需要注意的是，欧拉角的表示方式里，yaw、pitch、roll的顺序对旋转的结果是有影响的。
给定一组欧拉角角度值，比如yaw=45度，pitch=30度，roll=60度，按照yaw-pitch-roll的顺序旋转和按照yaw-roll-pitch的顺序旋转，最终刚体的朝向是不同的！换言之，若刚体需要按照两种不同的旋转顺序旋转到相同的朝向，所需要的欧拉角角度值则是不同的！另外需要注意的是，在欧拉角的表示方式里，三个旋转轴一般是随着刚体在运动，即wikipedia中所谓的intrinsic rotation，见下图动画所示(图来自wikipedia)。
相对应的另一种表示方式是，三个旋转轴是固定的，不随刚体旋转而旋转，即extrinsic rotation，这种表示方式在计算机视觉中不是很常用。
欧拉角的表示方式比较直观，但是有几个缺点：(1) 欧拉角的表示方式不唯一。
给定某个起始朝向和目标朝向，即使给定yaw、pitch、roll的顺序，也可以通过不同的yaw/pitch/roll的角度组合来表示所需的旋转。
比如，同样的yaw-pitch-roll顺序，(0,90,0)和(90,90,90)会将刚体转到相同的位置。
视觉SLAM十四讲——对极约束求解相机运动（2D-2D）
视觉SLAM⼗四讲——对极约束求解相机运动（2D-2D）主要内容1. 对极约束 ⼏何意义是，P，三者共⾯，对极约束同时包含了平移和旋转。
基础矩阵： 本质矩阵： 对极约束表⽰： 其中，分别表⽰为相机坐标系下归⼀化的平⾯坐标2. 本质矩阵的特点（3×3） 1）E在不同尺度下是等价的 2) 内在性质：奇异值必定是的形式 3）由于平移+旋转，共有6个⾃由度，但因为尺度等价性，E实际上有5个⾃由度3 本质矩阵求解——⼋点法 1） E的内在性质是⼀种⾮线性性质，在求解线性⽅程时会带来⿇烦，因此不会⽤5对点来求解。
2）未知数共有9个，只考虑尺度等价性，所以⽤8对2D-2D点来求解，线性变换的⽅法。
3）求解矩阵后，进⾏奇异值分解分（SVD），得到运动信息 4）分解的时候，会有4中情况，选取⼀个点进⾏求解，检测该点在两个相机下的深度信息，选取两个正的深度。
5）线性⽅程解可能不满⾜E的内在性质，将求解出来的矩阵投影到E所在的流形上。
4 单应矩阵H 定义与旋转，平移及平⾯的参数有关。
1) ⾃由度为8的单应矩阵可以通过4对匹配点进⾏计算（注意，这些特征点不能有三点共线的情况） 2）求解⽅法：直接线性变换法，对其进⾏分解得到运动信息，分解⽅法有数值法和解析法 4组解，（根据计算点的深度以及平⾯的法向量信息选取合理的解） 3）意义 3.1）描述了两个平⾯之间的映射关系，若所有特征点落在同⼀平⾯，则可以通过H估计运动， 具体应⽤在⽆⼈机携带俯视相机或扫地机器⼈的顶视相机中。
3.2）当特征点共⾯或者相机发⽣纯旋转时，基础矩阵⾃由度下降，H可以估计纯旋转的运动信息。
5. 在实际应⽤，为避免退化现象造成的影响，会同时估计F和H，选择其中重投影误差⽐较⼩的那个作为最终运动的估计结果。
6. 对极⼏何2D-2D的特点 1）尺度不确定性 t归⼀化相当于固定了尺度信息，成为单⽬SLAM的初始化，后⾯的轨迹和地图以此单位进⾏计算。
视觉SLAM：从理论到实践李群与李代数
• 至此，说明了 SO(3) 与 so(3) 的对应关系。
3. 指数映射和对数映射
• se(3)到SE(3)的指数映射：
其中J为雅可比矩阵（留作习题）
3. 指数映射和对数映射
4. 李代数求导与扰动模型
4. 求导与扰动模型
• SLAM的定位即位姿估计
• 但李群无加法：
导数无从定义
• 解决办法：
• 同理，SE(3)亦有李代数se(3)：
• 上尖尖^ 不再是反对称矩阵，但仍保留记法：
李括号：
2. 李群与李代数
• 习题部分请你验证se(3)和so(3)的李代数性质 • 注：
• 不同书籍对se(3)的平前的。
• 把李代数理解成向量形式或矩阵形式都是可以的。向量形式更加自然一些。
• 直观写法（以左乘为例）
• 在李群上左乘小量时，李代数上的加法相差左雅可比的逆
• 反之
• 李代数上进行小量加法时，相当于李群上左（右）乘一个带左（右）雅可比的量
• SE(3)比SO(3)更复杂： 这里不展开花体雅可比
4. 求导与扰动模型
• 通过BCH线性近似，可以定义李代数上的导数 • 考虑一个基本问题：旋转后的点关于旋转的导数
2. 李群与李代数
• 李代数：与李群对应的一种结构，位于向量空间。
• 通常记作小写的so(3)和se(3)。书中以哥特体突出显示。 • 事实上是李群单位元处的正切空间。
• 下面从旋转矩阵引出李代数 • 考虑任意旋转矩阵R，满足
2. 李群与李代数
• 令R随时间变化（连续运动），有： • 两侧对时间求导：
2. 李群与李代数
• 该式说明，对任意t，都可以找到一个R和一个 ������ 的对应关系 • 该关系称为指数映射（Exponential Map） • 这里的 ������ 称为SO(3)对应的李代数：so(3) • 问题：
理论力学第14章 刚体定点转动与刚体一般运动
v2 = ω × r2
=
−ω1(
j
+
l r
k)
× (ri + lk)
ω
γ
ω2
ζ ωω11
O
γ
α
= −ω1 (li + lj − rk ) x (ξ )
? 方向
a2B = α × r2
? =
−
l2 r
ω
2 1
j
方向
y
a2B
2
xα
a2B
A
z(η)
C*
v2
1
y
36
刚体空间运动
a2N = ω× v2
= ω12[−(
OC*为瞬轴 vC = AC ⋅ ω ω = vC = vC AC r cos β
=
h2 + rh
r2
v C = 常数
25
刚体空间运动
2） 求角加速度
先求牵连角速度和相对角速度。
运动分解： 研究对象：圆锥 动系： Oz轴
牵连运动：Oz轴绕ζ 轴的转动
相对运动：绕Oz轴的自转
ωe = ψ& ωr = ϕ& θ& = 0 θ = 常数 ω = ωe + ωr = ψ& + ϕ& 这种 θ = 常数，ψ& = 常数,ϕ& = 常数 的情况: 规则进动
18
刚体空间运动
§14-4 刚体绕相交轴转动的角速度合成定理
一、 刚体绕相交轴转动的角速度合成定理
框架以ω1绕 z轴转动，同时圆盘又以ω2绕CD轴转动。 ω1 ω2
19
刚体空间运动
动系： 框架 绝对运动： 定点转动 相对运动： 绕CD轴转动 牵连运动： 定轴转动
三维空间刚体运动——（1）齐次坐标与旋转矩阵
三维空间刚体运动——（1）齐次坐标与旋转矩阵⽬录：什么是齐次坐标？简单的说：齐次坐标就是在原有坐标上加上⼀个维度：使⽤齐次坐标有什么优势1、能⾮常⽅便的表达点在直线或平⾯上在2D平⾯上，⼀条直线 l 可以⽤⽅程 ax + by + c = 0 来表⽰，该直线⽤向量表⽰的话⼀般记做我们知道点p = (x, y)在直线 l 上的充分必要条件是 ax + by + c = 0如果使⽤齐次坐标的话，点p的齐次坐标就是p'=(x, y, 1)那么 ax + by + c = 0 就可以⽤两个向量的内积（点乘）来表⽰：因此，点p在直线l上的充分必要条件就是直线l 与p的齐次坐标p'的内积：是不是很⽅便呢！同理，我们知道三维空间的⼀个平⾯A可以⽤⽅程 ax + by + cz + d = 0 来表⽰，三维空间的⼀个点P=(x, y, z) 的齐次坐标 P'=(x, y, z, 1)，类似的，点P在空间平⾯A上可以⽤两个向量的内积来表⽰，如下：因此，点P在平⾯A上的充分必要条件就是平⾯A 向量与P的齐次坐标P'的内积（点乘）：2、⽅便表达直线与直线，平⾯与平⾯的交点先给出结论，后⾯再具体解释：结论：在齐次坐标下，可以⽤两个点 p, q 的齐次坐标叉乘结果来表达⼀条直线 l，也就是l = p x q也可以使⽤两条直线 l, m 的叉乘表⽰他们的交点 xx = l x m见下⾯⽰例图。
之所以可以这么简洁的表⽰交点是因为采⽤了齐次坐标的表⽰⽅式。
那么这是为什么呢？先介绍⼀下叉乘（也称叉积、外积）的概念：两个向量 a和b 的叉乘仅在三维空间中有定义，写作 a x ba xb 是与向量 a, b都垂直的向量，其⽅向通过右⼿定则（见下图）决定。
其模长等于以两个向量为边的平⾏四边形的⾯积（见下图）。
叉乘可以定义为：其中θ表⽰a, b的夹⾓（0°到180°之间），||a||, ||b||是向量a, b的模长n则是⼀个与向量a, b所构成的平⾯垂直的单位向量根据叉乘定义：向量⾃⾝叉乘结果为0，因为夹⾓为0。