【视觉SLAM】 十四讲ch6习题

1、证明线性方程Ax = b 当系数矩阵A 超定时，最小二乘解为x =(ATA)-1ATb。

方法1：

方法2：

---

2、调研最速下降法、牛顿法、GN 和LM 各有什么优缺点。除了我们举的Ceres 库和g2o 库，还有哪些常用的优化库？你可能会找到一些MATLAB 上的库。

求解方法	优点	缺点
最速下降法	直观意义比较简单，只要沿着反向梯度方向前进，在一阶线性近似下，目标函数必定下降。前几次迭代的时候目标函数下降的较快。	最速下降法过于贪心，容易走出锯齿路线，反而增加了迭代次数。
牛顿法	其思想是用二阶函数来近似原函数，然后用近似函数求出极小值来当作近似值。 较为直观，收敛较快，迭代次数较少。	牛顿法需要计算目标函数的H 矩阵当问题规模较大时非常困难，且迭代时间较长，通常需要避免 H矩阵的计算。
高斯牛顿法	高斯牛顿法用 JJT 作为牛顿法中二阶 Hessian 矩阵的近似，省略了计算H矩阵的过程。	JJT只有半正定性。实际数据中可能出现JJT为奇异矩阵或者病态(ill-condition)的情况，此时增量的稳定性较差，算法不收敛。
列文伯格-马夸尔特方法	引入了信赖区 域(Trust Region)来改进高斯牛顿法，能在一定程度上避免线性方程组的系矩阵的非奇异和病态问题，提供更稳定的增量 Δx。	收敛速度比高斯牛顿法慢

---

常用的优化库：NLopt、slam++、Ipopt、Optimization Toolbox（MATLAB）等。

Note：

---

3、为什么GN 的增量方程系数矩阵可能不正定？不正定有什么几何含义？为什么在这种情况下解就不稳定了？

高斯牛顿法将JJT作为Hessian矩阵的近似，而在实际的计算中 JJT只有半正定性。（例如：当J为零向量时，增量方程系数矩阵不是正定的。）当Hessian矩阵不正定时，此时的目标函数 f(x0+Δx)≈f(x0)，对于f(x)的近似是一条水平的直线，在x0点处函数是“平坦的”，无法给出下一次迭代的下降方向（即无法求解增量），所以在这种情况下解不稳定。

4、DogLeg是什么？它与高斯牛顿法和列文伯格-马夸尔特方法有什么异同？请搜索相关的材料。

DogLeg是信赖区域（Trust-Region）方法的一种，包含了高斯牛顿法和最速下降法两种方法，通过引入信赖区域的概念，对两种方法计算得到的增量，在信赖区域进行加权得到新的增量并用于后续的迭代，因此计算得到的下降路径类似于DogLeg，所以被称为DogLeg方法。

Dogleg算法步骤：利用置信域的方法在最速下降法和高斯牛顿法之间进行切换（将二者的搜索步长及方向转化为向量，两个向量进行叠加得到新的方向和置信域内的步长），相当于是一种加权求解。

DogLeg算法分为三步：

高斯牛顿法的解在信赖域中，则取其作为更新量。

否则如果最速下降法的解不在信赖域中（即高斯牛顿法和最速下降法的解都不在信赖域中），则将最速下降法的解步长缩短至信赖域半径，作为更新量。

否则（如图中第3个所示，高斯牛顿法的解不在信赖域中，而最速下降法的解在信赖域中）取两个向量连接起来与信赖域的交点，作为更新量。

DogLeg与高斯牛顿法和列文伯格-马夸尔特方法的异同：

DogLeg包含了高斯牛顿法，当高斯牛顿法计算得到的增量在信赖区域时，DogLeg较为接近高斯牛顿法；不同的是DogLeg还包含了最速下降法，使得下降搜索方向和增量计算更加合理，算法更加鲁棒。

列文伯格-马夸尔特方法与DogLeg都引入了信赖区域的概念，都结合了高斯牛顿法和最速下降法；不同的是DogLeg需要计算更多的中间量。

5、6题略。

7、请更改曲线拟合实验中的曲线模型，并用Ceres和g2o进行优化实验。例如，可以使用更多的参数和更复杂的模型。

选用的新模型为：

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, // 模型参数，有4维
    T *residual) const {
    residual[0] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2])- abc[3] * T(_x) ;//y-exp(ax^2+bx+c)-dx)
    return true;
  }
  const double _x, _y;    // x,y数据
};

int main(int argc, char **argv) {
  double ar = 1.0, br = 2.0, cr = 1.0, dr=2.0;         // 真实参数值
  double ae = 2.0, be = -1.0, ce = 5.0, de=-1.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) + dr * x  + rng.gaussian(w_sigma * w_sigma));
  }

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

  // 构建最小二乘问题
  ceres::Problem problem;
  for (int i = 0; i < N; i++) {
    problem.AddResidualBlock(     // 向问题中添加误差项
      // 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
      new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 4>(
        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,d= ";
  for (auto a:abc) cout << a << " ";
  cout << endl;

  return 0;
}

G2o：

#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<4, Eigen::Vector4d> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  // 重置
  virtual void setToOriginImpl() override {
    _estimate << 0, 0, 0, 0;
  }

  // 更新
  virtual void oplusImpl(const double *update) override {
    _estimate += Eigen::Vector4d(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::Vector4d abc = v->estimate();
    _error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0)) - abc(3, 0) * _x;
  }

  // 计算雅可比矩阵
  virtual void linearizeOplus() override {
    const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
    const Eigen::Vector4d abc = v->estimate();
    double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]) + abc[3] * _x;
    _jacobianOplusXi[0] = -_x * _x * y;
    _jacobianOplusXi[1] = -_x * y;
    _jacobianOplusXi[2] = -y;
    _jacobianOplusXi[3] = -_x;
  }

  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, dr=2.0;         // 真实参数值
  double ae = 2.0, be = -1.0, ce = 5.0, de=-1.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) + dr * x + rng.gaussian(w_sigma * w_sigma));
  }

  // 构建图优化，先设定g2o
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<4, 1>> BlockSolverType;  // 每个误差项优化变量维度为4，误差值维度为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::Vector4d(ae, be, ce, de));
  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;

  // 输出优化值
  Eigen::Vector4d abc_estimate = v->estimate();
  cout << "estimated model: " << abc_estimate.transpose() << endl;

  return 0;
}

上述内容参考了：视觉SLAM十四讲（第二版）第6讲习题解答