非线性优化-SLAM14CP6
在前声明下面有一部分直接引用高翔老师SLAM14讲中的内容。因为我实在是看不懂。临时放在这里。以后有用到再做详细研究。
在SLAM14讲的CP2中第一次引入运动方程以及观测方程来描述物体带着传感器在空间中运动。可以先观察运动方程以及观测方程的形式。
第一个运动方程的输入包括上一次的位置Xk-1、运动传感器的读数（也可称为输入）uk、噪声wk。以及观测方程根据在xk位置上看到观测点yj产生一个观测数据zk，j。vk，j为在此次观测中的噪声。不难看出这个方程具有通用性，不管是什么传感器都可以表示这两个方程。方程中的位姿可用变换矩阵来表示，再用李代数来优化，在前面的CP4讲到。观测方程由相机成像方程给出，内参是相机固定的，外参是相机的位姿。在前面的CP5讲到。但这讲的重点不是这个方程而是其中的噪声。
在SLAM问题中同一个点往往被一个相机在不同的时间内多次观测，同一个相机在每个时刻中观测到的点也不止一个。因为这些原因，我们具有了更多的约束，最终能够较好的从噪声数据中恢复出我们需要的的东西。
一、状态估计问题
我们可以讨论一下上面两个方程的具体参数化形式，首先位姿变量xk可以用Tk或者exp(ξ^k)表示。由于运动方程在SLAM没有特殊性，我们先看观测方程。可以表示为
K为相机内参，s为像素点的距离，同时这里的zk，j和yj要用齐次坐标来描述，且中间有一次齐次到非齐次的变换。考虑数据受噪声的影响后会有什么样的变化，通常假设两个噪声项wk，vk，j满足高斯分布。
在这些噪声的影响下，我们希望能够通过带噪声的数据z和u，推断位姿x和地图y（以及他们的分布概率），这就构成了一个状态估计问题。在SLAM过程中，这些数据是通过时间逐渐到来的，使用滤波器来求解是一种早期的方法，其中扩展卡尔曼滤波器（EKF）进行求解是一种方法，但是EKF关心当前时刻的状态估计xk，不考虑之前的状态。现在更多普遍使用非线性优化方式，使用所有时刻的采集到的数据进行状态估计。
在非线性优化中，将所有的待估计变量放入一个"状态变量"中：x={x1,...,xN,y1,...,yM}.
现在对机器人状态的估计，就是求已知输入数据u和观测数据z的情况下，计算状态x的条件概率分布P(x|z,u).类似于x这里的z，u同样是所有数据的统称。特别的，当我们没有检测运动的传感器，只有一张张图像时，即只考虑观测方程的数据，相当于估计P(x|x)的条件概率分布。如果忽略图像在时间上的联系，把他们看作一堆彼此没有关系的图片，该问题为SfM，即如何通过从许多图像中重建三维空间结构。而SLAM问题可以把图像看作有时间先后顺序，需要实时求解的SfM问题。我们需要利用贝叶斯公式来估计状态变量的条件分布。
贝叶斯法则的左侧通常称为后验概率。右侧P(z|x)称为似然，P(x)为先验。直接求后验概率是困难的，但求一个状态最优估计使得在该状态下后验概率最大化（MAP），则是可行的。
因为分母与x无关所以直接忽略，贝叶斯法则告诉我们，求最大后验概率，相当于最大似然和先验的乘积。接着当我们不知道机器人的位姿大概在什么地方，此时就没有了先验，那么就能求解x的最大似然估计。
似然指的是"在当前的位姿下，可能产生怎样的观测数据"。由于我们知道观测数据，所以最大似然估计，相当于"在什么样的状态下，最可能产生现在观测到的数据"。
二、最小二乘
如何求最大似然估计？在高斯分布的情况之下，最大似然有比较简单的形式。我们假设噪声项vk～N(0,Qk,j)所以观测数据的条件分布为
它依旧是一个高斯分布，为了计算它最大化的xk，yj，使用最小化负对数，求高斯分布的最大似然。
三、非线性最小二乘
上式是一最小二乘问题，这里的x属于Rn f是一个任意非线性函数，我们设它为m维；f(x)属于Rm 如果f是一个数学形式简单的函数，那么问题可以用解析形式来求，让目标函数的导数等于零，然后求解x的最优解。得到函数在导数为零的极值，只需要比较极大值、极小值、鞍部的函数值大小就行。那这个函数是否容易求解呢？取决于f导函数的形式。在SLAM中我们采用李代数来表示旋转与位移，但依旧不能够十分容易的求解一个非线性方程。
对于不方便直接求解的最小二乘问题，可以用迭代的方法，从一个初始值开始不断更新变量，使其函数值下降。具体步骤如下：
1.给定某个初始值x
2.对于第k次迭代，寻找一个增量Δxk，使得||f(xk+Δx1)||2 达到极小值。
3.若Δxk足够小，就停止
4.否则，另xk+1=xk+Δxk，返回2
这样让求解导函数为零的过程，变成一个不断寻找梯度并下降的过程，直到无法再减小。此时函数收敛。这个过程只要寻到迭代点的梯度方向即可，无需寻找全局导函数为零的情况。（？多个极小值）
四、一阶和二阶梯度法
求解增量最直接的方法就是把目标函数在x附近进行泰勒展开：||f(x+Δx)||2≈||f(x)||2+J(x)Δx+1/2ΔxTHΔx
这里的J是关于||f(x)||2关于x的导数（雅可比矩阵），而H则是二次导数（海塞矩阵）。我们可以保留泰勒展开的一阶和二阶项，对应求解方法就是一阶梯度和二阶梯度法。
如果保留一阶梯度，那么增量的方向就是 Δx*=-JT(x).
他的意义非常简单，只要我们沿着反向梯度的方向前进，我们还需要该方向取一个步长λ，求最快的下降方式，这种就是最速下降法。
如果保留二阶梯度那么增量函数
求右侧等式关于Δx的导数并令它等于零，就得到增量的解 HΔx=-JT
这种方法又称牛顿法。我们看到一阶和二阶梯度法都非常直观，只要把函数在迭代点附近进行泰勒展开，并且更新量最小化。由于泰勒展开之后函数形成多项式，于是求解增量只需求解线性方程即可。
五、Gauss-Newton
这个方法是最优化算法中最简单的方法之一。它的思想就是将f(x)进行一阶泰勒展开（注意不是目标函数f(x)2）
f(x+Δx)≈f(x)+J(x)Δx
这里的J（x）是f（x）关于x的导数实际上是一个m*n的矩阵，也是一个雅可比矩阵。为了求解Δx，我们需要求解一个线性最小二乘问题。
注意，我们要求解的变量是Δx，因此这是一个线性方程组，我们称之为增量方程，也是高斯牛顿方程，或是正规方程。我们把左式定义为H，右边定义为g，上式变成HΔx=g。
这里把左侧记作H是有意义的，对比牛顿法，Gauss-Newton用JTJ作为牛顿法中二阶海塞矩阵，从而忽略了H的过程。求解增量方程是整个优化问题的核心所在。如果我们能解出方程，那么算法步骤如下：
1.给定初始值x0
2.对于第k次迭代，求出当前的雅可比矩阵J（xk）和误差f（xk）。
3.求解增量方程
4.若Δxk足够小，则停止，否则xk+1=xk-Δxk，返回2
原则上，增量方程要求我们所用的近似H矩阵是可逆的（而且是正定的），但实际的JTJ只有半正定性（？）。也就是，在使用该方法，可能出现H为奇异矩阵或病态的情况，此时增量稳定性差，算法不收敛。更严重，就算我们的H非病态和非奇异，如果我们的步长太大，导致我们局部不够精确，这样我们不但不能保证它迭代收敛，甚至可能会让目标函数更大。
尽管Gauss-Newton法有这些确定，但依旧值得学习，因为有相当多的算法都是它的变种。
六、Levenberg-Marquadt
L-M方法比G-N方法更加鲁棒，尽管它的收敛比G-N慢，被称为阻尼牛顿法。
由于G-N方法采用近似二阶泰勒展开只能在展开点附近有比较好的近似效果，所以我们可以给Δx添加一个信赖区域。非线性优化具有一系列这种方法，可被称为信赖区域方法。在信赖区域里面我们认为近似是有效的，反之则可能出现问题。
那么怎么确定信赖区域的范围呢？一个比较好的方法就是根据我们近似模型跟实际函数直接的差距来确定这个范围，如果差异小，就让范围尽可能大，反之则缩小这个范围。
使用上式来判断近似是不是够好，ρ的分子是函数下降的值，如果接近1，则近似是好的，如果太小，说明实际减小的值远少于近似减小的值，则认为近似比较差，需要缩小范围，反之则扩大。
下面是一个改良版非线性优化框架：
1。给定初始值x0，以及初始优化半径μ
2.对于第k次迭代求
3.计算ρ
4.若ρ>3/4 则μ=2μ
4.若ρ<1/4 则μ=0.5μ
5.如果ρ大于某阈值，认为近似可行。令xk+1=xk+Δxk
6.判断算法是否收敛，不收敛返回2.
这里近似范围扩大倍数和阈值都是经验值，可以替换，在上式中我们把增量限定于一个半径为μ的球中，认为在球中是有效的，带上D后这个球可以看成一个椭球。在L提出的优化方法中，把D取成单位阵I，相当于直接把Δx约束在一个球里。随后M提出将D取为为负数对角阵，实际中通常用JTJ的对角元素平方根，使得在梯度小的维度约束范围更大。
（H+λI）Δx=g
当参数λ比较小，H占主要地位，说明二次近似模型在该范围内比较好，L-M近似于G-N。如果λ比较大，L-M更接近于一阶梯度下降法，说明二次近似模型不够好。这样L-M可以一定程度上避免线性方程组的系数矩阵的非奇异和病态问题。
---小结
总之非线性优化问题的框架分为LineSearch和TrustRegion。前者先固定搜索方向，然后再方向寻找步长，以最速下降法和G-N法。后者先固定搜索区域，然后找到最优点。以L-M为代表。
无论是G-N还是L-M，在做最优计算的时候，都需要提供变量的初始值。实际上非线性优化的所有迭代求解方案，都需要用户提供一个较好的初始值。由于目标函数太复杂，导致求解空间上难以琢磨，对于问题提供给不同的初始值往往得到不同的计算过程。这种情况是非线性优化的通病：大多数算法都容易陷入局部极小值，因此，无论是哪类问题，我们提供初始值都需要有科学依据。
如何求解线性增量方程？存在着许多针对线性方程组的数值求解方法。不同领域采用不同的方法，但几乎没有之中直接求系数矩阵的逆，我们采用矩阵分解的方法来求解，比如QR、Cholesky等分解方法。
接下来就是代码部分，书中的代码我会实验并且增加一些知识点来理解它，当然书中的讲解也是非常充分的。但可能需要一些基础。
实践一、Ceres
Ceres库主要是面对通用的最小二乘的问题，我们需要定义优化问题，设置选项输入求解即可。Ceres求解的问题最一般的形式如下（带边界的核函数最小二乘）：
目标函数有许多平方项，经过一个核函数ρ（）之后，求和组成。最简单的情况ρ取恒等函数。这个问题中，优化变量x1……xn，fi称为代价函数，在SLAM中亦可理解成误差项。lj uj为第j个优化变量的上下限。最简单的情况下上下限取无穷，并且核函数取恒等函数，就得到无约束的最小二乘。
下面的演示，假设有一条满足y=exp（ax2+bx+c）+w方程的曲线。
abc为曲线的参数，w为高斯噪声。我们假设有N点x，y观测数据点，想根据这些点来求出曲线的参数。那么，可以求解以下最小二乘的问题以估计曲线参数：
不得不说，写这个实践的过程还是十分痛苦的，因为一些版本问题，导致之前的代码不能直接使用，包括CMakeLists的编写也是有点困难。
cmake_minimum_required(VERSION 2.8) project(ceres_curve_fitting) set(CMAKE_CXX_FLAGS "-std=c++14 -O3") find_package(OpenCV REQUIRED) include_directories(${OpenCV_INCLUDE_DIRS}) find_package(Ceres REQUIRED) include_directories(${CERE_INCLUDE_DIRS}) add_executable(ceres_curve_fitting main.cpp) target_link_libraries(ceres_curve_fitting ${OpenCV_LIBS} ${CERES_LIBRARIES})
实践一的主要是安装Ceres然后倒入包就行了。
#include<iostream> #include<opencv2/core/core.hpp> #include<ceres/ceres.h> #include<chrono> using namespace std; struct CURVE_FITTING_COST{ CURVE_FITTING_COST(double x, double y): _x(x), _y(y){} template<typename T> bool operator() (const T* const abc, T* residual) const{ residual[0]= T(_y) - ceres::exp(abc[0]*T(_x) *T(_x) + abc[1]*T(_x) + abc[2]); return true; } const double _x,_y; }; int main(int argc, char** argv){ double a=1.0, b=2.0, c=1.0; int N=100; double w_sigma=1.0; cv::RNG rng; double abc[3]={0,0,0}; vector<double> x_data, y_data; cout<<"generating data:"<<endl; for(int i=0; i<N; i++){ double x=i/100.0; x_data.push_back(x); y_data.push_back(exp(a*x*x+b*x+c) + rng.gaussian(w_sigma)); cout<<x_data[i]<<" "<<y_data[i]<<endl; } ceres::Problem problem; for(int i=0; i<N;i++){ problem.AddResidualBlock(new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(new CURVE_FITTING_COST(x_data[i], y_data[i])), nullptr, abc); } ceres::Solver::Options options; options.linear_solver_type=ceres::DENSE_QR; options.minimizer_progress_to_stdout=true; ceres::Solver::Summary summary; chrono::steady_clock::time_point t1=chrono::steady_clock::now(); ceres::Solve( options, &problem, &summary); chrono::steady_clock::time_point t2=chrono::steady_clock::now(); chrono::duration<double> time_used=chrono::duration_cast<chrono::duration<double>>(t2-t1); cout<<"solve time const="<<time_used.count()<<"seconds."<<endl; cout<<summary.BriefReport()<<endl; cout<<"estimate a,b,c="; for(auto a:abc) cout<<a<<" "; cout<<endl; return 0; }
代码部分的话主要说一些我不太知道的，以及关键的，经过查阅的内容。
首先就是
struct CURVE_FITTING_COST{ CURVE_FITTING_COST(double x, double y): _x(x), _y(y){} template<typename T> bool operator() (const T* const abc, T* residual) const{ residual[0]= T(_y) - ceres::exp(abc[0]*T(_x) *T(_x) + abc[1]*T(_x) + abc[2]); return true; } const double _x,_y; }; problem.AddResidualBlock(new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(new CURVE_FITTING_COST(x_data[i], y_data[i])), nullptr, abc);
这个是代码中定义CURVE_FITTING_COST的以及使用的过程，首先这里是一个拟函数Functor。有什么用呢，就是我们可以看到AutoDiffCostFunction函数只接受一个参数，但是我们这个结构体同时需要x，y才能构建它，于是就用到拟函数，通过这个模板类的构建方法，我认为是先将它实例化输入x，y初始值，然后再带到对应的函数中，相当于是需要重载（）运算符。
然后就是AddResidualBlock这个函数，将误差项添加到目标函数，因为优化需要梯度，所以有几种选项1.自动求导，就是我们用的这种 2.使用数值求导 3.自行推倒解析导数形式。而自动求导AutoDiffCostFunction需要指定误差项以及优化变量，这里的误差项是一维的，所以就是1，优化量就是abc，所以就是3.
数据处理完之后就可以用Solve进行求解。通过Option进行设置使用linearSearch还是TrustRegion。
以下是对应的结构还是很接近我们设置的abc的。a=1，b=2，c=1
estimate model=0.890912 2.1719 0.943629
实践二、g2o
它是基于图优化的库，图优化是一种把非线性优化和图论结合起来的理论。我们先需要知道一些图优化理论。
我门已经介绍了非线性最小二乘的求解方法。他们是由许多个误差项之和组成的。然而仅有一组优化变量和有许多个误差项，我们不清楚他们之间的关联。我们希望能够看到优化问题长什么样。
图优化，是把优化问题表现成图的一种方式。这里的图是图论意义上的图，由多个顶点以及连接着顶点的边组成，用点来表示优化变量，用边来表示误差项。
我们用三角形表示相机位姿节点，圆形表示路标点，他们就是图优化的顶点，同时蓝色的的线表示运动模型，红色的虚线表示观测模型，构成了图优化的边。虽然还是我们之前说的观测方程以及运动方程，但我们更够看到问题的结构，我们可以去掉孤立的点活优先优化边数较多（度数较大，图论中的术语）的顶点这样的优化。
在上面实践一的内容语境下，我们这里的优化变量abc就可以作为图优化的顶点，而带噪声的数据点，就构成了一个个误差项也就是对应的边。
但是这里的边是一元边，即指链接一个顶点的，因为我们的图只有一个顶点。实际上图优化一条边可以连接多个顶点也可以是一个，主要反映在每个误差与多少个优化变量有关。我们把它叫做超边，整个图叫做超图。
我们主要做的事情分为以下几步：
1.定义顶点和边的类型
2.构建图
3.选择优化算法
4.调用g2o进行优化，返回结果。
一样先把对应的源码和配置放在这里。和原书上的不太一样，因为版本问题，都可以百度得到。说不定以后又不适用了。泪目。
cmake_minimum_required(VERSION 2.8) project(g2o_curve_fitting) set(CMAKE_BUILD_TYPE Release) set(CMAKE_CXX_FLAGS "-std=c++14 -O3") list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules ) find_package(OpenCV REQUIRED) include_directories(${OpenCV_INCLUDE_DIRS}) find_package(G2O REQUIRED) include_directories(${G2O_INCLUDE_DIRS}) add_executable(g2o_curve_fitting main.cpp) target_link_libraries(g2o_curve_fitting ${OpenCV_LIBS} g2o_core g2o_stuff)
这里使用的CMakeLists的方法需要将g2o文件夹下面的cmake_modules文件夹放到与build文件夹同一目录下。
#include<iostream> #include<g2o/core/base_vertex.h> #include<g2o/core/base_unary_edge.h> #include<g2o/core/block_solver.h> #include<g2o/core/optimization_algorithm_levenberg.h> #include<g2o/core/optimization_algorithm_gauss_newton.h> #include<g2o/core/optimization_algorithm_dogleg.h> #include<g2o/solvers/dense/linear_solver_dense.h> #include<Eigen/Core> #include<opencv2/core/core.hpp> #include<cmath> #include<chrono> using namespace std; class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW virtual void setToOriginImpl() { _estimate<< 0,0,0; } virtual void oplusImpl(const double* update) { _estimate+=Eigen::Vector3d(update); } virtual bool read(istream& in) {} virtual bool write(ostream& out) const {} }; class CurveFittingEdge:public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW CurveFittingEdge(double x):BaseUnaryEdge(), _x(x) {} void computeError(){ const CurveFittingVertex* v=static_cast<const CurveFittingVertex*>(_vertices[0]); const Eigen::Vector3d abc = v->estimate(); _error(0,0) = _measurement - std::exp(abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0)); } virtual bool read(istream& in) {} virtual bool write(ostream& out) const {} public: double _x; }; int main(int argc, char** argv) { double a=1.0, b=2.0, c=1.0; int N=100; double w_sigma=1.0; cv::RNG rng; double abc[3]={0,0,0}; vector<double> x_data, y_data; cout<<"generate data:"<<endl; for(int i=0; i<N; i++){ double x= i/100.0; x_data.push_back(x); y_data.push_back(exp(a*x*x+b*x+c)+rng.gaussian(w_sigma)); cout<<x_data[i]<<" "<<y_data[i]<<endl; } typedef g2o::BlockSolver<g2o::BlockSolverTraits<3,1>> Block; std::unique_ptr<Block::LinearSolverType> linearSolver(new g2o::LinearSolverDense<Block::PoseMatrixType>()); std::unique_ptr<Block> solver_ptr(new Block(std::move(linearSolver))); g2o::OptimizationAlgorithmLevenberg* solver=new g2o::OptimizationAlgorithmLevenberg(std::move(solver_ptr)); //g2o::OptimizationAlgorithmGaussNewton* solver= new g2o::OptimizationAlgorithmGaussNewton(std::move(solver_ptr)); //g2o::OptimizationAlgorithmDogleg* solver= new g2o::OptimizationAlgorithmDogleg(std::move(solver_ptr)); g2o::SparseOptimizer optimizer; optimizer.setAlgorithm(solver); optimizer.setVerbose(true); CurveFittingVertex* v=new CurveFittingVertex(); v->setEstimate(Eigen::Vector3d(0,0,0)); v->setId(0); optimizer.addVertex(v); for(int i=0; i<N; i++){ CurveFittingEdge* edge = new CurveFittingEdge(x_data[i]); edge->setId(i); edge->setVertex(0,v); edge->setMeasurement(y_data[i]); edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity()*1/(w_sigma*w_sigma)); optimizer.addEdge(edge); } cout<<"start optimization"<<endl; chrono::steady_clock::time_point t1=chrono::steady_clock::now(); optimizer.initializeOptimization(); optimizer.optimize(100); chrono::steady_clock::time_point t2=chrono::steady_clock::now(); chrono::duration<double> time_used =chrono::duration_cast<chrono::duration<double>> (t2-t1); cout<<"solve time cost="<<time_used.count()<<"seconds."<<endl; Eigen::Vector3d abc_estimate = v->estimate(); cout<<"estimate model="<<abc_estimate.transpose()<<endl; return 0; }
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW virtual void setToOriginImpl() { _estimate<< 0,0,0; } virtual void oplusImpl(const double* update) { _estimate+=Eigen::Vector3d(update); } virtual bool read(istream& in) {} virtual bool write(ostream& out) const {} }; class CurveFittingEdge:public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW CurveFittingEdge(double x):BaseUnaryEdge(), _x(x) {} void computeError(){ const CurveFittingVertex* v=static_cast<const CurveFittingVertex*>(_vertices[0]); const Eigen::Vector3d abc = v->estimate(); _error(0,0) = _measurement - std::exp(abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0)); } virtual bool read(istream& in) {} virtual bool write(ostream& out) const {} public: double _x; };
可以看到，这里首先是构建了边以及对应的点的类型，方便后面使用，实质上扩展了g2o的使用方法。
主要是重写了oplusImpl（！！！一定要写对）顶点的更新函数，我们知道要去计算Δx，而对应的函数就要进行顶点偏移的操作。我们需要重新定义这个过程的主要原因是当我们这个优化变量abc不处于向量空间中，比如说相机位姿，他不一定具有加法运算，就需要重新定义增量如何加到现有的估计上。就要用左乘或右乘。
setToOriginImpl顶点的重设函数。computeError是边的误差计算，该函数需要提取边与所连线的顶点估计当前估计值，根据曲线模型与观测点进行比较，和最小二乘问题一致。
这里提供了三种方法。
L-M: estimate model=0.890912 2.1719 0.943629
G-M: estimate model=0.890911 2.1719 0.943629
Dogleg: estimate model=0.890911 2.1719 0.943629
差距不大，可能要面对不同的问题才有结果。
总算是写完啦。还需要继续加油。感谢您看到这里。Cheers！
