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

方法1：

方法2：

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

常用的优化库：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：

<code>#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：

<code>#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讲习题解答