下载文档原格式
视觉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）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⼗四讲》课后习题—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相差不多。
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 中，相机通常是提前标定好的。
张正友于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使⽤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;// ⼀些矩阵运算// 四则运算就不演⽰了，直接⽤+-*/即可。
【转】SLAM视觉SLAM中的后端：后端优化算法与建图模板前⾯的话前⾯系列⼀中我们介绍了，VSLAM 是利⽤多视图⼏何理论，根据相机拍摄的图像信息对相机进⾏定位并同时构建周围环境地图。
按照相机的分类，有单⽬、双⽬、 RGBD、鱼眼、全景等。
同时，VSLAM 主要包括视觉⾥程计（visual odometry， VO）、后端优化、回环检测、建图。
VSLAM 前端为视觉⾥程计和回环检测，相当于是对图像数据进⾏关联；后端是对前端输出的结果进⾏优化，利⽤滤波或⾮线性优化理论，得到最优的位姿估计和全局⼀致性地图。
前⾯已经介绍了VSLAM的前端：视觉⾥程计和回环检测，这次我们将介绍系列⼆：VSLAM中的后端优化和建图。
接下来，我们将详细介绍。
2 后端：最优化位姿估计和全局⼀致性地图2.1 后端优化SLAM 的后端求解⽅法可⼤致分为两⼤类，⼀类是基于滤波器的⽅法；另⼀类则是⾮线性优化⽅法。
这是根据假设的不同，如果假设马尔可夫性， K 时刻状态只与 K-1 时刻状态有关，⽽与之前的状态⽆关，这样会得到以扩展卡尔曼滤波（EKF）为代表的滤波器⽅法。
在滤波⽅法中，本⽂会从某时刻的状态估计推导到下⼀个时刻。
另外⼀种⽅法是考虑K 时刻与之前所有状态的关系，这将得到⾮线性优化为主体的优化框架。
2.1.1 滤波⽅法由于SLAM 本质上是⼀个状态估计问题，该问题可以归结为⼀个运动⽅程和⼀个观测⽅程，顺理成章地把 SLAM 融⼊到滤波框架中。
早期的SLAM 研究基本都是在滤波器的框架下。
在假定从 0 到 t 时刻的观测信息以及控制信息已知的条件下，对系统状态的后验概率进⾏估计，根据后验概率表⽰⽅式的不同，存在多种基于滤波器的⽅法，如扩展卡尔曼滤波（EKF）⽅法、粒⼦滤波（PF）等。
1．卡尔曼滤波（KF）Kalman滤波算法的本质就是利⽤两个正态分布的融合仍是正态分布这⼀特性进⾏迭代⽽已。
步骤⼀：⽤上⼀次的最优状态估计和最优误差估计去计算这⼀次的先验状态估计和先验误差估计。
视觉SLAM⼗四讲——LK光流主要内容 直接法是从光流演变⽽来的。
光流（Optical Flow）描述了像素在图像中的运动，⽽直接法则附带着⼀个相机的运动模型。
1. 定义 描述像素随时间在像素中运动的⽅法，追踪像素在图像中运动。
追踪部分像素或全部像素分为稀疏光流和稠密光流，稀疏光流以Lucas-Kanade光流为代表，可以在SLAM中跟踪特征点的位置。
（避免随时间在两两图像之间进⾏特征点提取匹配，减少计算量，光流本省匹配需要⼀定的时间，跟踪以后可以直接⽤像素坐标信息，使⽤对极⼏何、PnP和ICP算法估计相机运动）2. LK光流 [P186-187] 1）灰度不变假设：同⼀空间点的像素灰度值，在各个图像中是固定不变的。
2) 理论公式：（两个变量的⼀次⽅程） 分别表⽰：在像素点x⽅向的梯度，像素点在x⽅向的运动速度，在像素点y⽅向的梯度，像素点在y⽅向上的梯度，像素点灰度随时间的变化量。
3）在LK光流中，我们假设某⼀个窗⼝内的像素具有相同的运动。
窗⼝⼤⼩w*w，则共有w2个⽅程。
最⼩⼆乘求解（由于像素梯度仅在局部有效，所以如果⼀次迭代不够好，可以多迭代⼏次这个⽅程）3. 光流特点 1）⾓点具有更好的辨识度，边缘次之，区块最少 2）光流法本⾝计算需要⼀定的时间，在具体SLAM系统的应⽤，根据测试进⾏选择 3）LK光流跟踪能够直接得到特征点的对应关系，实际应⽤会出现特征点跟丢的情况，不太会遇到误匹配（相对于描述⼦的⼀点优势） 4）描述⼦匹配在相机运动较⼤时仍能成功，光流必须要求相机运动是微⼩的（采集频率较⾼）。
光流的健壮性相⽐描述⼦差。
4. 代码关键点 1）根据associate.txt匹配时间点的信息，读取图像及深度信息， 2）遍历图像，第⼀幅图像提取FAST的特征点，后继续的图像利⽤光流跟踪这些特征点，删除跟踪失败的特征点 3）光流法函数调⽤： cv::calcOpticalFlowPyrLK （包括参数的调⽤，返回的结果信息等） 4）每⼀帧图像跟踪出来的特征点进⾏显⽰ 5）注意光流跟踪的点数以及光流计算耗费的时间信息参考链接代码#include <iostream>#include <fstream>#include <list>#include <vector>#include <chrono>using namespace std;#include <opencv2/core/core.hpp>#include <opencv2/highgui/highgui.hpp>#include <opencv2/features2d/features2d.hpp>#include <opencv2/video/tracking.hpp>int main( int argc, char** argv ){if ( argc != 2 ){cout<<"usage: useLK path_to_dataset"<<endl;return1;}string path_to_dataset = argv[1];string associate_file = path_to_dataset + "/associate.txt";ifstream fin( associate_file );if ( !fin ){cerr<<"I cann't find associate.txt!"<<endl;return1;}string rgb_file, depth_file, time_rgb, time_depth;list< cv::Point2f > keypoints; // 因为要删除跟踪失败的点，使⽤listcv::Mat color, depth, last_color;for ( int index=0; index<100; index++ ){fin>>time_rgb>>rgb_file>>time_depth>>depth_file;color = cv::imread( path_to_dataset+"/"+rgb_file );depth = cv::imread( path_to_dataset+"/"+depth_file, -1 );if (index ==0 ){// 对第⼀帧提取FAST特征点vector<cv::KeyPoint> kps;cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();detector->detect( color, kps );for ( auto kp:kps )keypoints.push_back( kp.pt );last_color = color;continue;}if ( color.data==nullptr || depth.data==nullptr )continue;// 对其他帧⽤LK跟踪特征点vector<cv::Point2f> next_keypoints;vector<cv::Point2f> prev_keypoints;for ( auto kp:keypoints )prev_keypoints.push_back(kp);vector<unsigned char> status;vector<float> error;chrono::steady_clock::time_point t1 = chrono::steady_clock::now();cv::calcOpticalFlowPyrLK( last_color, color, prev_keypoints, next_keypoints, status, error );chrono::steady_clock::time_point t2 = chrono::steady_clock::now();chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 ); cout<<"LK Flow use time："<<time_used.count()<<" seconds."<<endl;// 把跟丢的点删掉int i=0;for ( auto iter=keypoints.begin(); iter!=keypoints.end(); i++){if ( status[i] == 0 ){iter = keypoints.erase(iter);continue;}*iter = next_keypoints[i];iter++;}cout<<"tracked keypoints: "<<keypoints.size()<<endl;if (keypoints.size() == 0){cout<<"all keypoints are lost."<<endl;break;}// 画出 keypointscv::Mat img_show = color.clone();for ( auto kp:keypoints )cv::circle(img_show, kp, 10, cv::Scalar(0, 240, 0), 1);cv::imshow("corners", img_show);cv::waitKey(0);last_color = color;}return0;}结果及分析LK Flow use time：0.186846 seconds. //t1 ,对应下⾯的第⼀幅图tracked keypoints: 1749LK Flow use time：0.189048 seconds.tracked keypoints: 1742LK Flow use time：0.232017 seconds.tracked keypoints: 1703LK Flow use time：0.17844 seconds.tracked keypoints: 1676LK Flow use time：0.234777 seconds.tracked keypoints: 1664LK Flow use time：0.185145 seconds.tracked keypoints: 1656LK Flow use time：0.186722 seconds.tracked keypoints: 1641LK Flow use time：0.177791 seconds. // t2,对应下⾯的第⼆幅图tracked keypoints: 1634 结果分析： 1）根据输出结果，随着时间，相机姿态发⽣了⽐较⼤的旋转变化，跟踪的特征点数也从1749点下降到1634点； 2）从图中可以看出，随着图像视⾓的旋转，有些特征消失，新进来的特征点⽆法跟踪（可能需要对新进来的该部分特征重新进⾏提取，不然随着⾓度更⼤的时候，特征点可能完全丢失，⽆法进⾏位姿的解算）。
《视觉SLAM十四讲》第六讲g2o实践代码报错解决方法https:///xueyuanaichiyu/p/7921382.html 问题：首先贴出报错部分代码：typedef g2o::BlockSolver g2o::BlockSolverTraits3,1> > Block;Block::LinearSolverType* linearSolver = newg2o::LinearSolverDense::PoseMatrixType>();Block* solver_ptr = new Block( linearSolver );g2o::OptimizationAlgorithmLevenberg* solver = newg2o::OptimizationAlgorithmLevenberg( solver_ptr );g2o::SparseOptimizer optimizer;optimizer.setAlgorithm( solver );optimizer.setVerbose( true );按照书上的例程编写代码，编译时报错（部分截图）如下：/home/wxy/slambook/useg2o/main.cpp:77:49: error: no matchingfunction for call to ‘g2o::BlockSolver::BlockSolverTraits3, 1> >::BlockSolver(g2o::BlockSolver::BlockSolverTraits3,1> >::LinearSolverType*&)’Block* solver_ptr = new Block( linearSolver );^/home/wxy/slambook/useg2o/main.cpp:77:49: note: candidate is:In file included from/usr/local/include/g2o/core/block_solver.h:199:0,from/home/wxy/slambook/useg2o/main.cpp:4:/usr/local/include/g2o/core/block_solver.hpp:40:1: note:g2o::BlockSolver::BlockSolver(std::unique_ptrTraits::LinearSolverType>) [with Traits =g2o::BlockSolverTraits3, 1>; typenameTraits::LinearSolverType = g2o::LinearSolver::Matrix, 3, 3> >]BlockSolver::BlockSolver(std::unique_ptr linearSolver)好像是编译器无法找到某个函数，或者某个调用出错。
视觉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的初始化，后⾯的轨迹和地图以此单位进⾏计算。
