非线性优化-SLAM14CP6

在前声明下面有一部分直接引用高翔老师SLAM14讲中的内容。由于我实在是看不懂。临时放在这里。之后有用到再作详细研究。

在SLAM14讲的CP2中第一次引入运动方程以及观测方程来描述物体带着传感器在空间中运动。能够先观察运动方程以及观测方程的形式。

第一个运动方程的输入包括上一次的位置Xk-一、运动传感器的读数（也可称为输入）uk、噪声wk。以及观测方程根据在xk位置上看到观测点yj产生一个观测数据zk，j。vk，j为在这次观测中的噪声。不难看出这个方程具备通用性，无论是什么传感器均可以表示这两个方程。方程中的位姿可用变换矩阵来表示，再用李代数来优化，在前面的CP4讲到。观测方程由相机成像方程给出，内参是相机固定的，外参是相机的位姿。在前面的CP5讲到。但这讲的重点不是这个方程而是其中的噪声。

在SLAM问题中同一个点每每被一个相机在不一样的时间内屡次观测，同一个相机在每一个时刻中观测到的点也不止一个。由于这些缘由，咱们具备了更多的约束，最终可以较好的从噪声数据中恢复出咱们须要的的东西。

1、状态估计问题

咱们能够讨论一下上面两个方程的具体参数化形式，首先位姿变量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的最大似然估计。

似然指的是"在当前的位姿下，可能产生怎样的观测数据"。因为咱们知道观测数据，因此最大似然估计，至关于"在什么样的状态下，最可能产生如今观测到的数据"。

2、最小二乘

 如何求最大似然估计？在高斯分布的状况之下，最大似然有比较简单的形式。咱们假设噪声项vk～N(0,Qk,j)因此观测数据的条件分布为

它依旧是一个高斯分布，为了计算它最大化的xk，yj，使用最小化负对数，求高斯分布的最大似然。

 

3、非线性最小二乘

上式是一最小二乘问题，这里的x属于Rⁿ f是一个任意非线性函数，咱们设它为m维；f(x)属于R^m 若是f是一个数学形式简单的函数，那么问题能够用解析形式来求，让目标函数的导数等于零，而后求解x的最优解。获得函数在导数为零的极值，只须要比较极大值、极小值、鞍部的函数值大小就行。那这个函数是否容易求解呢？取决于f导函数的形式。在SLAM中咱们采用李代数来表示旋转与位移，但依旧不可以十分容易的求解一个非线性方程。

对于不方便直接求解的最小二乘问题，能够用迭代的方法，从一个初始值开始不断更新变量，使其函数值降低。具体步骤以下：

1.给定某个初始值x

2.对于第k次迭代，寻找一个增量Δxk，使得||f(xk+Δx1)||² 达到极小值。

3.若Δxk足够小，就中止

4.不然，另xk+1=xk+Δxk，返回2

这样让求解导函数为零的过程，变成一个不断寻找梯度并降低的过程，直到没法再减少。此时函数收敛。这个过程只要寻到迭代点的梯度方向便可，无需寻找全局导函数为零的状况。（？多个极小值）

 

4、一阶和二阶梯度法

求解增量最直接的方法就是把目标函数在x附近进行泰勒展开：||f(x+Δx)||2≈||f(x)||²+J(x)Δx+1/2Δx^THΔx

这里的J是关于||f(x)||²关于x的导数（雅可比矩阵），而H则是二次导数（海塞矩阵）。咱们能够保留泰勒展开的一阶和二阶项，对应求解方法就是一阶梯度和二阶梯度法。

若是保留一阶梯度，那么增量的方向就是 Δx^*=-J^T(x).

他的意义很是简单，只要咱们沿着反向梯度的方向前进，咱们还须要该方向取一个步长λ，求最快的降低方式，这种就是最速降低法。

若是保留二阶梯度那么增量函数

求右侧等式关于Δx的导数并令它等于零，就获得增量的解 HΔx=-J^T

这种方法又称牛顿法。咱们看到一阶和二阶梯度法都很是直观，只要把函数在迭代点附近进行泰勒展开，而且更新量最小化。因为泰勒展开以后函数造成多项式，因而求解增量只需求解线性方程便可。

5、Gauss-Newton

这个方法是最优化算法中最简单的方法之一。它的思想就是将f(x)进行一阶泰勒展开（注意不是目标函数f(x)²）

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用J^TJ做为牛顿法中二阶海塞矩阵，从而忽略了H的过程。求解增量方程是整个优化问题的核心所在。若是咱们能解出方程，那么算法步骤以下：

1.给定初始值x0

2.对于第k次迭代，求出当前的雅可比矩阵J（xk）和偏差f（xk）。

3.求解增量方程

4.若Δxk足够小，则中止，不然xk+1=xk-Δxk，返回2

原则上，增量方程要求咱们所用的近似H矩阵是可逆的（并且是正定的），但实际的J^TJ只有半正定性（？）。也就是，在使用该方法，可能出现H为奇异矩阵或病态的状况，此时增量稳定性差，算法不收敛。更严重，就算咱们的H非病态和非奇异，若是咱们的步长太大，致使咱们局部不够精确，这样咱们不但不能保证它迭代收敛，甚至可能会让目标函数更大。

尽管Gauss-Newton法有这些肯定，但依旧值得学习，由于有至关多的算法都是它的变种。

 6、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取为为负数对角阵，实际中一般用J^TJ的对角元素平方根，使得在梯度小的维度约束范围更大。

（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等分解方法。

接下来就是代码部分，书中的代码我会实验而且增长一些知识点来理解它，固然书中的讲解也是很是充分的。但可能须要一些基础。

实践1、Ceres

Ceres库主要是面对通用的最小二乘的问题，咱们须要定义优化问题，设置选项输入求解便可。Ceres求解的问题最通常的形式以下（带边界的核函数最小二乘）：

目标函数有许多平方项，通过一个核函数ρ（）以后，求和组成。最简单的状况ρ取恒等函数。这个问题中，优化变量x1……xn，fi称为代价函数，在SLAM中亦可理解成偏差项。lj uj为第j个优化变量的上下限。最简单的状况下上下限取无穷，而且核函数取恒等函数，就获得无约束的最小二乘。

下面的演示，假设有一条知足y=exp（ax²+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})

View Code

实践一的主要是安装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;
}

View Code

代码部分的话主要说一些我不太知道的，以及关键的，通过查阅的内容。

首先就是

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

实践2、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)

View Code

这里使用的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;
}

View Code

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！