高斯牛顿法与gtsam学习心得

1 高斯牛顿法与GTSAM学习心得

最近一周在梳理GTSAM，以及非线性优化部分，把进度整理如下。学习流程是学习了非线性优化部分，分别写了gaussNewton.cpp、testGtsam.cpp、curveFitting.cpp三个cpp，并用GTSAM实现，源码链接详见zhao-zhibo/Test-GTSAM

cpp名称	具体内容
gaussNewton.cpp	手写高斯牛顿法（视觉slam十四讲书中的曲线拟合133页曲线拟合指数函数），参考了b站讲解，链接主要是帮助理解高斯牛顿增量方程中的累加，因为原始式子（1）是没有累加的，但是在实际计算时式子（2）进行了累加。
testGtsam.cpp	复现GTSAM官网中的BetweenFactor，特别是自定义因子UnaryFactor，参考GTSAM文档
curveFitting.cpp	用GTSAM实现视觉slam十四讲133页的曲线拟合指数函数，构造自定义因子，参考gtsam曲线拟合
源码链接	zhao-zhibo/Test-GTSAM

写testGtsam.cpp时遇到了一个问题，这里面自定义因子UnaryFactor，需要构造evaluateError函数，场景是二维刚体的旋转，需要推导出误差方程的雅克比矩阵，这部分在1.2中介绍。
写curveFitting.cpp时，代码运行过程中遇到的两个问题，这两个问题分别记录到下面1.3中.

1.1 手写高斯牛顿法 gaussNewton.cpp

高斯牛顿原始的增量方程可以表示为：
$$\left({\mathbf{J}}^T\mathbf{J}\right)\Delta \mathbf{x}=-{\mathbf{J}}^T\mathbf{r}\text{\hspace{1em}(1)}$$
其中，
$$\begin{array}{rl}\mathbf{J}& =\frac{\partial \mathbf{r}}{\partial \mathbf{x}}\\ & ={\left[\begin{array}{cccc}\frac{\partial r_1}{\partial x_1}& \frac{\partial r_1}{\partial x_2}& \cdots & \frac{\partial r_1}{\partial x_n}\\ \frac{\partial r_2}{\partial x_1}& \frac{\partial r_2}{\partial x_2}& \cdots & \frac{\partial r_2}{\partial x_n}\\ ⋮& ⋮& \ddots & ⋮\\ \frac{\partial r_m}{\partial x_1}& \frac{\partial r_m}{\partial x_2}& \cdots & \frac{\partial r_m}{\partial x_n}\end{array}\right]}_{m\times n},\mathbf{r}={\left[\begin{array}{c}{r}_1\\ r_2\\ ⋮\\ r_m\end{array}\right]}_{m\times 1},\mathbf{x}={\left[\begin{array}{c}{x}_1\\ x_2\\ ⋮\\ x_n\end{array}\right]}_{n\times 1}\end{array}\text{\hspace{1em}(2)}$$ $\mathbf{J}$表示雅可比矩阵，$\mathbf{r}$表示对应的残差， $\Delta \mathbf{x}$表示所有状态的增量向量。
接下来对指数函数的高斯牛顿法的具体实现进行描述，函数的背景见《视觉slam十四讲》第二版133页，这个曲线拟合的雅克比矩阵比较好推导，书中写的很清晰。
高斯牛顿法的具体迭代过程如下：

1. 初始化${\mathbf{x}}_0$，也就是 ${\mathbf{a}}_0$、 ${\mathbf{b}}_0$、 ${\mathbf{c}}_0$，代码中初始化为（2.0，-1.0，5.0），计算 ${\mathbf{J}}_0$ 和 ${\mathbf{r}}_0$，通过式1求出来增量$\Delta {\mathbf{x}}_0$，也就是$\Delta {\mathbf{a}}_0$、$\Delta {\mathbf{b}}_0$、$\Delta {\mathbf{c}}_0$。
2. 接下来计算${\mathbf{x}}_1={\mathbf{x}}_0+\Delta {\mathbf{x}}_0$。
3. 重复这个过程，也就是利用公式（1）计算 ${\mathbf{J}}_k$ 和 ${\mathbf{r}}_k$，然后计算增量 $\Delta {\mathbf{x}}_k$，并更新 ${\mathbf{x}}_{k+1}={\mathbf{x}}_k+\Delta {\mathbf{x}}_k$。
4. 计算 ${\mathbf{J}}_k$ 、 ${\mathbf{r}}_k$与$\Delta {\mathbf{x}}_k$，通过判定 ${\mathbf{r}}_k$是否满足终止条件，满足则输出 ${\mathbf{x}}_{k+1}$，否则继续迭代。这里的终止条件我们设置为误差项大于等于上一次误差，即可终止，也就是认为收敛了，因为有可能拟合的过程中出现下一次的误差项比上一次误差项大的情况。

视觉slam十四讲第二版中的133页的指数函数对应的高斯牛顿增量方程的累加形式为：
$$\sum _{i=1}^{100}\left({{\mathbf{J}}_i^T}_{n\times m}{(\mathbf{\Sigma }}^{-1}{)}_{m\times m}{{\mathbf{J}}_i}_{m\times n}\right){\Delta {\mathbf{x}}_k}_{n\times 1}=-\sum _{i=1}^{100}\left({{\mathbf{J}}_i^T}_{n\times m}{{\mathbf{\Sigma }}^{-1}}_{m\times m}{{\mathbf{r}}_i}_{m\times 1}\right)\text{\hspace{1em}(3)}$$ 其中，$\mathbf{\Sigma }$表示噪声协方差矩阵，${\mathbf{J}}_i$表示第$i$个状态的雅可比矩阵，${\mathbf{r}}_i$表示对应的残差。$\Delta {\mathbf{x}}_k$表示所有状态的增量向量。这里面的$k$是迭代次数，也就是每次迭代算出来一个$x$，$i$是观测次数，也就是对变量$x$做了一百次观测。
因为其有一百对$x$的观测，因此取了累加，可以形象的理解为取了平均值，可以参考b站讲解，我是看这个视频理解了为什么做了累加。

1.2 gtsam自定义UnaryFactor因子 testGtsam.cpp

这个对应的是testGtsam.cpp，对二维刚体变换的推导过程中遇到了点问题，因为自定义因子时，需要用到误差方程的雅克比矩阵，这里的推导稍微复杂些，详情见我写的另一篇博客二维空间和三维空间刚体变换中的雅克比矩阵的推导。

1.3 gtsam实现曲线拟合指数函数（视觉slam十四讲第二版133页）curveFitting.cpp

这个cpp是curveFitting.cpp，其中结合了matplotlibcpp.h，matplotlibcpp.h是安装的c++画图的依赖库，方便画图使用，安装参考链接，这里面和1.1节中一样，都是对曲线进行拟合，因此它的雅克比矩阵也是现成的，具体可以参考下面代码中的evaluateError函数。先把我写的代码附上，这个代码已经是正常能使用的，如下所示：

#include <gtsam/base/Vector.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/Values.h>
#include <opencv2/core/core.hpp>
#include <random>
#include "matplotlibcpp.h"
#include <cmath>

#include <ros/ros.h>

using namespace std;
using namespace gtsam;

namespace plt = matplotlibcpp;
using gtsam::symbol_shorthand::X;

// y = exp(a x^2 + b x + c)
// 利用x和参数a,b,c计算y
double funct(const gtsam::Vector3 &p, const double x)
{
    return exp(p(0) * x * x + p(1) * x + p(2));
}

// 自定义类名 : 继承于一元因子类<优化变量的数据类型>
class curvfitFactor : public gtsam::NoiseModelFactor1<gtsam::Vector3>
{
    double xi, yi; // 观测值

public:

    curvfitFactor(gtsam::Key j, const gtsam::SharedNoiseModel &model, double x, double y)
        : gtsam::NoiseModelFactor1<gtsam::Vector3>(model, j), xi(x), yi(y)  {}

    ~curvfitFactor() override {}

    // 自定义因子一定要重写evaluateError函数(优化变量, 雅可比矩阵)
    Vector evaluateError(const gtsam::Vector3 &p, boost::optional<Matrix &> H = boost::none) const override
    {
        auto val = funct(p, xi);
        if (H) // 残差为1维，优化变量为3维，雅可比矩阵为1*3
        {
            gtsam::Matrix Jac = gtsam::Matrix::Zero(1, 3);
            Jac << xi * xi * val, xi * val, val;
            (*H) = Jac;
        }
        gtsam::Vector1 ret; 
        ret[0] = val - yi;
        return ret;
        // return gtsam::Vector1(val - yi); // 返回值为残差
    }

};

int main(int argc, char** argv)
{
    // ros::init(argc, argv, "lio_sam");    

    gtsam::NonlinearFactorGraph graph;
    const gtsam::Vector3 para(1.0, 2.0, 1.0); // a,b,c的真实值
    double w_sigma = 1.0;                     // 噪声Sigma值
    cv::RNG rng; // OpenCV随机数产生器

    std::vector<double> x_data, y_data;

    for (int i = 0; i < 100; ++i)
    {
        double xi = i / 100.0;

        double yi = funct(para, xi) + rng.gaussian(w_sigma * w_sigma); // 加入了噪声数据
        // auto noiseM = gtsam::noiseModel::Isotropic::Sigma(1, w_sigma); // 噪声的维度需要与观测值维度保持一致
        auto noiseM = gtsam::noiseModel::Diagonal::Sigmas(Vector1(w_sigma)); // 噪声的维度需要与观测值维度保持一致
        // 这里面的X(0)表示的是在第一个变量上加入一元因子
        graph.emplace_shared<curvfitFactor>(X(0), noiseM,xi, yi);     // 加入一元因子

        x_data.push_back(xi);
        y_data.push_back(yi);
    }

    gtsam::Values intial;
    intial.insert<gtsam::Vector3>(X(0), gtsam::Vector3(2.0, -1.0, 5.0));

    // gtsam::DoglegOptimizer opt(graph, intial); // 使用Dogleg优化
    gtsam::GaussNewtonOptimizer opt(graph, intial); // 使用高斯牛顿优化
    // gtsam::LevenbergMarquardtOptimizer opt(graph, intial); // 使用LM优化

    std::cout << "initial error=" << graph.error(intial) << std::endl;
    auto res = opt.optimize();
    res.print("final res:");

    std::cout << "final error=" << graph.error(res) << std::endl;

    gtsam::Vector3 matX0 = res.at<gtsam::Vector3>(X(0));
    std::cout << "a b c: " << matX0 << "\n";

    int n = 5000;
    std::vector<double> x(n), y(n), w(n, 2);
    for (int i = 0; i < n; ++i)
    {
        x.at(i) = i * 1.0 / n;
        y.at(i) = exp(matX0(0) * x[i] * x[i] + matX0(1) * x[i] + matX0(2));
    }

    plt::figure_size(640, 480);
    plt::plot(x_data, y_data, "ro");
    plt::plot(x, y, {{"color", "blue"}, {"label", "$y = e^{ax^2+bx+c}$"}});
    plt::show();

    // ros::MultiThreadedSpinner spinner(3);
    // spinner.spin();

    return 0;
}

最终拟合后的曲线如下图所示：

在写自定义因子的时候踩了两个坑，分别在1.31.和1.3.2中进行介绍。

1.3.1 Eigen返回错误

该问题已经解决，第一个错误是在上述代码中的 // return gtsam::Vector1(val - yi); // 返回值为残差，这样写会返回错误，
lio_sam_curveFitting: /usr/local/include/gtsam/3rdparty/Eigen/Eigen/src/Core/Matrix.h:241: Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::Matrix(Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::Index) [with _Scalar = double; int _Rows = 1; int _Cols = 1; int _Options = 0; int _MaxRows = 1; int _MaxCols = 1; Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::Index = long int]: ' Assertion SizeAtCompileTime == Dynamic || SizeAtCompileTime == dim' failed. [lio_sam_curveFitting-2] process has died [pid 1903900, exit code -6, cmd /home/zhao/Codes/competition_code/lio-sam-mergePoints/devel/lib/lio_sam/lio_sam_curveFitting __name:=lio_sam_curveFitting __log:=/home/zhao/.ros/log/46e8b8fa-9510-11ee-b1d4-65b78ef41fcf/lio_sam_curveFitting-2.log]. log file: /home/zhao/.ros/log/46e8b8fa-9510-11ee-b1d4-65b78ef41fcf/lio_sam_curveFitting-2*.log
最后的解决方式在见stackoverflow，这个是我在stackoverflow提的问题，别人回答说主要是Eigen的版本太老了或者gtsam依赖的太老了。主要是更换了返回的方式，如下

  gtsam::Vector1 ret; 
  ret[0] = val - yi;
  return ret;

1.3.2 ros的初始化干扰gtsam的优化

该问题已经解决，这个问题的主要干扰项是 ros::init(argc, argv, "lio_sam");，它会影响gtsam中的优化函数optimize()的使用，目前的解决方案是直接删除这一行代码，要不然会报错，报错如下：

我在stackoverflow提了问题，目前还没人回答。