首页 > 其他分享> 文章详细

视觉SLAM十四讲学习笔记-第六讲学习笔记总结（2）---非线性优化应用

2022-02-01 22:58:38 阅读：237 来源： 互联网

标签：xi abc 求解优化笔记 --- SLAM double sigma

第六讲学习笔记如下：

视觉SLAM十四讲学习笔记-第六讲-非线性优化的状态估计问题_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第六讲-非线性优化的非线性最小二乘问题_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第六讲-非线性优化的实践-高斯牛顿法和曲线拟合_goldqiu的博客-CSDN博客

前五讲学习笔记总结如下：

视觉SLAM十四讲学习笔记---前三讲学习笔记总结之SLAM的作用、变换和位姿表示_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲---第五讲学习笔记总结---李群和李代数、相机_goldqiu的博客-CSDN博客

第六讲

视觉SLAM十四讲学习笔记-第六讲学习笔记总结（1）---非线性优化原理

总结：

一. 文伯格—马夸尔特方法（LM法）：高斯牛顿法只在问题性质比较好时比较有效果，但问题比较病态时可以采用LM法，能够更加稳定实现优化求解问题。

二. 使用三种方法对曲线

进行拟合：

手写高斯牛顿法：

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));
  }

  // 开始Gauss-Newton迭代
  int iterations = 100;    // 迭代次数
  double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的cost

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  for (int iter = 0; iter < iterations; iter++) {

    Matrix3d H = Matrix3d::Zero();             // Hessian = J^T W^{-1} J in Gauss-Newton
    Vector3d b = Vector3d::Zero();             // bias
    cost = 0;

    for (int i = 0; i < N; i++) {
      double xi = x_data[i], yi = y_data[i];  // 第i个数据点
      double error = yi - exp(ae * xi * xi + be * xi + ce);
      Vector3d J; // 雅可比矩阵
      J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce);  // de/da
      J[1] = -xi * exp(ae * xi * xi + be * xi + ce);  // de/db
      J[2] = -exp(ae * xi * xi + be * xi + ce);  // de/dc

      H += inv_sigma * inv_sigma * J * J.transpose();
      b += -inv_sigma * inv_sigma * error * J;

      cost += error * error;
    }

    // 求解线性方程 Hx=b
    Vector3d dx = H.ldlt().solve(b);
    if (isnan(dx[0])) {
      cout << "result is nan!" << endl;
      break;
    }

    if (iter > 0 && cost >= lastCost) {
      cout << "cost: " << cost << ">= last cost: " << lastCost << ", break." << endl;
      break;
    }

    ae += dx[0];
    be += dx[1];
    ce += dx[2];

    lastCost = cost;

    cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpose() <<
         "\t\testimated params: " << ae << "," << be << "," << ce << endl;
  }

  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 << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
  return 0;
}

a. 用chrono类中的接口进行优化求解时间的计算。

b. 手写高斯牛顿优化器的步骤：

1）对待优化变量a、b、c设定初值，初始化的时候设定了。

2）在初值的条件下，估计输出y与真实输出y比较得到误差，定义误差为:

3）求出每个误差项对于状态变量(a,b,c)的导数

这里待估计变量是3x1，故J矩阵为3维向量。

4）第2步和第3步根据数据长度循环100次，对H(x)和g(x)进行增量处理。

根据以下公式：

      H += inv_sigma * inv_sigma * J * J.transpose();
      b += -inv_sigma * inv_sigma * error * J;

其实这里就是进行求和处理。误差的平方为代价，在优化过程中代价会越来越小，代价达到一定条件或者达到优化次数限制后就跳出优化。

5）调用eigen库的方法进行线性方程的求解，求解得到的dx对估计值进行更新。

6）重复第2~5步的优化求解迭代过程，最多100次。当dx为0时或优化的代价变大则跳出优化过程，此时说明优化过程已经开始收敛。（这里可以改变优化跳出的条件）

2.用ceres进行曲线拟合的优化问题求解

a.构建最小二乘问题

// 构建最小二乘问题
  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                 // 待估计参数，初值
    );
  }

b.定义残差计算的模板类，使用了（）运算符重载的仿函数功能。

// 代价函数的计算模型
struct CURVE_FITTING_COST {
  CURVE_FITTING_COST(double x, double y) : _x(x), _y(y) {}

  // 残差的计算
  template<typename T>
  bool operator()(
    const T *const abc, // 模型参数，有3维
    T *residual) const {
    residual[0] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2]); // y-exp(ax^2+bx+c)
    return true;
  }

  const double _x, _y;    // x,y数据
};

这里状态变量是三维，分别为a，b，c；残差输出是一维，所以是residual[0]。

c. 配置求解器，进行优化，会按照前面定义的残差计算方式进行优化求解。

 // 配置求解器
  ceres::Solver::Options options;     // 这里有很多配置项可以填
  options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;  // 增量方程如何求解，用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;

3.使用g2o进行曲线拟合的优化问题求解。图优化其实就是用概率图去描述优化问题然后进行求解。

将曲线拟合问题抽象成图优化。这个过程中，节点为优化变量，边为误差项。在这个问题只有一个顶点：曲线模型的参数 a, b, c；而各个带噪声的数据点，构成了一个个误差项，也就是图优化的边。这里的边是一元边（Unary Edge），即只连接一个顶点。事实上，图优化中一条边可以连接一个、两个或多个顶点，这主要反映每个误差与多少个优化变量有关。

主要步骤：

定义顶点和边的类型。
构建图。
选择优化算法。
调用g2o进行优化，返回结果。

主函数中：

// 构建图优化，先设定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));
  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(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;

图优化顶点类：

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
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 {}
};

这个类继承BaseVertex类，重写了一些虚函数：

顶点的更新函数：oplusImpl。优化过程最重要的是增量∆x的计算，而该函数处理的是 xk+1 = xk + ∆x 的过程。在曲线拟合过程中，由于优化变量（曲线参数）本身位于向量空间中，这个更新计算就是简单的加法。但是当优化变量不在向量空间中时，比如x是位姿，它本身不一定有加法运算。这时就需要重新定义增量如何加到现有的估计上的行为了。
顶点的重置函数：setToOriginImpl。即把估计值置零即可。
存盘和读盘函数：read、write。

图优化边类：

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
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
};

这个类继承BaseUnaryEdge类，重写了一些虚函数：

边的误差计算函数：computeError。该函数需要取出边所连接的顶点的当前估计值，根据曲线模型，与它的观测值进行比较。这和最小二乘问题中的误差模型是一致的。
边的雅可比计算函数：linearizeOplus。这个函数里计算了每条边相对于顶点的雅可比。
存盘和读盘函数：read、write。

定义了顶点和边之后，在main函数里声明了一个图模型，然后按照生成的噪声数据，往图模型中添加顶点和边，最后调用优化函数进行优化。

三. 在SLAM中更多的问题是，一个带有许多个位姿和许多个空间点的优化问题如何求解。特别地，当位姿以李代数表示时，误差项关于位姿的导数如何计算。g2o提供了大量现成的顶点和边，非常便于位姿估计问题。而在 Ceres 中，不得不自己实现每一个Cost Function，有一些不便。但是Ceres库提供了基于模板元的自动求导和运行时的数值求导，而g2o只提供了运行时数值求导这一种方式。对于大多数问题，如果能够推导出雅可比矩阵的解析形式并告诉优化库，就可以避免数值求导中的诸多问题，求解也更快。

标签：xi,abc,求解,优化,笔记,---,SLAM,double,sigma
来源： https://blog.csdn.net/weixin_36773706/article/details/122766899

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

ICode9

视觉SLAM十四讲学习笔记-第六讲学习笔记总结（2）---非线性优化应用