视觉SLAM十四讲-第六讲-非线性优化教学教材
视觉slam十四讲 引用
视觉SLAM十四讲引言视觉SLAM（Simultaneous Localization and Mapping）是一种通过摄像头获取图像数据，并在其中实时地定位和构建地图的技术。
它在无人驾驶、增强现实、机器人导航等领域有着广泛的应用。
《视觉SLAM十四讲》是一本经典的教材，本文将对该教材进行探讨和总结。
什么是视觉SLAM视觉SLAM是一种通过计算机视觉技术来实现实时定位和地图构建的技术。
通过摄像头获取图像，利用SLAM算法来实时地对机器人的位置和运动进行估计，并同时构建地图。
与传统的SLAM技术相比，视觉SLAM能够减少对其他传感器的依赖，提高系统的自主性和灵活性。
视觉SLAM的基本流程视觉SLAM的基本流程包括图像预处理、特征提取与匹配、运动估计、地图更新等步骤。
具体步骤如下：1.图像预处理–图像去畸变：对图像进行去除镜头畸变的处理，提高后续特征提取和匹配的效果。
–图像降噪：通过滤波等方法降低图像中的噪声，提高图像质量。
2.特征提取与匹配–特征提取：通过提取图像中的角点、边缘等特征点，用于后续的特征匹配和运动估计。
–特征匹配：通过比较两幅图像中的特征点，找到它们之间的对应关系，用于后续的运动估计和地图更新。
3.运动估计–单目SLAM：通过分析图像序列中的特征点的运动，估计机器人的运动轨迹。
–双目SLAM：利用双目摄像头获取的图像，通过立体视觉的方法来估计机器人的运动轨迹。
–深度估计SLAM：通过利用深度传感器获取的深度信息，估计机器人的运动轨迹。
4.地图更新–同步优化：通过对图像序列中的特征点和机器人的位姿进行联合优化，得到更精确的运动轨迹和地图。
–闭环检测：通过对图像序列中的特征点和地图进行匹配，检测是否存在闭环，进而修正运动估计和地图。
视觉SLAM算法简介视觉SLAM算法有很多种，常用的包括特征点法、直接法、半直接法等。
•特征点法：通过提取图像中的特征点，利用这些特征点之间的关系来进行定位和地图构建。
《视觉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距离⼩于最⼩距离的两倍）结果分析：尽管在这个例⼦中利⽤⼯程经验优化筛选出正确的匹配，但并不能保证在所有其他图像中得到的匹配都是正确的，所以，在后⾯的运动估计中，还要使⽤去除误匹配的算法。
视觉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
用形式更简单的 E 。
三角测量确定深度
• 在得到运动之后，下一步我们需要用相机的运动估计特征点的空间位置。在单目 SLAM 中，仅通过单张图 像无法获得像素的深度信息，我们需要通过三角测量（ Triangulation ）（或三角化）的方法来估计地图点
回环检测
• 回环检测，又称闭环检测（ Loop Closure Detection ），主要解决位置 估计随时间漂移的问题。 • 前端提供特征点的提取和轨迹、地图的初值，而后端负责对这所有的数据
进行优化。然而，如果像 VO 那样仅考虑相邻时间上的关联，那么，之前
产生的误差将不可避免地累计到下一个时刻，使得整个 SLAM 会出现累积 误差。长期估计的结果将不可靠，或者说，我们无法构建全局一致的轨迹
用对极几何恢复出在两帧之间摄像机的运动
• 现在，假设我们从两张图像中，得到了一对配对好的特征点，像图 7-7 里显示的那样。如果我们有若 干对这样的匹配点，就可以通过这些二维图像点的对应关系，恢复出在两帧之间摄像机的运动。
• 1. 根据配对点的像素位置，求出 E 或者 F ；
• 2. 根据 E 或者 F ，求出 R, t 。由于 E 和 F 只相差了相机
视觉 SLAM
蔺志强 苏 敬
• SLAM 是 Simultaneous Lo“同时定位与地图构
建”。它是指搭载特定传感器的主体，在没有环 境先验信息的情况下，于运动过程中建立环境的 模型，同时估计自己的运动。如果这里的传感器 主要为相机，那就称为“视觉 SLAM ”。
和地图。
核心问题是如何计算图像间的相似性
• 最简单的方式就是对任意两张图像都做一遍特征匹配，根据正确匹配的数量确定哪两个图像存在关联。但 计算量大，效率低下。 • 基于里程计的几何关系（ Odometry based ）， • 基于外观（ Appearanc e based ）仅根据两张图像的相似性确定回环检测关系，这种做法摆脱了累计误差， 使回环检测模块成为 SLAM 系统中一个相对独立的模块。
《视觉SLAM十四讲》课后习题—ch6
《视觉SLAM⼗四讲》课后习题—ch6 7.请更改曲线拟合实验中的曲线模型，并⽤Ceres和g2o进⾏优化实验。
例如，可以使⽤更多的参数和更复杂的模型 Ceres：以使⽤更多的参数为例：y-exp(ax^3+bx^2+cx+d) 仅仅是在程序中将模型参数增加到4维，没什么创新⽽⾔1 #include <iostream>2 #include <opencv2/core/core.hpp>3 #include <ceres/ceres.h>4 #include <chrono>56using namespace std;789//cost function的计算模型10struct CURVE_FITTING_COST11 {12 CURVE_FITTING_COST(double x,double y):_x(x),_y(y){}13//残差的计算14 template <typename T>15bool operator()(16const T* const abcd,//参数模型，有3维17 T* residual) const//残差18 {19//y-exp(ax^3+bx^2+cx+d)20 residual[0]=T(_y)-ceres::exp(abcd[0]*T(_x)*T(_x)*T(_x)+abcd[1]*T(_x)*T(_x)+21 abcd[2]*T(_x)+abcd[3]);22return true;23 }24const double _x,_y;//x,y数据25 };262728int main(int argc, char *argv[])29 {30double a=1.0,b=2.0,c=1.0,d=1.0;//真实参数值31int N=100; //数据点32double w_sigma=1.0; //噪声Sigma值33 cv::RNG rng; //opencv随机数产⽣器34double abcd[4]={0,0,0,0}; //abc参数的估计值35 vector<double> x_data,y_data; //数据3637 cout<<"generating data: "<<endl;38for(int i=0;i<N;++i)39 {40double x=i/100.0;41 x_data.push_back(x);42 y_data.push_back(43 exp(a*x*x*x+b*x*x+c*x+d)+rng.gaussian(w_sigma)44 );45 cout<<x_data[i]<<""<<y_data[i]<<endl;46 }4748//构建最⼩⼆乘问题49 ceres::Problem problem;50for(int i=0;i<N;++i){51 problem.AddResidualBlock(//向问题中添加误差项52//使⽤⾃动求导，模板参数：误差类型，输出维度，输⼊维度，数值参照前⾯struct中写法53new ceres::AutoDiffCostFunction<CURVE_FITTING_COST,1,4>(54new CURVE_FITTING_COST(x_data[i],y_data[i])55 ),56 nullptr,//核函数，这⾥不使⽤，为空57 abcd //待估计参数58 );59 }6061//配置求解器62 ceres::Solver::Options options;//这⾥有很多配置项可以填63 options.linear_solver_type=ceres::DENSE_QR;//增量⽅程如何求解64 options.minimizer_progress_to_stdout=true;//输出到out6566 ceres::Solver::Summary summary;//优化信息67 chrono::steady_clock::time_point t1=chrono::steady_clock::now();68 ceres::Solve(options,&problem,&summary);//开始优化69 chrono::steady_clock::time_point t2=chrono::steady_clock::now();70 chrono::duration<double> time_used=chrono::duration_cast<chrono::duration<double>>(t2-t1);71 cout<<"solve time cost= "<<time_used.count()<<" seconds."<<endl;7273//输出结果74 cout<<summary.BriefReport()<<endl;75 cout<<"eastimated a,b,c,d= ";76for(auto a:abcd) cout<<a<<"";77 cout<<endl;78return0;79 }运⾏结果为： generating data:0 2.718280.01 2.904290.02 2.074510.03 2.377590.04 4.077210.05 2.594330.06 2.259460.07 3.250260.08 3.499160.09 1.880010.1 3.837770.11 3.066390.12 4.465770.13 1.249440.14 1.361490.15 2.777490.16 2.6230.17 5.327020.18 3.76660.19 2.17730.2 5.162080.21 2.910420.22 1.296550.23 2.301670.24 2.565490.25 3.954110.26 5.4590.27 5.000780.28 4.037680.29 3.883330.3 6.346150.31 4.993920.32 6.039290.33 4.231590.34 4.144030.35 6.218830.36 5.338380.37 5.165940.38 5.450640.39 5.406120.4 7.003210.41 6.616340.42 6.230230.43 7.576960.44 6.021860.45 6.392850.46 7.033930.47 8.666770.48 5.807180.49 8.765480.5 8.156410.51 8.79390.52 9.300430.53 8.562260.54 10.43220.55 10.02040.56 12.28580.57 10.79170.58 10.76250.59 12.790.6 13.22310.61 13.8990.62 13.34770.63 13.91560.64 15.32540.65 14.99430.66 15.79690.67 18.78250.68 19.17310.69 19.8720.7 19.38180.71 23.00330.72 24.15260.73 25.59620.74 25.04040.75 25.95590.76 29.73160.77 30.83040.78 31.28140.79 33.6270.8 36.19120.81 37.66640.82 41.62950.83 43.91260.84 46.7420.85 48.88380.86 54.12650.87 58.21420.88 60.20130.89 65.86820.9 72.31780.91 77.75780.92 82.43110.93 86.74930.94 94.66510.95 98.74120.96 109.8230.97 117.3950.98 128.150.99 135.634iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time0 6.898490e+04 0.00e+00 2.14e+03 0.00e+00 0.00e+00 1.00e+04 0 1.17e-04 2.15e-041 7.950822e+100 -7.95e+100 0.00e+00 5.63e+02 -1.17e+96 5.00e+03 1 1.61e-04 4.42e-042 3.478360e+99 -3.48e+99 0.00e+00 4.95e+02 -5.12e+94 1.25e+03 1 8.29e-05 5.58e-043 3.566582e+95 -3.57e+95 0.00e+00 3.09e+02 -5.30e+90 1.56e+02 1 5.68e-05 6.41e-044 1.183153e+89 -1.18e+89 0.00e+00 1.51e+02 -1.78e+84 9.77e+00 1 5.18e-05 7.13e-045 3.087066e+73 -3.09e+73 0.00e+00 7.00e+01 -4.91e+68 3.05e-01 1 5.72e-05 7.89e-046 4.413641e+31 -4.41e+31 0.00e+00 2.13e+01 -1.04e+27 4.77e-03 1 5.10e-05 8.59e-047 6.604687e+04 2.94e+03 4.98e+03 6.39e-01 1.65e+00 1.43e-02 1 1.23e-04 1.00e-038 5.395798e+04 1.21e+04 1.59e+04 8.07e-01 2.02e+00 4.29e-02 1 1.14e-04 1.13e-039 3.089338e+04 2.31e+04 3.19e+04 5.71e-01 1.62e+00 1.29e-01 1 1.13e-04 1.26e-0310 8.430982e+03 2.25e+04 3.21e+04 3.74e-01 1.30e+00 3.86e-01 1 1.12e-04 1.39e-0311 8.852002e+02 7.55e+03 1.29e+04 1.77e-01 1.08e+00 1.16e+00 1 1.11e-04 1.52e-0312 2.313901e+02 6.54e+02 2.59e+03 4.89e-02 1.01e+00 3.48e+00 1 1.11e-04 1.65e-0313 1.935710e+02 3.78e+01 8.37e+02 2.72e-02 1.01e+00 1.04e+01 1 1.11e-04 1.77e-0314 1.413188e+02 5.23e+01 6.05e+02 6.43e-02 1.01e+00 3.13e+01 1 1.11e-04 1.90e-0315 8.033187e+01 6.10e+01 3.36e+02 1.08e-01 1.01e+00 9.39e+01 1 1.11e-04 2.03e-0316 5.660145e+01 2.37e+01 7.69e+01 9.43e-02 9.99e-01 2.82e+02 1 1.11e-04 2.15e-0317 5.390796e+01 2.69e+00 1.52e+01 5.86e-02 9.97e-01 8.45e+02 1 1.18e-04 2.29e-0318 5.233724e+01 1.57e+00 9.31e+00 9.73e-02 9.96e-01 2.53e+03 1 1.12e-04 2.42e-0319 5.125192e+01 1.09e+00 3.58e+00 1.21e-01 9.98e-01 7.60e+03 1 1.11e-04 2.54e-0320 5.098190e+01 2.70e-01 1.08e+00 9.37e-02 1.00e+00 2.28e+04 1 1.25e-04 2.68e-0321 5.086440e+01 1.18e-01 6.49e-01 1.47e-01 1.00e+00 6.84e+04 1 1.28e-04 2.84e-0322 5.070258e+01 1.62e-01 4.62e-01 2.86e-01 1.00e+00 2.05e+05 1 1.12e-04 2.97e-0323 5.059978e+01 1.03e-01 2.40e-01 3.30e-01 1.00e+00 6.16e+05 1 1.11e-04 3.09e-0324 5.058282e+01 1.70e-02 7.40e-02 1.68e-01 1.00e+00 1.85e+06 1 1.11e-04 3.22e-0325 5.058233e+01 4.94e-04 1.16e-02 3.17e-02 1.00e+00 5.54e+06 1 1.25e-04 3.36e-03solve time cost= 0.00346683 seconds.Ceres Solver Report: Iterations: 26, Initial cost: 6.898490e+04, Final cost: 5.058233e+01, Termination: CONVERGENCE eastimated a,b,c,d= 0.796567 2.2634 0.969126 0.969952与我们设定的真值a=1,b=2,c=1,d=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入门之视觉里程计（6）：相机标定张正友经典标定法详解
SLAM ⼊门之视觉⾥程计（6）：相机标定张正友经典标定法详解想要从⼆维图像中获取到场景的三维信息，相机的内参数是必须的，在SLAM 中，相机通常是提前标定好的。
张正友于1998年在论⽂："A Flexible New Technique fro Camera Calibration"提出了基于单平⾯棋盘格的相机标定⽅法。
该⽅法介于传统的标定⽅法和⾃标定⽅法之间，使⽤简单实⽤性强，有以下优点：不需要额外的器材，⼀张打印的棋盘格即可。
标定简单，相机和标定板可以任意放置。
标定的精度⾼。
相机的内参数设P =(X ,Y ,Z )为场景中的⼀点，在针孔相机模型中，其要经过以下⼏个变换，最终变为⼆维图像上的像点p =(µ,ν):1. 将P 从世界坐标系通过刚体变换(旋转和平移)变换到相机坐标系，这个变换过程使⽤的是相机间的相对位姿，也就是相机的外参数。
2. 从相机坐标系，通过透视投影变换到相机的成像平⾯上的像点p =(x ,y )。
3. 将像点p 从成像坐标系，通过缩放和平移变换到像素坐标系上点p =(µ,ν)。
相机将场景中的三维点变换为图像中的⼆维点，也就是各个坐标系变换的组合，可将上⾯的变换过程整理为矩阵相乘的形式：s µν1=α0c x 0βc y1f 0000f 0001R t 0T1X Y Z1=f x 0c x 00f yc y 001Rt 0T1X Y Z1将矩阵K 称为相机的内参数,K =f x0c x 0f yc y 001其中，α,β表⽰图像上单位距离上像素的个数，则f x =αf ,f y =βf 将相机的焦距f 变换为在x,y ⽅向上像素度量表⽰。
另外，为了不失⼀般性，可以在相机的内参矩阵上添加⼀个扭曲参数γ，该参数⽤来表⽰像素坐标系两个坐标轴的扭曲。
则内参数K 变为K =f xγc x 0f yc y 01对于⼤多数标准相机来说，可将扭曲参数γ设为0. Multiple View Geometry in Computer Vision张⽒标定法在上⼀篇博⽂，介绍的单应矩阵表⽰两个平⾯间的映射。
视觉SLAM十四讲——矩阵eigen使用
视觉SLAM⼗四讲——矩阵eigen使⽤1. 矩阵定义以及常⽤的矩阵运算，包括矩阵的转置，求逆等运算#include <iostream>using namespace std;#include <ctime>// Eigen 部分#include <Eigen/Core>// 稠密矩阵的代数运算（逆，特征值等）#include <Eigen/Dense>#define MATRIX_SIZE 50/***************************** 本程序演⽰了 Eigen 基本类型的使⽤****************************/int main( int argc, char** argv ){// Eigen 中所有向量和矩阵都是Eigen::Matrix，它是⼀个模板类。
它的前三个参数为：数据类型，⾏，列// 声明⼀个2*3的float矩阵Eigen::Matrix<float, 2, 3> matrix_23;// 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix// 例如 Vector3d 实质上是 Eigen::Matrix<double, 3, 1>，即三维向量Eigen::Vector3d v_3d;// 这是⼀样的Eigen::Matrix<float,3,1> vd_3d;// Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); //初始化为零// 如果不确定矩阵⼤⼩，可以使⽤动态⼤⼩的矩阵Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > matrix_dynamic;// 更简单的Eigen::MatrixXd matrix_x;// 这种类型还有很多，我们不⼀⼀列举// 下⾯是对Eigen阵的操作// 输⼊数据（初始化）matrix_23 << 1, 2, 3, 4, 5, 6;// 输出cout << matrix_23 << endl;// ⽤()访问矩阵中的元素for (int i=0; i<2; i++) {for (int j=0; j<3; j++)cout<<matrix_23(i,j)<<"\t";cout<<endl;}// 矩阵和向量相乘（实际上仍是矩阵和矩阵）v_3d << 3, 2, 1;vd_3d << 4,5,6;// 但是在Eigen⾥你不能混合两种不同类型的矩阵，像这样是错的// Eigen::Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;// 应该显式转换Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;cout << result << endl;Eigen::Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;cout << result2 << endl;// 同样你不能搞错矩阵的维度// 试着取消下⾯的注释，看看Eigen会报什么错// Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * v_3d;// ⼀些矩阵运算// 四则运算就不演⽰了，直接⽤+-*/即可。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
4 第六讲 非线性优化
• 上讲回顾
• 三维世界中刚体运动的描述：旋转矩阵、旋转向量、欧拉角、四元数等。 • 观测：相机投影模型。
在给定模型和具体观测时，如何对估计 进行优化？
5
6.1状态估计问题
6 6.1状态估计问题
• 状态估计问题
• 最简单情况：线性系统，高斯噪声 • 复杂情况：非线性系统，非高斯噪声
视觉SLAM十四讲
从理论到实践
高翔 清华大学 2016年冬
2
第六讲 非线性优化
Chapter 6: Non-linear Optimization
3 第六讲 非线性优化
• 本讲目标
• 理解最小二乘法的含义和处理方式。 • 理解Gauss-Newton, Levenburg-Marquadt等下降策略。 • 学习Ceres库和g2o库的基本使用方法。
•该问题有何结构？
• 由许多个误差的平方和（或Sigma范数和）组成。 • 虽然总体维度高，但每个项很简单，只关联2个变量。 • 如果用李代数表达位姿，那么是无约束优化问题。
• 如何求解？
• 下面先来介绍通用的非线性最小二乘问题。
14
6.2 非线性最小二乘
15 6.2 非线性最小二乘
• 先考虑简单的问题：
• 令关于 导数为零：
21 6.2 非线性最小二乘
• G-N用J的表达式近似了H • 步骤：
22 6.2 非线性最小二乘
• Gauss-Newton简单实用，但
当中无法保证H可逆（二次近似不可靠）
• Levenberg-Marquadt 方法一定程度上改善了它
• G-N属于线搜索方法：先找到方向，再确定长度
这里
• 当 f 很简单时：
• 解：
将得到极值点或鞍点，比较这些解即可
，f 为任意函数
• 当 f 复杂时：
• df/dx难求，或df/dx=0很难解 • 使用迭代方式求解
16 6.2 非线性最小二乘
• 迭代方式：
问题：如何确定这个增量？
17 6.2 非线性最小二乘
注
• 确定增量的方法（即梯度下降策略）：一阶的或二阶的
12 6.1状态估计问题
• 我们把状态最大似然估计变成了最小二乘问题
• 对于原问题：
定义误差：
• 最小化误差的二范数：min
13 6.1状态估计问题
• 直观解释
• 由于噪声的存在，当我们把估计的轨迹与地图代入SLAM的运动、观测方程中时，它们 并不会完美的成立。
• 此时就调整状态的估计，使得误差最小化
29 5.3 实践：CERES
• 实验：曲线拟合 • 设曲线方程： • 我们得到一些带噪声的样本数据：x, y • 希望拟合（回归）曲线参数：a, b, c
7 6.1状态估计问题
• 历史上很长一段时间使用滤波器求解状态估计 • 但近年非线性优化已成为主流 • 我们首先来分析一下如何从概率角度看待此问题
8 6.1状态估计问题
• 状态变量（所有待求解的量）
• 状态估计等同于求解条件分布：
类似的，这里z,u也是统称
• 考虑更简单的情况：只有观测时，类似于Structure from Motion （SfM）
• 从增量方程上来看，可以看成一阶和二阶的混合
• 参数 控制着两边的权重
26 6.2 非线性最小二乘
• 小结
• 非线性优化是个很大的主题，研究者们为之奋斗多年 • 主要方法：最速下降、牛顿、G-N、L-M、DogLeg等 • 与线性规划不同，非线性需要针对具体问题具体分析 • 问题非凸时，对初值敏感，会陷入局部最优
• 从最大似然到最小二乘
• 例子：某次观测 • 由于噪声是高斯的： • 于是
• 现在要求x,y的最大似然估计，怎么求？
11 6.1状态估计问题
• 一般的高斯分布：
• 负对数形式：
• 原问题的最大化，相当于负对数最小化 • 因此，关于原问题的最大似然： • 相当于最小化：
最小化x时，只和它有关
这货就是所谓的最小二乘
• 贝叶斯法则：
9 6.1状态估计问题
• P(x|z) 条件分布很难求解，但可以求：
a) 最大后验估计（Maximize a Posterior，MAP）
b) 最大似然估计（Maximize Likelihood Estimation, MLE）
“在哪种状态下，最容易产生当前的观测”
10 6.1状态估计问题
• 最速下降法会碰到zigzag问题（过于贪婪） • 牛顿法迭代次数少，但需要计算复杂的Hessian矩阵
• 能否回避Hessian的计算？
• Gauss-Newton • Levenberg-Marquadt
20 6.2 非线性最小二乘
• Gauss-Newton
• 一阶近似 f(x)： • 平方误差变为：
意 这
• 泰勒展开：
里
是
对
平
• 若只保留一阶梯度：
方
展
增量的方向：
开
的
• 称为最速下降法（Steepest Method）
雅可比 海塞
（通常还需要计算步长）
18 6.2 非线性最小二乘
• 若保留二阶梯度：
• 则得到（令上式关于 的导数为零）： • 该方法又称为牛顿法
19 6.2 非线性最小二乘
• 最速下降法和牛顿法虽然直观，但实用当中存在一些缺点
椭球
24 6.2 非线性最小二乘
• Trust Region内的优化，利用Lagrange乘子转化为无约束：
• 仍参照G-N展开，增量方程为： • 在Levenberg方法中，取D=I，则：
25 6.2 非线性最小二乘
• LM相比于GN，能够保证增量方程的正定性
• 即，认为近似只在一定范围内成立，如果近似不好则缩小范围
• 目前没有非凸问题的通用最优值的寻找办法 • 问题凸时，二阶方法通常一两步就能收敛
27
5.3 实践：CERES
28 5.3 实践：CERES
• Google Ceres Solver
• 通用最小二乘问题求解库 • 最一般的形式（带边界的核函数最小二乘）
• f 在Ceres中称为代价函数（Cost Function），x称为参数块（Parameter Block）
• L-M属于信赖区域方法（Trust Region），认为近似只在区域内可靠
• 考虑近似程度的描述
实际下降/近似下降
• 若太小，则减小近似范围
• 若太大，则增加近似范围
23 6.2 非线性最小二乘
• 改进版的G-N：
这些都是经验值
Levenberg令D=I，即 取一个球，Marquardt 令D取非负对角阵，即