slam十四讲之第六讲g2oCurveFitting代码

#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;//定义顶点的模板，后面会调用，BaseVertex为顶点最底层的基类，定义 最小维数是3维，类型为三维向量
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW//让操作数内存对齐// 重置virtual void setToOriginImpl() override { //函数初始化set 建立 to 变成 origin 原始 Impl 函数_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 {}//写入
};//定义边的模板，后面会调用，BaseUnaryEdge为顶点最底层的基类，定义 最小维数是1维(一般为误差，就是一个数)，类型为双精度，连接的是顶点
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWCurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}//对父类BaseUnaryEdge初始化，同时初始化成员变量_x// 计算曲线模型误差virtual void computeError() override {const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);// 将其他参数转化成顶点类型参数const Eigen::Vector3d abc = v->estimate();//abc是要估计的_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;//对a求导数_jacobianOplusXi[1] = -_x * y;//对b求_jacobianOplusXi[2] = -y;//对c}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);//给x一些数据y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));}//给y一些数据   需要一些误差，误差服从高斯分布，别说还挺智能的// 构建图优化，先设定g2otypedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;  // 每个误差项优化变量维度为3，误差值维度为1 Trait 特点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;
}

slam十四讲之第六讲带你看懂g2o相关推荐

视觉SLAM十四讲学习笔记-第六讲学习笔记总结（1）---非线性优化原理
第六讲学习笔记如下: 视觉SLAM十四讲学习笔记-第六讲-非线性优化的状态估计问题_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第六讲-非线性优化的非线性最小二乘问题_ ...
视觉SLAM十四讲学习笔记-第六讲-非线性优化的实践-高斯牛顿法和曲线拟合
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习 ...
视觉SLAM十四讲学习笔记-第六讲-非线性优化的非线性最小二乘问题
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习 ...
视觉SLAM十四讲学习笔记-第六讲-非线性优化的状态估计问题
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习 ...
视觉SLAM总结——视觉SLAM十四讲笔记整理
视觉SLAM总结--视觉SLAM十四讲笔记整理说明基础知识点 1. 特征提取.特征匹配 (1)Harris (2)SIFT (3)SUFT (4)ORB (5)特征匹配 2. 2D-2D:对极约束 ...
视觉SLAM十四讲学习笔记-第七讲-视觉里程计-三角测量和实践
专栏汇总视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第 ...
视觉SLAM十四讲学习笔记-第七讲-视觉里程计-对极几何和对极约束、本质矩阵、基础矩阵
专栏系列文章如下: 专栏汇总视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLA ...
视觉SLAM十四讲学习笔记-第七讲-视觉里程计-特征点法和特征提取和匹配实践
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习 ...
视觉SLAM十四讲学习笔记专栏汇总
专栏汇总视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二 ...

slam十四讲之第六讲带你看懂g2o

slam十四讲之第六讲g2oCurveFitting代码

slam十四讲之第六讲带你看懂g2o相关推荐

最新文章

热门文章