視覺slam學習之——ch6 非線性曲線擬合（centos系統）

非線性曲線擬合，高博士給的demo主要用谷歌ceres庫實現，高斯牛頓方式實現，g2o庫實現，這三個程序例子。

---

一.首先介紹ceres庫安裝與實現

ceres庫是谷歌開發的C++庫，用於建模和解決複雜的優化問題的。能用於解決非線性最小二乘問題。ceres介紹

ceres的官方安裝鏈接：ceres安裝文檔

進入以上鍊接，首先下載文件，可以用git，或者點擊最新穩定發佈版本下載：

 我下載的是：ceres-solver-1.14.0.tar.gz

放在 slambook2-master/3rdparty/ceres-solver 然後解壓：

tar -zxvf ceres-solver-1.14.0.tar.gz 

 開始編譯

cd ceres-solver-1.14.0/
mkdir build
cd build/
cmake ..

你會發現提示一些依賴庫沒裝（針對性的去裝完整）：

我看官方給出的安裝依賴有：

Eigen 3.2.2 or later strongly recommended, 3.1.0 or later required.
CMake 2.8.0 or later. Required on all platforms except for Android.
glog 0.3.1 or later. Recommended
gflags. Needed to build examples and tests.
SuiteSparse. Needed for solving large sparse linear systems. Optional; strongly recomended for large scale bundle adjustment
CXSparse. Similar to SuiteSparse but simpler and slower. CXSparse has no dependencies on LAPACK and BLAS. This makes for a simpler build process and a smaller binary. Optional

BLAS and LAPACK routines are needed by SuiteSparse, and optionally used by Ceres directly for some operations.

TBB is a C++11 template library for parallel programming that optionally can be used as an alternative to OpenMP. Optional

我這邊繼續裝了個CXSparse,SuiteSparse

ubuntu下的指令爲：

sudo apt-get install libcxsparse3.1.4 libsuitesparse-dev libeigen3-dev libgoogle-glob-dev libgtest-dev 

其中CXSparse是一個簡潔稀疏的cholesky分解包；

centos下：（由於我測試的centos7，因此下載的包都含el7這個名稱，需要針對自己的系統對應下載）

yum install suitesparse   
yum install suitesparse-devel ##這個是suitesparse的headers

用上面第一個指令安裝suitesparse 或者在suitesparse-4.0.2-10.el7.x86_64.rpm 下載suitesparse-4.0.2-10.el7.x86_64.rpm用以下指令安裝：

sudo rpm -i suitesparse-4.0.2-10.el7.x86_64.rpm

我在安裝時提示已經安裝過了，在未安裝 suitesparse-devel 這個庫時去直接cmake ceres依舊提示無法找到suitesparse 的那些庫和頭文件，然而實際上我在/usr/lib64中找到了所需的so文件了，cmake竟然沒有在這個目錄下找到。

然後我接着在pkgs.org這個網站搜到 suitesparse-devel-4.0.2-10.el7.x86_64.rpm ，將其用如下指令裝上：

sudo rpm -i suitesparse-devel-4.0.2-10.el7.x86_64.rpm

這會去cmake ceres時沒有提示找不到，成功了。具體步驟如下：

#進到下載解壓好的ceres文件夾中
#刪除之前爲了cmake創建的build文件，如果你之前編譯過且報錯的話
>rm -rf build

>mkdir build
>cd build
>cmake ..

 

你會發現在centos下裝了suitesparse-devel後很多依賴的頭文件和庫文件都找到了（包括CXSparse），然後接下來就是安裝ceres:(這個指令適用於ubuntu和centos)

make -j2
sudo make install

 

 

 至此，關於ceres的安裝就到這算成功了。你會在usr/local/include下看到它

在/usr/local/lib64下有個libceres.a庫文件；

---

 工程代碼：

用的CLion IDE來打開slambook2-master的ch6工程，對CMakeLists.txt進行分析下：

cmake_minimum_required(VERSION 2.8)  #cmake的最低版本要求
project(ch6)                         #工程名稱

set(CMAKE_BUILD_TYPE Release)                #編譯類型爲release
set(CMAKE_CXX_FLAGS "-std=c++14 -O3")        #編譯的協議c++14標準

list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)  將當前工程文件加入cmake的模塊

# OpenCV
find_package(OpenCV REQUIRED)                 #opencv是必須的依賴庫
include_directories(${OpenCV_INCLUDE_DIRS})

# Ceres
find_package(Ceres REQUIRED)                 #ceres是必須的依賴庫
include_directories(${CERES_INCLUDE_DIRS})

# g2o
find_package(G2O REQUIRED)                    #g2o是必須的依賴庫
include_directories(${G2O_INCLUDE_DIRS})

# Eigen
include_directories("/usr/include/eigen3")   #eigen3是必須的依賴庫

add_executable(gaussNewton gaussNewton.cpp)
target_link_libraries(gaussNewton ${OpenCV_LIBS})

add_executable(ceresCurveFitting ceresCurveFitting.cpp)
target_link_libraries(ceresCurveFitting ${OpenCV_LIBS} ${CERES_LIBRARIES})

add_executable(g2oCurveFitting g2oCurveFitting.cpp)
target_link_libraries(g2oCurveFitting ${OpenCV_LIBS} g2o_core g2o_stuff)

由於我這邊裝了多個版本的opencv，並且用的是c++11的標註，因此對上面的CMakeLists.txt做了點更改，更改如下：

set(CMAKE_CXX_FLAGS "-std=c++11 -O3")   #更改爲c++11

# OpenCV
find_package(OpenCV 3.0 REQUIRED)  # 加了3.0版本的需求，用於過濾低版本

然後編譯得到對應的執行文件：

 由於沒有安裝g2o，多以當前只編譯得到ceresCurveFitting，運行結果如下：

 對於待估計的參數a,b,c進行了8次迭代，最後估計的值接近真實的a=1,b=2,c=1;

曲線方程的形式爲：

 a,b,c爲參數，w爲噪聲，假設有N個關於 x，y的觀測數據，根據這些點求曲線的參數a,b,c，可以求解下面的最小二乘來得到曲線參數：

ceresCurveFitting.cpp 的源碼分析如下：

//
// Created by xiang on 18-11-19.
//

#include <iostream>                   //輸入輸出頭文件
#include <opencv2/core/core.hpp>     //opencv核心代碼頭文件
#include <ceres/ceres.h>            //ceres 頭文件
#include <chrono>                  //計時用

using namespace std;

// 代價函數的計算模型
//首先定義求解問題的代價函數cost function<CURVE_FITTING_COST>
struct CURVE_FITTING_COST {
  CURVE_FITTING_COST(double x, double y) : _x(x), _y(y) {}  //結構體中首先構造構造函數，相當於CURVE_FITTING_COST ( double x,double y) {_x=x; _y=y}

  // 殘差的計算
////template的使用是爲了簡化不同類型的函數和類的重複定義，模版實例化時可以替換任意類型，不僅包括內置類型（int等），也包括自定義類型class,實例化之後才知道的數據的類型。
  template<typename T>
  bool operator()(
    const T *const abc, // 模型參數，有3維
    T *residual) const {  //殘差，即誤差，1維
    residual[0] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2]); // y-exp(ax^2+bx+c)   真實觀測y值減去估計值
    return true;
  }

  const double _x, _y;    // x,y數據
};

int main(int argc, char **argv) {
  double ar = 1.0, br = 2.0, cr = 1.0;         // 真實參數值
  double ae = 2.0, be = -1.0, ce = 5.0;        // 估計參數值 ，初始化值
  int N = 100;                                 // 數據點
  double w_sigma = 1.0;                        // 噪聲Sigma值
  double inv_sigma = 1.0 / w_sigma;
  cv::RNG rng;                                 // OpenCV隨機數產生器

  vector<double> x_data, y_data;      // 數據
  for (int i = 0; i < N; i++) {
    double x = i / 100.0;    //x的值爲0到1
    x_data.push_back(x);
    y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
  }

  double abc[3] = {ae, be, ce};

  // 構建最小二乘問題
  ceres::Problem problem;
  for (int i = 0; i < N; i++) {
    problem.AddResidualBlock(     // 向問題中添加誤差項
      // 使用自動求導，模板參數：誤差類型，輸出維度，輸入維度，維數要與前面struct中一致
      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_NORMAL_CHOLESKY;  // 增量方程如何求解
  options.minimizer_progress_to_stdout = true;   // 輸出到cout

  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 cost = " << time_used.count() << " seconds. " << endl;

  // 輸出結果
  cout << summary.BriefReport() << endl;
  cout << "estimated a,b,c = ";
  for (auto a:abc) cout << a << " ";
  cout << endl;

  return 0;
}

代碼中需要關注的有這麼幾點：

（1）定義代價函數的方式是自定義一個結構體。只需要在結構體中實現operator()方法，就算是給Ceres提供了一個調用接口，由Ceres內部在合適的時候調用該方法計算代價函數。
（2）構建最小二乘問題時，需要將所有誤差項依次添加進去。而每個誤差項又是由前面定義的結構體構成的。需要注意的是，每個誤差項需要指定代價函數求導的方法，有三種方法可供選擇：自動求導、數值求導和自行推導。一般採用自動求導，既方便，又節約運行時時間。
（3）以自動求導爲例，ceres::AutoDiffCostFunction是個模板類，後兩個模板參數爲輸出維度和輸入維度，必須與前面定義的結構體中的residual和abc的維度一樣。
（4）使用ceres::Solver::Options配置求解器。這個類有許多字段，每個字段都提供了一些枚舉值供用戶選擇。所以需要時只要查一查文檔就知道怎麼設置了。

---

二.利用g2o庫安裝與實現

g2o是一個基於圖優化的庫，圖優化是一種將非線性理論與圖論結合起來的理論，在圖優化中將頂點表示優化變量，邊表示誤差項，從而將非線性最小二乘問題轉化成構建與之對應的一個圖。

g2o下載：

git clone https://github.com/RainerKuemmerle/g2o.git

#進到解壓後的文件夾
cd g2o

 

#開始編譯
mkdir build
cd build
cmake ..

 彈出編譯結果，提示一些需要的依賴庫。

我這邊缺少安裝的依賴庫是 LAPACK API和 QGLVIEWER ~（下面我會針對我的centos7系統做安裝說明）

 

*****也可在編譯安裝之前確保一些必要的依賴庫已經安裝，

ubuntu下安裝依賴庫指令：

sudo apt-get install libqt4-dev libqglviewer-dev libcholmod3.0.6 qt4-qmake

*****這邊推薦個QT，QTVIEWER源文件下載和安裝的連接：http://libqglviewer.com/installUnix.html#linux

centos下安裝依賴庫指令：

從上面的連接下載 libQGLViewer-2.7.1.tar.gz

解壓並安裝

>tar -zxvf libQGLViewer-2.7.1.tar.gz 
>cd libQGLViewer-2.7.1/QGLViewer/
>qmake

###會彈出：Info: creating stash file /home/chensq/Downloads/slambook2-master/3rdparty/g2o/libQGLViewer-2.7.1/QGLViewer/.qmake.stash

>make

###然後會生成so文件；

>sudo make install

以上表示QGLViewer安裝好了，其中library在 /usr/local/lib     header文件在/usr/local/include/QGLViewer , doc和example在 /usr/local/share/doc下面。

 我這裏暫且不管LAPACK,然後重新編譯g2o：

rm -rf build
mkdir build
cd build
cmake ..

如上圖，QGLViewer裝上了。

開始安裝g2o:

sudo make install

g2o是一個基於圖優化的庫，圖優化是一種將非線性優化與圖論結合起來的理論。
圖優化，是把優化問題表現成圖（Graph）的一種優化方式。這裏的圖是圖論意義上的圖。一個圖由若干的頂點（Vertex），以及連接這些頂點的邊（Edge）組成。進而，用頂點表示優化變量，用邊表示誤差項。於是，對任意一個上述形式的非線性最小二乘問題，我們可以構建與之對應的一個圖。

節點爲優化變量，邊爲誤差項，曲線擬合圖優化問題可以化成如圖： 

 在曲線擬合問題中，整個問題只有一個頂點:曲線模型的參數a, b,c;而各個帶噪聲的數據點，構成了一個個誤差項，也就是圖優化的邊。這些邊是一-元邊( Unary Edge),即只連接一個頂點一因爲整 個圖只有一個頂點。圖6-3中，自己連到自己。事實上，圖優化中一條邊可以連接一個、兩個或多個頂點，這主要反映每個誤差與多少個優化變量有關。

g2o主要包含以下步驟:
1.定義頂點和邊的類型;
2.構建圖;
3.選擇優化算法;
4.調用g2o進行優化，返回結果;

g2oCurveFitting.cpp 工程代碼：

#include <iostream>
#include <g2o/core/g2o_core_api.h>
#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() override {
    _estimate << 0, 0, 0;    //頂點重置的初始值
  }

  // 更新
  virtual void oplusImpl(const double *update) override {
    _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) {}

  // 計算曲線模型誤差
  virtual void computeError() override {
    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 void linearizeOplus() override {
    const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
    const Eigen::Vector3d abc = v->estimate();
    double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
    _jacobianOplusXi[0] = -_x * _x * y;
    _jacobianOplusXi[1] = -_x * y;
    _jacobianOplusXi[2] = -y;
  }

  virtual bool read(istream &in) {}

  virtual bool write(ostream &out) const {}

public:
  double _x;  // x 值， y 值爲 _measurement
};

int main(int argc, char **argv) {
  double ar = 1.0, br = 2.0, cr = 1.0;         // 真實參數值
  double ae = 2.0, be = -1.0, ce = 5.0;        // 估計參數值
  int N = 100;                                 // 數據點
  double w_sigma = 1.0;                        // 噪聲Sigma值
  double inv_sigma = 1.0 / w_sigma;
  cv::RNG rng;                                 // OpenCV隨機數產生器

  vector<double> x_data, y_data;      // 數據
  for (int i = 0; i < N; i++) {
    double x = i / 100.0;
    x_data.push_back(x);
    y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
  }

  // 構建圖優化，先設定g2o
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;  // 每個誤差項優化變量維度爲3，誤差值維度爲1
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 線性求解器類型

  // 梯度下降方法，可以從GN, LM, DogLeg 中選
  auto solver = new g2o::OptimizationAlgorithmGaussNewton(
    g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
  g2o::SparseOptimizer optimizer;     // 圖模型
  optimizer.setAlgorithm(solver);   // 設置求解器
  optimizer.setVerbose(true);       // 打開調試輸出

  // 往圖中增加頂點
  CurveFittingVertex *v = new CurveFittingVertex();
  v->setEstimate(Eigen::Vector3d(ae, be, ce));      //頂點是待估計的3維向量
  v->setId(0);                                      //總共一個節點
  optimizer.addVertex(v);

  // 往圖中增加邊
  for (int i = 0; i < N; i++) {
    CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);
    edge->setId(i);                       //總共N條邊
    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(10);
  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 << "estimated model: " << abc_estimate.transpose() << endl;

  return 0;
}

 

---

參考：

視覺slam十四講 6.非線性優化

Ceres入門詳解