SLAM14-6
一、理论
1.相机观测模型
2.状态估计到最大似然
3.从最大似然到最小二乘
4.n维高斯分布
5.状态估计到最小二乘
6.非线性最小二乘
7.列文伯格-马夸尔特方法（一些理解）
8.相关文章链接：
雅可比矩阵：
（棒） https://blog.csdn.net/gwplovekimi/article/details/104977255
https://zhuanlan.zhihu.com/p/81102093
（棒） https://blog.csdn.net/qq_28087491/article/details/115620590
解析解与数值解理解：
https://blog.csdn.net/qq_28087491/article/details/107369412
二阶 Hessian 海塞矩阵
https://zhuanlan.zhihu.com/p/138334587
二、实践
1.手写高斯牛顿（C++）
牛顿迭代法的基本思想是使用泰勒级数展开式去近似地代替非线性回归模型，然后通过多次迭代，多次修正回归系数，使回归系数不断逼近非线性回归模型的最佳回归系数，最后使原模型的残差平方和达到最小。
14讲中，通过高斯牛顿去求状态量，逼近真实的状态量。
问题描述：
其中a,b,c为待估计的参数，w为噪声。在程序里利用模型生成x，y的数据，在给数据添加服从高斯分布的噪声。之后用ceres优化求解参数a,b,c。
代码：main.cpp
#include <iostream>
#include <cmath>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
// 有关矩阵的代数运算
#include <Eigen/Dense>
#include <chrono>
using namespace std;
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;
double inv_sigma = 1.0/ w_sigma;
cv::RNG rng; // OpenCV随机数产生器
// 创建两个容器
vector<double> x_data,y_data;
for(int i = 0;i<N;i++)
{
// 100 个 数据点
double x = i / 100.0;
x_data.push_back(x);
// 实际的 y = exp(ax^2 + bx + c) + w
// rng.gaussian(w_sigma * w_sigma) 产生一个高斯分布的随机数 服从均值为0 方差为 (w_sigma)^2
y_data.push_back(exp(ar*x*x + br*x +cr) + rng.gaussian(w_sigma * w_sigma));
}
// 开始高斯牛顿迭代
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++)
{
Eigen::Matrix3d H = Eigen::Matrix3d::Zero(); // 左边的系数 H 矩阵
Eigen::Vector3d b = Eigen::Vector3d::Zero(); // 右边的系数 g
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);
Eigen::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 求解增量 x
Eigen::Vector3d dx = H.ldlt().solve(b); // Eigen 库中的LDLT分解
// isnan()用于检查给定的值是否为NaN(非数字)
if(isnan(dx[0]))
{
cout << "result is nam ! " << 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 << "slove time cost = " << time_used.count() << " seconds. " << endl;
cout << "estimated abc = " << ae << " , " << be << " , " << ce << endl;
return 0;
}
CMakeLists.txt:
cmake_minimum_required(VERSION 3.20)
project(GaussNewton)
set(CMAKE_CXX_STANDARD 14)
# 寻找opencv库
find_package(OpenCV 3 REQUIRED)
# 添加OpenCV头文件
include_directories(${OpenCV_INCLUDE_DIRS})
include_directories("/usr/local/include/eigen3")
add_executable(GaussNewton main.cpp)
target_link_libraries(GaussNewton ${OpenCV_LIBS})
运行结果：
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.
slove time cost = 0.00438675 seconds.
estimated abc = 0.890912 , 2.1719 , 0.943629
进程已结束，退出代码为 0
相关链接：
使用opencv中的RNG产生随机数
https://blog.csdn.net/qq_33485434/article/details/78980587
2.利用得到的数据通过matlab绘制曲线
clc
clear
% 噪音数据点
ar = 1.0;br = 2.0;cr = 1.0;
x =[];y=[];
for i = 1:1:100
x(i) = i / 100;
y(i) = exp(ar*x(i)*x(i) + br*x(i) +cr) + 1*randn(1);
end
scatter(x,y);
hold on;
% 真实图
y2 = exp(ar.*x.*x + br.*x +cr);
plot(x,y2,'Color',[1,0,0]);
% 逼近的参数
ar=0.890912 ;br=2.1719 ; cr=0.943629;
y3 = exp(ar.*x.*x + br.*x +cr);
plot(x,y3,'Color',[0,1,0]);
显示结果:
3.使用Ceres进行曲线拟合
CMakeLists.txt :
cmake_minimum_required(VERSION 3.20)
project(ceresCurveFitting)
set(CMAKE_CXX_STANDARD 14)
# 寻找opencv 库
find_package(OpenCV 3 REQUIRED)
# 寻找Ceres 库
find_package(Ceres REQUIRED)
# 添加opencv头文件
include_directories(${Opencv_INCLUDE_DIRS})
# 添加 Ceres 库头文件
include_directories(${CERES_INCLUDE_DIRS})
# 添加 Eigen 头文件
include_directories("/usr/local/include/eigen3")
add_executable(ceresCurveFitting main.cpp)
# 链接库 对于CERES库是 CERES_LIBRARIES 而非 CERES_LIBS
target_link_libraries(ceresCurveFitting ${OpenCV_LIBS} ${CERES_LIBRARIES})
报错：写CMakeLists.txt时，链接 Ceres 的库 ，应该使用CERES_LIBRARIES，而非CERES_LIBS
/usr/bin/ld: CMakeFiles/ceresCurveFitting.dir/main.cpp.o: in function `main':
/home/zxz/my_slam14/ch6/ceresCurveFitting/main.cpp:44: undefined reference to `ceres::Problem::Problem()'
/usr/bin/ld: /home/zxz/my_slam14/ch6/ceresCurveFitting/main.cpp:48: undefined reference to `ceres::Problem::AddResidualBlock(ceres::CostFunction*, ceres::LossFunction*, double*)'
/usr/bin/ld: /home/zxz/my_slam14/ch6/ceresCurveFitting/main.cpp:60: undefined reference to `ceres::Solver::Summary::Summary()'
/usr/bin/ld: /home/zxz/my_slam14/ch6/ceresCurveFitting/main.cpp:63: undefined reference to `ceres::Solve(ceres::Solver::Options const&, ceres::Problem*, ceres::Solver::Summary*)'
/usr/bin/ld: /home/zxz/my_slam14/ch6/ceresCurveFitting/main.cpp:69: undefined reference to `ceres::Solver::Summary::BriefReport[abi:cxx11]() const'
/usr/bin/ld: /home/zxz/my_slam14/ch6/ceresCurveFitting/main.cpp:44: undefined reference to `ceres::Problem::~Problem()'
/usr/bin/ld: /home/zxz/my_slam14/ch6/ceresCurveFitting/main.cpp:44: undefined reference to `ceres::Problem::~Problem()'
/usr/bin/ld: CMakeFiles/ceresCurveFitting.dir/main.cpp.o: in function `ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0>::AutoDiffCostFunction(CURVE_FITTING_COST*)':
/usr/local/include/ceres/autodiff_cost_function.h:174: undefined reference to `google::LogMessageFatal::LogMessageFatal(char const*, int, google::CheckOpString const&)'
/usr/bin/ld: /usr/local/include/ceres/autodiff_cost_function.h:174: undefined reference to `google::LogMessage::stream()'
...
main.cpp:
#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){}
// 残差计算(重载()运算符) // 模型参数 abc 有三维
// template <class T> 模板函数写法
template <class T> bool operator()(const T *const abc,T *residual) const
{
// y-exp(ax^2+bx+c)
residual[0]= T(_y) - ceres::exp(abc[0]*T(_x)*T(_x)+abc[1]*T(_x)+abc[2]);
return true;
}
const double _x,_y; // _x _y 数据
};
int main() {
/*第一部分生成观测数据 xi yi*/
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值
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);
// rng.gaussian(w_sigma*w_sigma) 添加一个 均值0 方差为w_sigma*w_sigma的高斯分布噪音
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(
// 自动求导 AutoDiffCostFunction需要传入输入维度和输出维度 这里的误差项是标量，维度为 1；优化的是 a, b, c 三个量，维度为 3
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.max_num_iterations = 100; // 设置最大迭代次数 （若不设置最大为50）
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 << "slove time cost = " << time_used.count() << " seconds. " << endl;
/*** 第四部分: 输出结果 ***/
// Termination: CONVERGENCE（收敛） 求解器终止原因
cout << summary.BriefReport() << endl;
cout << "estimated a,b,c = ";
for(auto a:abc)
cout << a << " ";
cout << endl;
return 0;
}
输出结果:
/home/zxz/my_slam14/ch6/ceresCurveFitting/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.06e-03 1.12e-03
1 1.884440e+05 1.41e+06 4.86e+05 9.88e-01 8.82e-01 1.81e+04 1 1.03e-03 6.95e-03
2 1.784821e+04 1.71e+05 6.78e+04 9.89e-01 9.06e-01 3.87e+04 1 8.74e-04 7.85e-03
3 1.099631e+03 1.67e+04 8.58e+03 1.10e+00 9.41e-01 1.16e+05 1 8.08e-04 8.70e-03
4 8.784938e+01 1.01e+03 6.53e+02 1.51e+00 9.67e-01 3.48e+05 1 5.12e-03 1.38e-02
5 5.141230e+01 3.64e+01 2.72e+01 1.13e+00 9.90e-01 1.05e+06 1 8.57e-04 1.47e-02
6 5.096862e+01 4.44e-01 4.27e-01 1.89e-01 9.98e-01 3.14e+06 1 8.42e-04 1.57e-02
7 5.096851e+01 1.10e-04 9.53e-04 2.84e-03 9.99e-01 9.41e+06 1 8.71e-04 1.65e-02
slove time cost = 0.0166348 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
进程已结束，退出代码为 0
注意：
为了让 Ceres 帮我们求解这个问题，我们需要做以下几件事：
- 定义每个参数块。参数块通常为平凡的向量，但是在 SLAM 里也可以定义成四元数、李代数这种特殊的结构。如果是向量，那么我们需要为每个参数块分配一个 double 数组来存储变量的值。
- 定义残差块的计算方式。残差块通常关联若干个参数块，对它们进行一些自定义的计算，然后返回残差值。Ceres 对它们求平方和之后，作为目标函数的值。
- 残差块往往也需要定义雅克比的计算方式。在 Ceres 中，可以使用它提供的“自动求导”功能，也可以手动指定雅克比的计算过程。如果要使用自动求导，那么残差块需要按照特定的写法书写：残差的计算过程应该是一个带模板的括号运算符。
- 把所有的参数块和残差块加入 Ceres 定义的 Problem 对象中，调用 Solve 函数求解即可。求解之前，我们可以传入一些配置信息，例如迭代次数、终止条件等，也可以使用默认的配置。
说明：
- 定义残差块的类。方法是书写一个类（或结构体），并在类中定义带模板参数的 () 运算符，这样该类就成为了一个拟函数（Functor）。这种定义方式使得 Ceres 可以像调用函数一样，对该类的某个对象（比如说 a）调用 a() 方法。事实上，Ceres 会把雅克比矩阵作为类型参数传入此函数，从而实现自动求导的功能
- 程序中的 double abc[3] 即参数块，而对于残差块，我们对每一个数据构造 CURVE_FITTING_COST 对象，然后调用 AddResidualBlock 将误差项添加到目标函数中。由于优化需要梯度，我们有若干种选择
- 使用 Ceres 的自动求导（Auto Diff）
- 使用数值求导（Numeric Diff）
- 自行推导解析的导数形式，提供给 Ceres
- 自动求导需要指定误差项和优化变量的维度(即AutoDiffCostFunction需要传入输入维度和输出维度)。这里的误差项是标量，维度为 1；优化的是 a, b, c 三个量，维度为 3。于是，在自动求导类 AutoDiffCostFunction 的模板参数中设定变量维度为 1、3。
- 设定好问题后，调用 Solve 函数进行求解。你可以在 options 里配置（非常详细的）优化选项。例如，我们可以选择使用 Line Search 还是 Trust Region，迭代次数，步长等等。读者可以查看 Options 的定义，看看有哪些优化方法可选，当然默认的配置已经可以用在很广泛的问题上了。
相关链接（重点）：
// 视觉SLAM十四讲学习6 Ceres Solver (1) 入门
https://blog.csdn.net/qq_41035283/article/details/122244578?spm=1001.2101.3001.6661.1&utm_medium=distribute.pc_relevant_t0.none-task-blog-2%7Edefault%7ECTRLIST%7ERate-1-122244578-blog-122697140.pc_relevant_3mothn_strategy_and_data_recovery&depth_1-utm_source=distribute.pc_relevant_t0.none-task-blog-2%7Edefault%7ECTRLIST%7ERate-1-122244578-blog-122697140.pc_relevant_3mothn_strategy_and_data_recovery&utm_relevant_index=1
// Ceres的Problem::AddResidualBlock()函数的介绍：
链接: https://zhuanlan.zhihu.com/p/567251728
// Ceres Solver介绍：包含 void Solve(const Solver::Options& options, Problem* problem, Solver::Summary* summary) 函数三个参数
// 1. Solver::Options 用来设置优化的参数 迭代次数 步长 线性求解器类型
// 2. Problem 用于构建最小二乘问题 设置误差函数 向问题中添加误差（残差）块
// 3. Solver::Summary 输出优化过程及结果
链接: https://blog.csdn.net/xiaojinger_123/article/details/123330824#:~:text=Ceres%20Solver%E4%BB%8B%E7%BB%8D%201%201%E3%80%81%E5%8F%82%E6%95%B0%E4%B8%80%20Solver%3A%3AOptions%20Solver%3A%3AOptions%20%E7%94%A8%E6%9D%A5%E8%AE%BE%E7%BD%AE%E4%BC%98%E5%8C%96%E7%9A%84%E5%8F%82%E6%95%B0%E3%80%82%20%E5%B8%B8%E7%94%A8%E7%9A%84%E5%B0%B1%E6%98%AF%E8%B0%83%E4%B8%80%E4%BA%9B%E8%BF%AD%E4%BB%A3%E7%9A%84%E6%AC%A1%E6%95%B0%EF%BC%8C%E6%AD%A5%E9%95%BF%EF%BC%8C%E7%BA%BF%E6%80%A7%E6%B1%82%E8%A7%A3%E5%99%A8%E7%9A%84%E7%B1%BB%E5%9E%8B%E4%B9%8B%E7%B1%BB%E7%9A%84%E3%80%82,...%203%203%E3%80%81%E5%8F%82%E6%95%B0%E4%B8%89%20Solver%3A%3ASummary%20%E8%BE%93%E5%87%BA%E4%BC%98%E5%8C%96%E8%BF%87%E7%A8%8B%E5%8F%8A%E7%BB%93%E6%9E%9C%20termination_type%20%E6%B1%82%E8%A7%A3%E5%99%A8%E7%BB%88%E6%AD%A2%E7%9A%84%E5%8E%9F%E5%9B%A0%EF%BC%9A%20
4.使用g2o进行曲线拟合
g2o安装链接:
https://github.com/RainerKuemmerle/g2o/tree/9b41a4ea5ade8e1250b9c1b279f3a9c098811b5a
// git clone 或者 直接 loaddown下来
报错
在CMakeLists.txt添加如下，寻找G2o库代码：
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake )
# 寻找 G2O
find_package(G2O REQUIRED) # G2O均大写
# 添加 G2O 头文件
include_directories(${G2o_INCLUDE_DIRS})
# 链接 G2O 库
target_link_libraries(g2oCurveFitting ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY})
出现如下报错：
CMake Error at CMakeLists.txt:6 (find_package):
By not providing "FindG2o.cmake" in CMAKE_MODULE_PATH this project has
asked CMake to find a package configuration file provided by "G2o", but
CMake did not find one.
Could not find a package configuration file provided by "G2o" with any of
the following names:
G2oConfig.cmake
g2o-config.cmake
Add the installation prefix of "G2o" to CMAKE_PREFIX_PATH or set "G2o_DIR"
to a directory containing one of the above files. If "G2o" provides a
separate development package or SDK, be sure it has been installed.
原因
原因在于CMakeLists.txt中，虽然定义了CMAKE_MODULE_PATH，但没有对应的文件和文件夹，也就是没有找到FindG2O.cmake文件
解决办法：
找到findg2o.cmake文件的位置，一般在g2o安装包中的"cmake_modules"文件夹中，将整个文件夹复制到相应工程的下面，与build文件夹在同一目录，然后编译可以正常通过，或者将cmake_modules 文件夹下的 findg2o.cmake 文件复制到当前工程的 build 目录下，并在 CMakeLists.txt中添加如下代码：
list( APPEND CMAKE_MODULE_PATH {当前findg20.cmake文件路径} )
list( APPEND CMAKE_MODULE_PATH /home/zxz/my_slam14/ch6/g2oCurveFitting/cmake-build-debug )
# 寻找 G2O
find_package(G2O REQUIRED) # G2O均大写
# 添加 G2O 头文件
include_directories(${G2o_INCLUDE_DIRS})
# 链接 G2O 库
target_link_libraries(g2oCurveFitting ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY})
代码
CMakeLists.txt
cmake_minimum_required(VERSION 3.16)
project(g2oCurveFitting)
set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CXX_STANDARD 14)
list( APPEND CMAKE_MODULE_PATH /home/zxz/my_slam14/ch6/g2oCurveFitting/cmake-build-debug )
# 寻找 G2O
find_package(G2O REQUIRED)
find_package(OpenCV 3 REQUIRED)
include_directories("/usr/local/include/eigen3")
# 添加 G2O 头文件
include_directories(${G2o_INCLUDE_DIRS})
include_directories(${Opencv_INCLUDE_DIRS})
add_executable(g2oCurveFitting main.cpp)
# 链接 opencv 库
target_link_libraries(g2oCurveFitting ${OpenCV_LIBS})
# 链接 G2O 库
target_link_libraries(g2oCurveFitting ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY})
main.cpp
步骤：
- 定义顶点与边的类型
- 构建图
- 选择优化算法
- 调用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;
/*** 第一部分 ： 定义顶点和边的类型 顶点：优化变量 边：误差项 ***/
// 曲线模型的顶点，模板参数，优化变量维度和数据类型 Vertex(释义：顶点)
class CurveFittingVertex : public g2o::BaseVertex<3,Eigen::Vector3d>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// 重置 （重写基类中的虚函数）
// setToOriginImpl 顶点重置函数（将估计值置为0（实际置为多少都无所谓，因为在迭代过程中会将初始估计的值进行优化） ）
virtual void setToOriginImpl() override{
_estimate << 0,0,0 ;
}
// 更新 (oplusImpl 顶点（优化变量-状态量）的更新函数) 14讲书中有详解
virtual void oplusImpl(const double *update) override{
_estimate += Eigen::Vector3d(update); // 若优化变量为 相机位姿 就不能简单的定义为加法，李群与李代数(左乘更新or右乘更新)
}
// 存盘与读盘 ：留空
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) {}
// 计算曲线模型误差
// computeError 边的误差计算函数（需要取出当前边所链接的顶点的当前估计值，根据曲线模型，与其观测值进行比较，同最小二乘的误差模型）
// 此处为 1元边 每一个边只链接一个顶点 _vertices[0] 索引为 0
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() {
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
// 线性求解器类型（LinearSolverDense ：使用dense cholesky分解法，继承自LinearSolver）
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;
}
可在相关Blog中看代码解析：
// g2o 曲线拟合
https://blog.csdn.net/qq_28087491/article/details/109499017
// g2o图优化中的要点与难点
https://blog.csdn.net/zkk9527/article/details/89673896
// SLAM从0到1——6. 图优化g2o：从看懂代码到动手编写（长文） 这篇文章巨棒
https://zhuanlan.zhihu.com/p/121628349
运行结果
/home/zxz/my_slam14/ch6/g2oCurveFitting/cmake-build-debug/g2oCurveFitting
iteration= 0 chi2= 376785.128234 time= 1.0333e-05 cumTime= 1.0333e-05 edges= 100 schur= 0
iteration= 1 chi2= 35673.566018 time= 5.365e-06 cumTime= 1.5698e-05 edges= 100 schur= 0
iteration= 2 chi2= 2195.012304 time= 4.702e-06 cumTime= 2.04e-05 edges= 100 schur= 0
iteration= 3 chi2= 174.853126 time= 4.703e-06 cumTime= 2.5103e-05 edges= 100 schur= 0
iteration= 4 chi2= 102.779695 time= 4.481e-06 cumTime= 2.9584e-05 edges= 100 schur= 0
iteration= 5 chi2= 101.937194 time= 4.397e-06 cumTime= 3.3981e-05 edges= 100 schur= 0
iteration= 6 chi2= 101.937020 time= 3.739e-06 cumTime= 3.772e-05 edges= 100 schur= 0
iteration= 7 chi2= 101.937020 time= 3.733e-06 cumTime= 4.1453e-05 edges= 100 schur= 0
iteration= 8 chi2= 101.937020 time= 4.462e-06 cumTime= 4.5915e-05 edges= 100 schur= 0
iteration= 9 chi2= 101.937020 time= 4.522e-06 cumTime= 5.0437e-05 edges= 100 schur= 0
start optimization
solve time cost = 0.000670046seconds.
estimated model: 0.890912 2.1719 0.943629
进程已结束，退出代码为 0
使用高斯牛顿进行梯度下降，在迭代9次之后得到优化结果，与Ceres以及手写高斯牛顿几乎相差无几。但是运行速度，手写高斯牛顿大于g2o,而g2o快于Ceres。**通用性与高效性往往是相矛盾的，**在Ceres中使用了自动求导，且求解其配置与高斯牛顿还并非完全一致，因此看起来慢。
相关c++语法
如果派生类在虚函数声明时使用了override描述符，那么该函数必须重载其基类中的同名函数，否则代码将无法通过编译。
如果派生类里面是像重载虚函数 就加上关键字override 这样编译器可以辅助检查是不是正确重载，如果没加这个关键字 也没什么严重的error 只是少了编译器检查的安全性
在本次实验中如：
// 重写基类的函数
virtual void setToOriginImpl() override{
_estimate << 0,0,0 ;
}
g2o定义顶点
从佬的Blog中学习来：
相关实践大佬Blog中也有，就不引过来了
https://blog.csdn.net/weixin_42905141/article/details/100827638
计算机视觉life:
https://blog.csdn.net/electech6/article/details/88018481?spm=1001.2101.3001.6661.1&utm_medium=distribute.pc_relevant_t0.none-task-blog-2%7Edefault%7ECTRLIST%7ERate-1-88018481-blog-100827638.pc_relevant_3mothn_strategy_recovery&depth_1-utm_source=distribute.pc_relevant_t0.none-task-blog-2%7Edefault%7ECTRLIST%7ERate-1-88018481-blog-100827638.pc_relevant_3mothn_strategy_recovery&utm_relevant_index=1
g2o顶点参数：
- D是int 类型的，表示vertex的最小维度，比如3D空间中旋转是3维的，那么这里 D = 3
- T是待估计vertex的数据类型，比如用四元数表达三维旋转的话，T就是Quaternion 类型
D并非是顶点（更确切的说是状态变量）的维度，而是其在流形空间（manifold）的最小表示
typedef T EstimateType;
EstimateType _estimate; // 在slam14实践程序中，a b c三个状态量，通过向量的形式保存，在此实验中_estimate的数据类型Eigen::Vector3d 即为 T
定义顶点：
VertexSE2 : public BaseVertex<3, SE2> //2D pose Vertex, (x,y,theta)
VertexSE3 : public BaseVertex<6, Isometry3> //6d vector (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion)
VertexPointXY : public BaseVertex<2, Vector2>
VertexPointXYZ : public BaseVertex<3, Vector3>
VertexSBAPointXYZ : public BaseVertex<3, Vector3>
// SE3 Vertex parameterized internally with a transformation matrix and externally with its exponential map
VertexSE3Expmap : public BaseVertex<6, SE3Quat>
// SBACam Vertex, (x,y,z,qw,qx,qy,qz),(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.
// qw is assumed to be positive, otherwise there is an ambiguity in qx,qy,qz as a rotation
VertexCam : public BaseVertex<6, SBACam>
// Sim3 Vertex, (x,y,z,qw,qx,qy,qz),7d vector,(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.
VertexSim3Expmap : public BaseVertex<7, Sim3>
这些顶点类型是可以直接用的，但是此外的就需要我们自己重写定义
重新定义顶点一般需要考虑重写如下函数：
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void oplusImpl(const number_t* update);
virtual void setToOriginImpl();
read，write：分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以setToOriginImpl：顶点重置函数，设定被优化变量的原始值oplusImpl：顶点更新函数。非常重要的一个函数，主要用于优化过程中增量△x的计算。我们根据增量方程计算出增量之后，就是通过这个函数对估计值进行调整的
eg1:
在slam14中曲线拟合的实例
// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void setToOriginImpl() // 重置
{
_estimate << 0,0,0; // 将优化变量初值设置为 0 0 0
}
virtual void oplusImpl( const double* update ) // 更新
{
_estimate += Eigen::Vector3d(update);
}
// 存盘和读盘：留空
virtual bool read( istream& in ) {}
virtual bool write( ostream& out ) const {}
};
顶点类型是Eigen::Vector3d，属于向量，是可以通过加法来更新的，有些例子就不行，比如下面这个复杂点例子：李代数表示位姿VertexSE3Expmap
eg2:
g2o/types/sba/types_six_dof_expmap.h (三维刚体的位姿)
g2o/types/sba/types_six_dof_expmap.h
/**
\* \brief SE3 Vertex parameterized internally with a transformation matrix
and externally with its exponential map
*/
class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat>{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
VertexSE3Expmap();
bool read(std::istream& is);
bool write(std::ostream& os) const;
virtual void setToOriginImpl() {
_estimate = SE3Quat();
}
virtual void oplusImpl(const number_t* update_) {
Eigen::Map<const Vector6> update(update_);
setEstimate(SE3Quat::exp(update)*estimate()); //更新方式 使用左乘扰动更新
}
};
- 第一个参数6 表示内部存储的优化变量维度，这是个6维的李代数
se3 - 第二个参数是优化变量的类型，这里使用了g2o定义的相机位姿类型：SE3Quat
可以具体查看g2o/types/slam3d/se3quat.h
它内部使用了四元数表达旋转，然后加上位移来存储位姿，同时支持李代数上的运算，比如对数映射（log函数）、李代数上增量（update函数）等操作
位姿不能直接加，变换矩阵不满足加法封闭
相机位姿顶点类VertexSE3Expmap使用了李代数表示相机位姿，因为旋转矩阵是有约束的矩阵，它必须是正交矩阵且行列式为1。使用它作为优化变量就会引入额外的约束条件，从而增大优化的复杂度。而将旋转矩阵通过李群-李代数之间的转换关系转换为李代数表示，就可以把位姿估计变成无约束的优化问题，求解难度降低。
向图中添加顶点
// setEstimate(type) 函数来设定初始值；setId(int) 定义节点编号
CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate( Eigen::Vector3d(0,0,0) );
v->setId(0);
optimizer.addVertex( v );
g2o定义边
从佬的Blog中学习来：
https://xiaohu.blog.csdn.net/article/details/100830126?spm=1001.2014.3001.5502
边上的继承关系如下图所示：
对应的文件如下：
- g2o/g2o/core/hyper_graph.h
- g2o/g2o/core/optimizable_graph.h
- g2o/g2o/core/base_edge.h
BaseUnaryEdge，BaseBinaryEdge，BaseMultiEdge 分别表示一元边，两元边，多元边。
一元边你可以理解为一条边只连接一个顶点，两元边理解为一条边连接两个顶点，也就是我们常见的边啦，多元边理解为一条边可以连接多个（3个以上）顶点
g2o一元边的参数
- D 是 int 型，表示测量值的维度 （dimension）
- E 表示测量值的数据类型
- VertexXi，VertexXj 分别表示不同顶点的类型
例如：slam14曲线拟合实验中：
class CurveFittingEdge : public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>{}
// 上述代码中的：
g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
// 类模板中的模板类型：
// 第一个参数 1 表示 观测量是 1维的
// 第二个参数 double 表示观测量的类型
// 第三个参数 CurveFittingVertex 表示 边链接顶点的类型，此处顶点的类型，就是实验中我们重写定义的顶点（派生类类名）
除了输入参数外，定义边我们通常需要复写一些重要的成员函数，顶点里主要复写了顶点更新函数oplusImpl和顶点重置函数setToOriginImpl
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void computeError(); // 尤为重要
virtual void linearizeOplus(); // 尤为重要
read，write：分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以computeError函数：非常重要，是使用当前顶点的值计算的测量值与真实的测量值之间的误差linearizeOplus函数：非常重要，是在当前顶点的值下，该误差对优化变量的偏导数，也就是我们说的Jacobian （雅可比矩阵）
（相当重要）除了上面几个成员函数，还有几个重要的成员变量和函数也一并解释一下：
_measurement：存储观测值
_error：存储computeError() 函数计算的误差
_vertices[]：存储顶点信息，比如二元边的话，_vertices[] 的大小为2，存储顺序和调用setVertex(int, vertex) 是设定的int 有关（0 或1）
setId(int)：来定义边的编号（决定了在H矩阵中的位置）
setMeasurement(type) 函数来定义观测值
setVertex(int, vertex) 来定义顶点
setInformation() 来定义协方差矩阵的逆 (信息矩阵)
2237
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
