视觉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                    //输入输出头文件
#include      //opencv核心代码头文件
#include             //ceres 头文件
#include                   //计时用

using namespace std;

// 代价函数的计算模型
//首先定义求解问题的代价函数cost function
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
  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 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(
        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 time_used = chrono::duration_cast>(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 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

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 (_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 (_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 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> BlockSolverType;  // 每个误差项优化变量维度为3，误差值维度为1
  typedef g2o::LinearSolverDense LinearSolverType; // 线性求解器类型

  // 梯度下降方法，可以从GN, LM, DogLeg 中选
  auto solver = new g2o::OptimizationAlgorithmGaussNewton(
    g2o::make_unique(g2o::make_unique()));
  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::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 time_used = chrono::duration_cast>(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入门详解