视觉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算法应用教程导语：随着人工智能技术的不断发展，机器人逐渐成为各个领域中不可或缺的工具。

机器人视觉导航是机器人实现自主移动和环境感知的关键技术之一。

而同样重要的是，SLAM（Simultaneous Localization and Mapping）算法的应用。

本篇文章将向大家介绍机器人视觉导航中的SLAM算法应用教程，帮助读者了解SLAM算法的原理和应用。

一、什么是SLAM算法SLAM，即同时定位与地图构建，是指在未知环境中，机器人实时地建立自身的地图，并通过自身感知的信息进行定位与路径规划。

SLAM算法是目前机器人自主导航和环境建模的基础。

SLAM算法的核心思想是通过传感器获取环境的感知信息，同时估计机器人自身的位置和姿态，从而实现对环境的建模和导航。

常用的传感器包括摄像头、激光雷达、超声波传感器等。

SLAM算法可以分为基于滤波和基于优化的方法。

滤波方法采用递推的方式在更新机器人位姿和地图，其中常用的滤波算法有扩展卡尔曼滤波（EKF）和粒子滤波（PF）。

优化方法则采用迭代优化的方式，通常使用最小二乘法进行参数优化，如图优化算法和非线性优化算法。

二、机器人视觉导航中SLAM算法的应用场景1. 室内导航机器人在未知的室内环境中实现自主移动和路径规划是目前SLAM算法的主要应用场景之一。

机器人通过摄像头或激光雷达等传感器获取环境的信息，通过SLAM算法实现自身位置和地图的实时更新，从而实现室内导航。

2. 建筑物巡检机器人用于建筑物的巡检和监控是SLAM算法的另一个重要应用场景。

机器人通过将建筑物的平面进行扫描并使用SLAM算法进行建图，可以实时检测建筑物的结构和安全问题，并提供状态反馈和报警。

3. 无人驾驶无人驾驶是SLAM算法的一个热门应用领域。

无人驾驶汽车通过激光雷达和摄像头等传感器获取道路和周围环境的信息，利用SLAM算法实时估计自身的位置和姿态，并规划行驶路径，从而实现自动驾驶。

SLAM_介绍以及浅析SLAM（Simultaneous Localization and Mapping），即同时定位与建图，是一种将移动机器人在未知环境中的位置定位与环境地图生成统一起来的技术。

SLAM技术是实现自主导航和智能导航的关键性技术之一，广泛应用于无人车、无人潜艇、无人机、机器人等领域。

SLAM技术分为前端和后端两部分。

前端主要负责机器人的位置定位，根据传感器获取的数据，通过运动估计（例如里程计模型）和感知估计（例如视觉、雷达感知）等方法，计算机器人在运动过程中的位置和姿态。

后端主要负责地图生成，根据机器人在不同时间点的位置估计和传感器获取的环境地图数据，利用优化算法估计机器人的位置和地图。

在前端中，常用的传感器有激光雷达、相机、惯性测量单元（IMU）等。

激光雷达可以提供高精度的距离和角度信息，常用于建立环境地图。

相机能够捕捉到图像信息，通过图像算法可以提取出环境中的特征点，用于定位和建图。

IMU能够提供线性加速度和角速度信息，用以估计机器人的运动。

在后端中，常用的算法有滤波器、优化方法和图优化等。

滤波器方法包括扩展卡尔曼滤波器（EKF）和无迹卡尔曼滤波器（UKF），通过状态估计和协方差矩阵来估计机器人的位置和姿态。

优化方法包括最小二乘法、非线性优化等，通过最小化误差函数来优化机器人的位置估计和地图。

图优化方法使用图模型来描述机器人的位置和环境地图，通过最大化后验概率来估计位置和地图。

SLAM技术的关键挑战之一是数据关联问题。

由于噪声和误差的存在，机器人在不同时刻获取的传感器数据可能不完全匹配。

因此，需要通过数据关联来确定当前获取的数据与之前数据的对应关系。

常用的数据关联方法有最近邻法、滤波法和图优化法等。

最近邻法通过计算不同数据之间的距离来确定对应关系。

滤波法通过滤波器来更新机器人的位置估计，并根据新的数据重新关联。

图优化法通过图模型来描述数据的关联关系，并通过最大后验概率来估计位置和地图。

《视觉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视觉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滤波算法的本质就是利⽤两个正态分布的融合仍是正态分布这⼀特性进⾏迭代⽽已。

步骤⼀：⽤上⼀次的最优状态估计和最优误差估计去计算这⼀次的先验状态估计和先验误差估计。