g2o曲线拟合参数y = a sin( b * t + c) + d

最新推荐文章于 2024-06-12 10:00:23 发布

揣在兜里的黑桃k

最新推荐文章于 2024-06-12 10:00:23 发布

阅读量95

点赞数

文章标签： c语言算法开发语言

本文链接：https://blog.csdn.net/weixin_52867897/article/details/134476858

版权

g2o_fit.cpp

#include <iostream>
#include <fstream>
#include <Eigen/Core>
#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/solvers/dense/linear_solver_dense.h>
#include <g2o/types/slam3d/types_slam3d.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_factory.h>

// 自定义顶点
class VertexParameters : public g2o::BaseVertex<4, Eigen::Vector4d> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    void setToOriginImpl() override {
        _estimate << 1.0, 1.0, 0.0, 0.0;
    }

    void oplusImpl(const double* update) override {
        _estimate += Eigen::Map<const Eigen::Vector4d>(update);
    }

    bool read(std::istream& is) override { return false; }

    bool write(std::ostream& os) const override { return false; }
};

// 自定义边
class EdgeResidual : public g2o::BaseUnaryEdge<1, double, VertexParameters> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EdgeResidual(double t, double y)
        : t_(t), y_(y) {}

    void computeError() override {
        // TODO: 完成这个函数（提示：需要对 _error[0] 进行赋值）
        const VertexParameters* v =static_cast<const VertexParameters*>(_vertices[0]);
        const Eigen::Vector4d abcd =  v->estimate();
        double a = abcd[0], b = abcd[1], c = abcd[2], d = abcd[3];
        _error(0,0) = y_ - (a * sin( b * t_ + c ) + d) ;
    }

    void linearizeOplus() override {
        // TODO: 完成这个函数（提示：需要对 _jacobianOplusXi 进行赋值）=
        VertexParameters* vi = static_cast<VertexParameters*>(_vertices[0]);
        Eigen::Vector4d abcd = vi -> estimate();
        double a = abcd[0], b = abcd[1], c = abcd[2], d = abcd[3];
        _jacobianOplusXi(0,0) = -sin( b * t_ + c );
        _jacobianOplusXi(0,1) = -a*t_*cos( b * t_ + c );
        _jacobianOplusXi(0,2) = -a*cos( b * t_ + c );
        _jacobianOplusXi(0,3) = -1;
    }

    bool read(std::istream& is) override { return false; }

    bool write(std::ostream& os) const override { return false; }

    double t_;
    double y_;
};

int main() {
    double w_sigma = 2.0;
    // 从文件中读取数据
    std::vector<double> t_values;
    std::vector<double> y_values;

    std::ifstream file("../data.txt");  // 注意修改为自己的文件路径
    if (!file) {
        std::cerr << "Error opening file." << std::endl;
        return 1;
    }

    // 读取数据到程序中
    double t, y;
    while (file >> t >> y) {
        t_values.push_back(t);
        y_values.push_back(y);
    }

    // 构建g2o优化器
    g2o::SparseOptimizer optimizer;
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<4, 1>>  Block;
    std::unique_ptr<Block::LinearSolverType> linearSolver (new g2o::LinearSolverDense<Block::PoseMatrixType> ());
    std::unique_ptr<Block > blockSolver (new Block (std::move(linearSolver)));
    g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg(std::move(blockSolver));

    optimizer.setAlgorithm(algorithm);

    // 添加顶点
    VertexParameters* parameters = new VertexParameters();
    /***    开始（添加顶点）    ***/ 
    parameters -> setEstimate ( Eigen::Vector4d (1,1,0,0));
    parameters -> setId(0);
    optimizer.addVertex (parameters);
    /***    结束（添加顶点）    ***/ 


    // 添加边
    for (size_t i = 0; i < t_values.size(); ++i) {
        /***    开始（添加边）    ***/ 
        EdgeResidual* edge = new EdgeResidual(t_values[i],y_values[i]);
        edge -> setId(i);
        edge -> setVertex( 0 , parameters );
        edge -> setMeasurement ( y_values[i] );
        edge -> setInformation (Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma));
        edge -> t_ = t_values[i]; 
        optimizer.addEdge(edge);
        /***    结束（添加边）    ***/ 
    }

    // 开始优化
    optimizer.initializeOptimization();
    optimizer.optimize(10);

    // 打印优化结果
    std::cout << "Optimization results:" << std::endl;
    std::cout << "Estimated parameters: " << parameters->estimate().transpose() << std::endl;

    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.10)
project(g2oFit)

set(CMAKE_CXX_STANDARD 17)

find_package(Eigen3 REQUIRED)
find_package(g2o REQUIRED)

include_directories(${EIGEN3_INCLUDE_DIRS})
include_directories(${G2O_INCLUDE_DIRS})

add_executable(g2o_fit g2o_fit.cpp)
target_link_libraries(g2o_fit g2o_core g2o_stuff)

data.txt 数据

0	0.76757
0.10101	0.93524
0.20202	1.1466
0.30303	1.1455
0.40404	1.154
0.50505	1.3246
0.60606	1.5646
0.70707	1.4691
0.80808	1.7375
0.90909	1.6842
1.0101	1.6719
1.1111	1.7399
1.2121	1.9719
1.3131	1.9569
1.4141	1.9843
1.5152	1.8329
1.6162	2.1149
1.7172	1.9869
1.8182	2.0363
1.9192	2.114
2.0202	1.8387
2.1212	1.9638
2.2222	1.7445
2.3232	1.6328
2.4242	1.893
2.5253	1.7023
2.6263	1.6607
2.7273	1.4326
2.8283	1.4774
2.9293	1.2658
3.0303	1.2903
3.1313	1.0478
3.2323	1.0599
3.3333	1.1794
3.4343	0.87257
3.5354	0.6227
3.6364	0.52524
3.7374	0.27585
3.8384	0.3178
3.9394	0.2932
4.0404	0.18571
4.1414	-0.10813
4.2424	-0.11886
4.3434	-0.23174
4.4444	-0.35856
4.5455	-0.53086
4.6465	-0.6226
4.7475	-0.63279
4.8485	-0.75072
4.9495	-0.81937
5.0505	-0.7828
5.1515	-0.81525
5.2525	-0.93389
5.3535	-0.92959
5.4545	-0.85862
5.5556	-1.055
5.6566	-0.96281
5.7576	-0.94334
5.8586	-1.0017
5.9596	-0.97491
6.0606	-0.91803
6.1616	-0.94708
6.2626	-0.67075
6.3636	-0.75539
6.4646	-0.76205
6.5657	-0.63985
6.6667	-0.37652
6.7677	-0.60884
6.8687	-0.28017
6.9697	-0.14374
7.0707	-0.046163
7.1717	-0.14572
7.2727	0.28633
7.3737	0.19477
7.4747	0.37418
7.5758	0.40421
7.6768	0.58243
7.7778	0.57071
7.8788	0.79059
7.9798	0.86386
8.0808	1.0311
8.1818	1.203
8.2828	1.2812
8.3838	1.4714
8.4848	1.4824
8.5859	1.2881
8.6869	1.5937
8.7879	1.8294
8.8889	1.8893
8.9899	2.0411
9.0909	1.8181
9.1919	2.0668
9.2929	1.981
9.3939	2.0563
9.4949	2.0291
9.596	2.0428
9.697	2.0146
9.798	1.9306
9.899	1.7098
10	1.7872

结果