非線性優化-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！