SLAM4-非线性优化

一、状态估计

有两个方程，x是相机位姿，u是位姿改变数据，y是路标点在相机坐标系的坐标，z是路标点在图片上的坐标
状态估计问题：在有一个已知的xk-1时，通过uk我们求出xk; 通过yj,xk我们求出zk；但是问题是求的过程中都会有噪声wk,vk，所以我们只能求出来xk和zk的概率分布，所以构成了状态估计问题

!](https://img-blog.csdnimg.cn/2020070409274871.png)

滤波器：为了处理状态估计问题，我们假设已经有了一个当前时刻的估计状态（有误差），之后用新的数据来更新他，从而减小误差，这种方法称为增量/渐进方法，或者叫做滤波器，滤波器尽管新
批量：为了处理状态估计问题，把0到k时刻的所有uk,zk放在一起，然后进行优化，减小误差
非线性优化方法是批量方法的一种，要求的状态量是，P（x,y|u,z），有下面的关系，后验正比与似然乘以先验；
后验：已知观测数据z（比如在图像中的位置）的情况下，估计系统的状态（比如它的位姿），这就是后验概率
似然：直接求后验比较难，我们可以不停的试各种状态，看看是在什么样的状态下(x,y)，最有可能产生现在观测到的数据。找到这个状态了，那就是我要求的位姿了

最大似然估计：所以非线性优化转为求最大似然估计的问题，估计的方程是，要求的是当P最大时，x,y是多少。

求解最大似然估计：我们把P变成-log( P ),求解该式的最小值，就相当于求P的最大值，
P的概率密度函数是

-log( p)的概率密度函数是

密度函数求和之后是

转换为最小二乘问题：把上面的用误差表示，可以写为下面的形式：
Eu,k=Xk-f(Xk-1,Uk)
Ez,j,k=Zk,j-h(Xk,Yj)

求解最小二乘问题：一般思路是求导数为0处所有点的值，然后比较求最小的，但是有时候方程可能不便于求导，所以有两种方法，line search和Trust Region
1、Line search

在line search具体实施的时候，需要找到增量Δxk,所以又分为两种方法，梯度下降法和高斯牛顿法
梯度下降法：在x附近展开目标函数，H是二阶导数，海塞矩阵。J是雅克比矩阵，表示fx关于x的导数。对于多个数据点u,z,J是多个数据点的fx关于x变量求导之和。这种方式的优点是比较直观，解线性方程组就行了

如果是一阶梯度法，那就不要最后一项，让它最小就完事了。这又叫最速下降法，缺陷是太贪心了，容易走锯齿路线。

如果是二阶梯度法，那么最后一项项还是要的，求右侧关于△x的导数，并令其为0。这种方法又叫牛顿法，缺陷是它得算一个H矩阵，不好算。

两者结果的区别，都是-JT，后者是H△x=-JT，前者直接△x就是-JT（T是转置符号）

高斯牛顿法：把目标函数变为一次方程，然后进行展开。然后求展开之后的目标函数的最小值。


求△x的导数，令其为0。

最后求得的
结果是JT *J △x=-JTfx （这里的fx是误差），近似写成H△x=g

这种方法的限制在于：JT和J构成的近似H矩阵应该是可逆的，正定的（特征值为正，各阶顺序主子式为正）。但实际算出来的是半正定的。如果出现它是奇异矩阵或者是病态矩阵，那算法就不会收敛。

对于这个的改进，有的方法是在找到△x以后，再找一个α，让||f(x+α△x)||2最小，不是简单的让α等于1。

2、Trust Region

这里的ρ如下定义：

分子表明实际函数下降的值，分母表明用雅克比矩阵的近似估计值。可以根据ρ来调整近似范围，太小说明分母较大，说明估计的太大，应该缩小范围；太大说明分母较小，说明的估计的小，可以放大近似范围。

上面算法里的阈值3/4和1/4，还有半径的放缩系数2和0.5都是经验值。
列文伯格-马尔夸特方法又可以叫做阻尼牛顿法。对于上面的D，我认为这个是控制区域的形状。如果区域是球，那就取单位阵E，它也可以把球弄成一个椭球。

注意上面步骤里面，是有个st不等式约束的，可以用拉格朗日乘子把目标函数转化成一个整体的无约束优化问题。

λ较小，H占主要地位，接近上面的高斯牛顿法。

λ较大，更接近一阶梯度下降法。DT*D可以简化为单位矩阵E。

列文伯格-马尔夸特方法优点就是可以避免非奇异与病态问题，缺点是收敛速度会很慢。

二、手写高斯牛顿做曲线拟合

y=exp(ax2+bx+c)+w
#include
#include
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

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 J in Gauss-Newton
Vector3d b = Vector3d::Zero(); // g(x)
cost = 0;
//使用100个数据点获取J
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;
}
//
// 求解线性方程 H*dx=b获得增量dx
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 time_used = chrono::duration_cast<chrono::duration>(t2 - t1);
cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
return 0;
}

三、使用Ceres做曲线拟合

//
// Created by xiang on 18-11-19.
//

#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> 声明模板函数，类型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数据
};

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

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

// 构建最小二乘问题
ceres::Problem problem;)
for (int i = 0; i < N; i++) {
problem.AddResidualBlock( // 向问题中添加误差项
// 使用自动求导完成de/da,de/db,de/dc，模板参数：误差计算函数，输出维度，输入维度，维数要与前面struct中一致
new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(
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<chrono::duration>(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;
}

四、使用g2o做曲线拟合

定点是待估计参数abc,边是误差项e
#include
#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;

// 顶点Vertex的类型，模板参数：优化变量维度和数据类型
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

// 重置函数，将待优化变量置0
virtual void setToOriginImpl() override {
_estimate << 0, 0, 0;
}

// 更新函数，定义增量dx如何更新_estimate的公式
virtual void oplusImpl(const double *update) override {
_estimate += Eigen::Vector3d(update);
}

// 存盘和读盘：留空
virtual bool read(istream &in) {}

virtual bool write(ostream &out) const {}
};

// Edge的误差模型 模板参数：观测值维度，类型，连接的顶点类型
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]);vertices是g2o给定值
const Eigen::Vector3d abc = v->estimate();以上两句话可以直接背住，方便下面计算误差
_error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
}measurement也是g2o给定值

// 计算雅可比矩阵
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
};

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

// 构建图优化，先设定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);设定每个边的ID
edge->setVertex(0, v); // 设置边连接的顶点
edge->setMeasurement(y_data[i]); // 设定有噪声的观测数值y传给measurement
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 time_used = chrono::duration_cast<chrono::duration>(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;
}