首页 > 其他分享 >G2O(1) 基本例子 线性方程组

G2O(1) 基本例子 线性方程组

时间：2024-07-18 22:07:29浏览次数：13  
标签：SOLVER 线性方程组 LIBRARY 例子 g2o G2O include FIND

 

 

 

CMakeLists.txt

cmake_minimum_required(VERSION 3.1)
project(untitled2)

set(CMAKE_CXX_STANDARD 11)
set(CMAKE_BUILD_TYPE Release)

set(ALL_TARGET_LIBRARIES "")


include(cmake/FindG2O.cmake)

#方式1
find_package(Eigen3 REQUIRED)
include_directories("/usr/local/include/eigen3")

#方式2 
#include(cmake/FindEigen3.cmake)


add_executable(fit_curve fit_curve.cpp)
target_link_libraries(fit_curve ${ALL_TARGET_LIBRARIES})

　　

 

fit_curve.cpp

#include <Eigen/Core>
#include <iostream>

#include "g2o/stuff/sampler.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/base_vertex.h"
#include "g2o/core/base_unary_edge.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include "g2o/core/robust_kernel_impl.h"

using namespace std;

/*!
 * 继承BaseVertex类，构造顶点
 */
class VertexParams : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    VertexParams() = default;

    bool read(std::istream & /*is*/) override {
        cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
        return false;
    }

    bool write(std::ostream & /*os*/) const override {
        cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
        return false;
    }

    //该函数作用是更新顶点的估计值
    void setToOriginImpl() override {
        cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
    }

    //更新优化之后的顶点
    void oplusImpl(const double *update) override {
        Eigen::Vector3d::ConstMapType v(update);
        _estimate += v;
    }
};

/*!
 * 从BaseUnaryEdge继承得到一元边
 */
class EdgePointOnCurve : public g2o::BaseUnaryEdge<1, Eigen::Vector2d, VertexParams> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EdgePointOnCurve() = default;

    bool read(std::istream & /*is*/) override {
        cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
        return false;
    }

    bool write(std::ostream & /*os*/) const override {
        cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
        return false;
    }

    //边的误差计算
    void computeError() override {
        const VertexParams *params = dynamic_cast<const VertexParams *>(vertex(0));//顶点
        const double &a = params->estimate()(0);
        const double &b = params->estimate()(1);
        const double &lambda = params->estimate()(2);
        double fval = a * exp(-lambda * measurement()(0)) + b;
        _error(0) = std::abs(fval - measurement()(1));
    }
};

int main(int argc, char **argv) {
    int numPoints = 50;
    int maxIterations = 50;
    bool verbose = true;

    double a = 2.;
    double b = 0.4;
    double lambda = 0.2;
    Eigen::Vector2d *points = new Eigen::Vector2d[numPoints];
    ofstream points_file("../points.txt", ios::out);

    //准备用于拟合的数据
    for (int i = 0; i < numPoints; ++i) {
        double x = g2o::Sampler::uniformRand(0, 10);
        double y = a * exp(-lambda * x) + b;
        y += g2o::Sampler::gaussRand(0, 0.02);

        if (i == 20) {
            x = 8;
            y = 2.5;
        }

        points[i].x() = x;
        points[i].y() = y;
        points_file << x << " " << y << endl;
    }
    points_file.close();

    typedef g2o::BlockSolver<g2o::BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic> > MyBlockSolver;
    typedef g2o::LinearSolverDense<MyBlockSolver::PoseMatrixType> MyLinearSolver;

    g2o::SparseOptimizer optimizer;

    g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(
            g2o::make_unique<MyBlockSolver>(g2o::make_unique<MyLinearSolver>()));

    optimizer.setAlgorithm(solver);

    VertexParams *params = new VertexParams();
    params->setId(0);
    params->setEstimate(Eigen::Vector3d(1, 1, 1));//初始化顶点的估计值
    optimizer.addVertex(params);

    for (int i = 0; i < numPoints; ++i) {
        EdgePointOnCurve *e = new EdgePointOnCurve;
        e->setInformation(Eigen::Matrix<double, 1, 1>::Identity());

        if (i == 20) {
            e->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 10);
        }

        e->setVertex(0, params);
        e->setMeasurement(points[i]);

        g2o::RobustKernelHuber *robust_kernel_huber = new g2o::RobustKernelHuber;
        robust_kernel_huber->setDelta(0.3);
        e->setRobustKernel(robust_kernel_huber);

        optimizer.addEdge(e);
    }

    optimizer.initializeOptimization();
    optimizer.setVerbose(verbose);
    optimizer.optimize(maxIterations);

    ofstream result_file("../result.txt");
    result_file << params->estimate()[0] << " "
                << params->estimate()[1] << " "
                << params->estimate()[2];
    result_file.close();


    cout << endl << "a, b, lambda: "
         << params->estimate()[0] << ", "
         << params->estimate()[1] << ", "
         << params->estimate()[2] << endl;

    delete[] points;

    return 0;
}

　　

FindG2O.cmake

 

找到g2o

# Find the header files

find_path(G2O_INCLUDE_DIR g2o/core/base_vertex.h
        ${G2O_ROOT}/include
        $ENV{G2O_ROOT}/include
        $ENV{G2O_ROOT}
        /usr/local/include
        /usr/include
        /opt/local/include
        /sw/local/include
        /sw/include
        NO_DEFAULT_PATH
        )

# Macro to unify finding both the debug and release versions of the
# libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY
# macro.

macro(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME)

    find_library("${MYLIBRARY}_DEBUG"
            NAMES "g2o_${MYLIBRARYNAME}_d"
            PATHS
            ${G2O_ROOT}/lib/Debug
            ${G2O_ROOT}/lib
            $ENV{G2O_ROOT}/lib/Debug
            $ENV{G2O_ROOT}/lib
            NO_DEFAULT_PATH
            )

    find_library("${MYLIBRARY}_DEBUG"
            NAMES "g2o_${MYLIBRARYNAME}_d"
            PATHS
            ~/Library/Frameworks
            /Library/Frameworks
            /usr/local/lib
            /usr/local/lib64
            /usr/lib
            /usr/lib64
            /opt/local/lib
            /sw/local/lib
            /sw/lib
            )

    find_library(${MYLIBRARY}
            NAMES "g2o_${MYLIBRARYNAME}"
            PATHS
            ${G2O_ROOT}/lib/Release
            ${G2O_ROOT}/lib
            $ENV{G2O_ROOT}/lib/Release
            $ENV{G2O_ROOT}/lib
            NO_DEFAULT_PATH
            )

    find_library(${MYLIBRARY}
            NAMES "g2o_${MYLIBRARYNAME}"
            PATHS
            ~/Library/Frameworks
            /Library/Frameworks
            /usr/local/lib
            /usr/local/lib64
            /usr/lib
            /usr/lib64
            /opt/local/lib
            /sw/local/lib
            /sw/lib
            )

    if (NOT ${MYLIBRARY}_DEBUG)
        if (MYLIBRARY)
            set(${MYLIBRARY}_DEBUG ${MYLIBRARY})
        endif (MYLIBRARY)
    endif (NOT ${MYLIBRARY}_DEBUG)

endmacro(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME)

# Find the core elements
FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff)
FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core)

# Find the CLI library
FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli)

# Find the pluggable solvers
FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod)
FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse)
FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension)
FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense)
FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg)
FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear)
FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only)
FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen)

# Find the predefined types
FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data)
FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp)
FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba)
FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d)
FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3)
FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d)
FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d)

# G2O solvers declared found if we found at least one solver
set(G2O_SOLVERS_FOUND "NO")
if (G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
    set(G2O_SOLVERS_FOUND "YES")
endif (G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)

# G2O itself declared found if we found the core libraries and at least one solver
set(G2O_FOUND "NO")
if (G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
    set(G2O_FOUND "YES")
endif (G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)

include_directories(SYSTEM ${G2O_INCLUDE_DIR})
list(APPEND ALL_TARGET_LIBRARIES
        ${G2O_TYPES_DATA}
        ${G2O_CORE_LIBRARY}
        ${G2O_STUFF_LIBRARY}
        ${G2O_SOLVER_PCG}
        ${G2O_SOLVER_CSPARSE}
        ${G2O_SOLVER_CHOLMOD}
        ${G2O_TYPES_SLAM3D}
        ${G2O_TYPES_SLAM3D_ADDONS})

　　

 

可视化代码

import numpy as np
import matplotlib.pyplot as plt

filename = './points.txt'
X, Y = [], []
with open(filename, 'r') as f:
    lines = f.readlines()
    for line in lines:
        value = [float(s) for s in line.split()]
        X.append(float(value[0]))
        Y.append(float(value[1]))

result_name = './result.txt'
with open(result_name, 'r') as r:
    lines = r.readlines()
    for line in lines:
        value = [float(s) for s in line.split()]
        a = float(value[0])
        b = float(value[1])
        my_lambda = float(value[2])

x = np.linspace(0, 10, 10)
y = a * np.exp(-my_lambda * x) + b

plt.plot(x, y, 'r')
plt.scatter(X, Y)
plt.show()

　　

标签：SOLVER,线性方程组,LIBRARY,例子,g2o,G2O,include,FIND
From： https://www.cnblogs.com/gooutlook/p/18310516

相关文章

  • camke(11)配置g2o
     适配openvslam和slam14讲解代码版本1.Eigen安装(最新3.3.7）wget-qhttps://gitlab.com/libeigen/eigen/-/archive/3.3.7/eigen-3.3.7.tar.bz2tarxfeigen-3.3.7.tar.bz2rm-rfeigen-3.3.7.tar.bz2cdeigen-3.3.7mkdir-pbuild&&cdbuildcmake\-DCMAKE_BU......
  • python 3D例子
    importpygame#导入Pygame库，用于创建游戏窗口和处理事件frompygame.localsimport*#导入Pygame的本地模块，包含常用的变量和函数fromOpenGL.GLimport*#导入OpenGL的核心功能fromOpenGL.GLUTimport*#导入OpenGL的实用工具库fromOpenGL.GLUimpor......
  • HAL库源码移植与使用之FSMC (例子加思路与理解，万字良心保证你能听懂)
    FMC和FSMC是一样的东西，只是FMC更可控地址更多又可以驱动SDRAM,用法都一样！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！！总结：其实fsmc更像是一个有着特定转换时序功能的寄存器，每个fsmc映射在芯片内存里的地址都有一个这样的寄存器，你往这个映射的地址里赋值，这个赋值信息先到达对应fsmc寄存器，他不会像普通寄存器一样直接控制......
  • IO输入输出流例子：Java对象输出json文本：
    读取文件：原始字节输入流(低级)：publicclassCharCacheIOReader{publicstaticvoidmain(String[]args){try(//原始字节输入流(低级)Readerfr=newFileReader("src\\OutputStream.txt");//创建一个字......
  • extjs中treepanel例子
    ：TreePanel继承自Panel，在ExtJS中使用树控件含有丰富的属性和方法实现复杂的功能。其中Ext.tree.TreeNode代表一个树节点，比较常用的属性包括text、id、icon、checked等、异步树Ext.tree.AsyncTreeNode、树加载器Ext.tree.TreeLoader。下面介绍几个extjs中treepanel例子一、TreePan......
  • 研究gRPC所给的helloworld例子
    这里我以编写一个远程过程调用，客户端传过来请求，远程过程调用就可以返回当前时间。（daytime服务器熟知端口是13，这里并不是搭建daytime，只是为了测试远程过程调用是否成功）CMakeLists.txt文件的编写cmake_minimum_required(VERSION3.8)project(HelloWorldCCXX)include(../cmake/......
  • PixiJS源码分析系列： 第一章 从最简单的例子入手
    从最简单的例子入手分析PixiJS源码我一般是以使用角度作为切入点查看分析源码，例子中用到什么类，什么方法，再入源码。高屋建瓴的角度咱也做不到啊，毕竟水平有限pixijs的源码之前折腾了半天都运行不起来，文档也没有明确说明如何调式我在github上看到过也有歪果仁在问如何本地......
  • 前端面试题28（Vue3的Teleport功能在什么场景下特别有用？能给个例子吗？）
    Vue3的Teleport功能在需要将组件的渲染结果放置在DOM树中与当前组件位置无关的任意位置时特别有用。这通常涉及到需要将某些UI元素（如模态框、弹出菜单、通知、工具提示等）从其逻辑上的父级组件中“提取”出来，放置到页面的更高层级或完全不同的位置，以避免样式冲突或层......
  • 目标检测小例子
    YOLO（YouOnlyLookOnce）是一种流行的目标检测算法，它以其快速和高效而闻名。YOLOv5是YOLO系列的第五个版本，它在性能和速度上都有所改进。以下是使用YOLOv5进行目标检测的一个基本示例代码，假设你已经安装了Python和必要的库，比如PyTorch和OpenCV。首先，你需要安装YOLOv5的库。......
  • 强化学习与控制模型结合例子
    强化学习与模型控制结合强化学习（ReinforcementLearning,RL）与控制模型结合，可以通过整合传统控制理论和现代RL算法，利用控制模型提供的动态信息和稳定性保障，同时利用RL的学习能力优化控制策略。这种结合的方式被称为模型辅助强化学习（Model-AssistedReinforcementLearning）......

赞助商

阅读排行

网站主页关于我们联系我们网站地图

本网站内容转载自其他媒体，侵权联系[admin##ips99.com]。

Copyright © 2020-2023 IPS99 版权所有 IPS99