视觉SLAM十四讲-第三讲-三维空间刚体运动教学内容

第三章刚体力学§3.1 刚体运动的分析§3.2 角速度矢量§3.3 刚体运动微分方程§3.4 刚体平衡方程§3.5 转动惯量§3.6 刚体的平动与定轴转动§3.7 刚体的平面平行运动§3.1 刚体运动的分析一、描述刚体位置的独立变量1.刚体是特殊质点组 dr ij=0,注意:它是一种理想模型,形变大小可忽略时可视为刚体。

2.描述刚体位置的独立变数描述一个质点需(x,y,z), 对刚体是否用 3n 个变量？否,由于任意质点之间的距离不变, 如确定不在同一直线上的三点,即可确定刚体的位置,需 9 个变量，由于两点间的距离保持不变，所以共需 9-3=6 个变量即可。

刚体的任意运动=质心的平动+绕质心的转动，描述质心可用(x,y,z), 描述转轴可由α, β,γ。

二、刚体的运动分类1.平动：刚体在运动过程中，刚体上任意直线始终平行.任意一点均可代表刚体的运动，通常选质心为代表.需要三个独立变量,可以看成质点力学问题.(注意:平动未必是直线运动)2.定轴转动: 刚体上有两点不动,刚体绕过这两点的直线转动,该直线为转轴. 需要一个独立变量φ3.平面平行运动: 刚体上各点均平行于某一固定平面运动。

可以用平行于固定平面的截面代表刚体。

需要三个独立变量。

4.定点运动: 刚体中一点不动,刚体绕过固定点的瞬转转动。

需三个独立的欧拉角。

5.一般运动: 平动+转动§3.2 角速度矢量定轴转动时角位移用有向线段表示,右手法确定其方向.有向线段不一定是矢量,必须满足平行四边形法则,对定点转动时,不能直接推广,因不存在固定轴.ω = lim ∆n=d n刚体在 dt 时间内转过的角位移为 d n ，则角速度定义为角速度反映刚体转动的快慢。

∆t →0 ∆t dt线速度与角速度的关系：d r =d n ⨯r , ∴ v =d rdt=ω ⨯rF 1 F ⨯ M§3.3 刚体运动微分方程一、 基础知识1.力系：作用于刚体上里的集合。

《视觉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）齐次坐标与旋转矩阵⽬录：什么是齐次坐标？简单的说：齐次坐标就是在原有坐标上加上⼀个维度：使⽤齐次坐标有什么优势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。

第一节课习题屋卡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 ⼀件。