宁德做网站,什么事网页设计,自费社保太坑了亏大了,wordpress文章表格文章目录 06 g2o 学习6.1 概念6.2 框架简介6.3 代码示例 06 g2o 学习
6.1 概念
g2o(General Graphic Optimization)是基于图优化的库。图优化是把优化问题表现成图的一种方式。一个图由若干个顶点(Vertex)#xff0c;以及连接这这些顶点的边(Edge)组成。用顶点表示优化变量以及连接这这些顶点的边(Edge)组成。用顶点表示优化变量用边表示误差项。
那么在 SLAM 中不同时刻的位姿和路标点为待优化变量即顶点将他们之间的观测作为边。数学表述为传感器的观测方程 z k h ( x k ) z_{k}h\left(x_{k}\right) zkh(xk)
实际上二者并不会相等而是有误差存在 e k z k − h ( x k ) e_kz_k-h\left(x_k\right) ekzk−h(xk)
于是位姿 x k x_k xk 和 路标 z k z_k zk 为待优化变量图中节点误差 e k e_k ek 为约束红色虚线。 6.2 框架简介 几个需要注意的点
1迭代形式为 H Δ x b H\Delta xb HΔxb也就是求出每次迭代步长 Δ x \Delta x Δx三个算法可选高斯牛顿、LM 和 Dog-Leg
2定义顶点顶点也就是待优化变量它继承自基础类 BaseVertexD, T其中 D 为 int 类型表示维度T 为数据类型。例如
g2o::BaseVertex3, Eigen::Vector3d // 三维点Eigen::Vector3d 类型g2o::BaseVertex6, SE3Quat // SE3 变换矩阵6个参数平移旋转3顶点更新对于一般的函数更新策略是 x k 1 x k Δ x x_{k1}x_k\Delta x xk1xkΔx也就是加上求出的 Δ x \Delta x Δx而对于位姿 S E ( 3 ) SE(3) SE(3) 这样的数据类型类型来说要用乘法。
// 顶点更新函数
void curveVetex::oplusImpl(const double* update)
{_estimate Eigen::Vector3d(update); // 加法更新
}4添加顶点有多少个顶点就添加多少个。
// 新建顶点
curveVetex* v new curveVetex(); // 自定义顶点类型
v-setEstimate(Eigen::Vector3d(0,0,0)); // 初始化
v-setId(0); // 设置 Id
optimizer.addVertex(v); // 加入优化器中5定义边包括一元边、二元边和多元边。误差测量值-估计值 以二元边为例
g2o::BaseBinaryEdgeerrorDim, errorType, Vertex1Type, Vertex2Type // Vertex1Type 连接的顶点的类名6添加边
EdgePointOnCurve* e new EdgePointOnCurve;
e-setId(0); // 设置 Id
e-setInformation(Eigen::Matrixdouble,1,1::Identity()); // 信息矩阵
e-setVertex(0, v); // 设置连接的顶点
e-setMeasurement(y_data[i]); // 观测值
optimizer.addEdge(e); 7主要步骤
定义顶点和边的类型构建图添加顶点和边选择优化算法调用 g2o 进行优化返回结果
通用模板
// 1. 定义顶点
class curveVetex: public g2o::BaseVertex3,Eigen::Vector3d
{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWvirtual void setToOriginImpl(); // 顶点初始值置零virtual void oplusImpl(const double* update); // 更新virtual bool read(std::istream is); // 读盘、存盘留空即可virtual bool write(std::ostream os) const;
};// 2. 添加顶点
curveVetex* v new curveVetex(); // 自定义顶点类型
v-setEstimate(Eigen::Vector3d(0,0,0)); // 初始化
v-setId(0); // 设置 Id
optimizer.addVertex(v); // 加入优化器中// 3. 定义边
class myEdge: public g2o::BaseBinaryEdgeerrorDim, errorType, Vertex1Type, Vertex2Type
{
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWmyEdge(){}// 计算曲线模型误差测量值-估计值void computeError();//存取bool read(std::istream is);bool write(std::ostream os) const;// 增量计算函数误差对优化变量的偏导数void linearizeOplus();
}// 4. 添加边
EdgePointOnCurve* e new EdgePointOnCurve;
e-setId(0); // 设置 Id
e-setInformation(Eigen::Matrixdouble,1,1::Identity()); // 信息矩阵
e-setVertex(0, v); // 设置连接的顶点
e-setMeasurement(y_data[i]); // 观测值
optimizer.addEdge(e); 6.3 代码示例
拟合函数 y exp ( a x 2 b x c ) y\exp(ax^2bxc) yexp(ax2bxc)。
显然待优化变量为 a b c abc abc只有一个顶点误差值观测值实际值-估计值理论值一元边。 需要注意的是这里用的三种优化算法都是误差 e i e_i ei 对代优化变量的偏导数而不是 F ( x ) \boldsymbol{F}(x) F(x)即 e i y i − e x p ( a x i 2 b i x c ) e_iy_i-exp(ax_i^2b_ixc) eiyi−exp(axi2bixc) ∂ e i ∂ a − x i 2 exp ( a x i 2 b i x c ) ∂ e i ∂ b − x i exp ( a x i 2 b i x c ) ∂ e i ∂ c − exp ( a x i 2 b i x c ) \frac{ \partial e_i }{ \partial a}-x_i^2\exp(ax_i^2b_ixc) \\ \frac{ \partial e_i }{ \partial b}-x_i\exp(ax_i^2b_ixc) \\ \frac{ \partial e_i }{ \partial c}-\exp(ax_i^2b_ixc) ∂a∂ei−xi2exp(axi2bixc)∂b∂ei−xiexp(axi2bixc)∂c∂ei−exp(axi2bixc)
关于 g2o 使用的几个问题
1安装
安装依赖项
sudo apt-get install libqt4-dev qt4-qmake libqglviewer-dev libsuitesparse-dev libcxsparse3.1.2 libcholmod-dev安装下列命令依次执行安装
git clone https://github.com/RainerKuemmerle/g2o.git
cd g2o
mkdir build
cd build
cmake ..
make
sudo make install安装完成后在目录/usr/local/includ 下能找到 g2o 目录在 /usr/local/lib 下能找到libg2o_**.so的文件。
2cmakelists.txt
include_directories( ${G2O_INCLUDE_DIRS})
SET(G2O_LIBS g2o_cli g2o_ext_freeglut_minimal g2o_simulator g2o_solver_slam2d_linear g2o_types_icp g2o_types_slam2d g2o_core g2o_interface g2o_solver_csparse g2o_solver_structure_only g2o_types_sba g2o_types_slam3d g2o_csparse_extension g2o_opengl_helper g2o_solver_dense g2o_stuff g2o_types_sclam2d g2o_parser g2o_solver_pcg g2o_types_data g2o_types_sim3 cxsparse )include_directories(/usr/include/eigen3)add_executable(g2oCurveFitting ./src/g2oCurveFitting.cpp)
target_link_libraries(g2oCurveFitting ${G2O_LIBS})3编译过程中遇到如下错误
error while loading shared libraries: libg2o_core.so: cannot open shared object file: No such file or directory
出现这个问题的主要原因是新安装的 g2o 没能生效。执行
sudo ldconfig代码
/*********************************************************** *
* Time: 2023/8/27
* Author: xiaocong
* Function: g2o
* 注意这里用的是 ae * xi * xi be * xi sin(ce)
***********************************************************/#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_gauss_newton.h
#include g2o/core/optimization_algorithm_levenberg.h
#include g2o/core/optimization_algorithm_dogleg.h
#include g2o/solvers/dense/linear_solver_dense.h
#include eigen3/Eigen/Core
#include cmathconst int N 100; // 数据点个数using namespace std;// 定义顶点即待优化变量 abc
class CurveFittingVertex : public g2o::BaseVertex3, Eigen::Vector3d
{
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW// 设置初始值virtual void setToOriginImpl(){_estimate 0, 0, 0;}// 更新直接加上 delta_xvirtual void oplusImpl(const double* update){_estimate Eigen::Vector3d(update);}// 存盘和读盘留空virtual bool read(istream in) {}virtual bool write(ostream out) const {}
};// 定义边包括误差项及其对优化变量的偏导数
class CurveFittingEdge : public g2o::BaseUnaryEdge1, double, CurveFittingVertex
{
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;// 构造函数CurveFittingEdge(double x) : _x(x) {}// 计算曲线模型误差测量值-估计值virtual void computeError(){// 取出 _vertices 中的第一个顶点强制转换为 CurveFittingVertex* 类型const CurveFittingVertex* v static_castconst 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(){const CurveFittingVertex* v static_castconst 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; // 真实参数值// 生成数据vectordouble x_data, y_data;for (int i 0; i N; i){double xi i / 100.0; // [0~1]double sigma 0.02 * (rand() % 1000) / 1000.0 - 0.01; // 随机噪声[-0.01, 0.01]double yi exp(ar * xi * xi br * xi cr) sigma;x_data.push_back(xi);y_data.push_back(yi);}// 构建图优化typedef g2o::BlockSolver g2o::BlockSolverTraits3, 1 Block; // 每个误差项优化变量维度为3误差值维度为1Block::LinearSolverType* linearSolver new g2o::LinearSolverDenseBlock::PoseMatrixType(); // 线性方程求解器Block* solver_ptr new Block(linearSolver); // 矩阵块求解器// 高斯牛顿法优化 g2o::OptimizationAlgorithmGaussNewton* solver new g2o::OptimizationAlgorithmGaussNewton(solver_ptr);g2o::SparseOptimizer optimizer; // 图模型optimizer.setAlgorithm(solver); // 设置求解器optimizer.setVerbose(true); // 打开调试输出// 向图中添加顶点只有一个顶点CurveFittingVertex* v new CurveFittingVertex();v-setEstimate(Eigen::Vector3d(0, 0, 0)); // 初始值v-setId(0); // 顶点序号optimizer.addVertex(v); // 加入优化器// 向图中添加边for (int i 0; i N; i){CurveFittingEdge* e new CurveFittingEdge(x_data[i]);e-setId(i); // 设置 Ide-setVertex(0, v); // 连接的顶点e-setMeasurement(y_data[i]); // 观测值e-setInformation(Eigen::Matrixdouble, 1, 1::Identity()); // 信息矩阵optimizer.addEdge(e); // 加入优化器}// 执行优化cout start optimization endl;optimizer.initializeOptimization();optimizer.optimize(100); // 最大迭代次数// 输出优化值Eigen::Vector3d abc_estimate v-estimate();cout result: abc_estimate.transpose() endl;return 0;
}结果
result: 0.999313 2.00098 0.999658
