系列文章目录
SLAM系列——第一讲 预备知识[2023.1]
SLAM系列——第二讲 初识SLAM[2023.1]
SLAM系列——第三讲 三维空间刚体运动[2023.1]
SLAM系列——第四讲 李群李代数[2023.1]
SLAM系列——第五讲 相机与图像[2023.1]
SLAM系列——第六讲 非线性优化[2023.1]
SLAM系列——第七讲 视觉里程计1[2023.1]
SLAM系列——第八讲 视觉里程计2[2023.1]
SLAM系列——第九讲 后端1[2023.1]
SLAM系列——第十讲 后端2[2023.1]
SLAM系列——第十一讲 回环检测[2023.1]
SLAM系列——第十二讲 建图[2023.1]
SLAM系列——第十三讲 实践:设计SLAM系统[2023.1]
SLAM系列——第十四讲 SLAM:现在与未来[2023.1]
文章目录
6.1 状态估计问题
详见。
6.2 非线性最小二乘
详见。
6.3 实践:曲线拟合问题
详见。
6.3.1 手写高斯牛顿法
#include <iostream>
#include <chrono>
#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 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;
}
/slambook2/ch6/cmake-build-debug/gaussNewton
total cost: 3.19575e+06, update: 0.0455771 0.078164 -0.985329 estimated params: 2.04558,-0.921836,4.01467
total cost: 376785, update: 0.065762 0.224972 -0.962521 estimated params: 2.11134,-0.696864,3.05215
total cost: 35673.6, update: -0.0670241 0.617616 -0.907497 estimated params: 2.04432,-0.0792484,2.14465
total cost: 2195.01, update: -0.522767 1.19192 -0.756452 estimated params: 1.52155,1.11267,1.3882
total cost: 174.853, update: -0.537502 0.909933 -0.386395 estimated params: 0.984045,2.0226,1.00181
total cost: 102.78, update: -0.0919666 0.147331 -0.0573675 estimated params: 0.892079,2.16994,0.944438
total cost: 101.937, update: -0.00117081 0.00196749 -0.00081055 estimated params: 0.890908,2.1719,0.943628
total cost: 101.937, update: 3.4312e-06 -4.28555e-06 1.08348e-06 estimated params: 0.890912,2.1719,0.943629
total cost: 101.937, update: -2.01204e-08 2.68928e-08 -7.86602e-09 estimated params: 0.890912,2.1719,0.943629
cost: 101.937>= last cost: 101.937, break.
solve time cost = 0.00011859 seconds.
estimated abc = 0.890912, 2.1719, 0.943629
6.3.2 使用ceres进行曲线拟合
ceres简介
安装ceres
使用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>
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( // 向问题中添加误差项
// 使用自动求导,模板参数:误差类型,输出维度,输入维度,维数要与前面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<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 = ";
for (auto a:abc) cout << a << " ";
cout << endl;
return 0;
}
/slambook2/ch6/cmake-build-debug/ceresCurveFitting
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 1.597873e+06 0.00e+00 3.52e+06 0.00e+00 0.00e+00 1.00e+04 0 1.81e-05 5.60e-05
1 1.884440e+05 1.41e+06 4.86e+05 9.88e-01 8.82e-01 1.81e+04 1 3.79e-05 1.21e-04
2 1.784821e+04 1.71e+05 6.78e+04 9.89e-01 9.06e-01 3.87e+04 1 1.69e-05 1.44e-04
3 1.099631e+03 1.67e+04 8.58e+03 1.10e+00 9.41e-01 1.16e+05 1 1.60e-05 1.64e-04
4 8.784938e+01 1.01e+03 6.53e+02 1.51e+00 9.67e-01 3.48e+05 1 1.62e-05 1.83e-04
5 5.141230e+01 3.64e+01 2.72e+01 1.13e+00 9.90e-01 1.05e+06 1 1.69e-05 2.05e-04
6 5.096862e+01 4.44e-01 4.27e-01 1.89e-01 9.98e-01 3.14e+06 1 1.60e-05 2.24e-04
7 5.096851e+01 1.10e-04 9.53e-04 2.84e-03 9.99e-01 9.41e+06 1 1.50e-05 2.42e-04
solve time cost = 0.00025855 seconds.
Ceres Solver Report: Iterations: 8, Initial cost: 1.597873e+06, Final cost: 5.096851e+01, Termination: CONVERGENCE
estimated a,b,c = 0.890908 2.1719 0.943628
Process finished with exit code 0
6.3.3 使用g2o进行曲线拟合
g2o的编译与安装
使用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<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 {}
};
// 误差模型 模板参数:观测值维度,类型,连接顶点类型
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
};
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);
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::Vector3d abc_estimate = v->estimate();
cout << "estimated model: " << abc_estimate.transpose() << endl;
return 0;
}
/slambook2/ch6/cmake-build-debug/g2oCurveFitting
start optimization
solve time cost = 0.000211744 seconds.
estimated model: 0.890912 2.1719 0.943629
iteration= 0 chi2= 376785.128234 time= 1.018e-05 cumTime= 1.018e-05 edges= 100 schur= 0
iteration= 1 chi2= 35673.566018 time= 4.666e-06 cumTime= 1.4846e-05 edges= 100 schur= 0
iteration= 2 chi2= 2195.012304 time= 4.01e-06 cumTime= 1.8856e-05 edges= 100 schur= 0
iteration= 3 chi2= 174.853126 time= 3.82999e-06 cumTime= 2.2686e-05 edges= 100 schur= 0
iteration= 4 chi2= 102.779695 time= 3.789e-06 cumTime= 2.6475e-05 edges= 100 schur= 0
iteration= 5 chi2= 101.937194 time= 3.779e-06 cumTime= 3.0254e-05 edges= 100 schur= 0
iteration= 6 chi2= 101.937020 time= 3.753e-06 cumTime= 3.4007e-05 edges= 100 schur= 0
iteration= 7 chi2= 101.937020 time= 3.806e-06 cumTime= 3.7813e-05 edges= 100 schur= 0
iteration= 8 chi2= 101.937020 time= 3.769e-06 cumTime= 4.1582e-05 edges= 100 schur= 0
iteration= 9 chi2= 101.937020 time= 3.762e-06 cumTime= 4.5344e-05 edges= 100 schur= 0
Process finished with exit code 0
6.4 小结
详见。
习题
详见。