基于gtsam进行曲线拟合

步骤：

使用opencv生成一组带有噪声的(x,y)点集
定义一个曲线模型 $y=e^{ax^2+bx+c}$
因子图的变量为系数[a b c]，一元因子为基于观测模型的残差 $f(x) = e^{ax^2+bx+c} - y$
使用LM优化模型，迭代1000次

代码如下：

#include <gtsam/base/Vector.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/Values.h>
#include <opencv2/core/core.hpp>
#include <random>
#include <cmath>

using namespace std;
using namespace gtsam;
using gtsam::symbol_shorthand::X;

/********************
 * 问题描述：定义曲线模型y = exp(a x^2 + b x + c)，已知一组带有噪声的点集(x,y)，求解函数的系数abc，使得拟合残差最小
 * 因子图变量：系数[a b c]
 * 一元因子：观测值(x,y)，存在观测函数
 * *****************************/

// 定义曲线模型, 用于计算残差
double funct(const gtsam::Vector3 &p, const double x){
    
    
    // y = exp(a x^2 + b x + c)
    return exp(p(0) * x * x + p(1) * x + p(2));
}

// 自定义类名 : 继承于一元因子类NoiseModelFactor1，因为abc有3维，所以数据类型为Vector3
class curvfitFactor : public gtsam::NoiseModelFactor1<gtsam::Vector3>{
    
    
    // 观测值，对应数据中的x和y值
    double mx, my;
public:
    // key表示该因子连接到的变量id，noise表示噪声，xy表示观测值
    curvfitFactor(gtsam::Key key, const gtsam::SharedNoiseModel &noise, double x, double y)
        : gtsam::NoiseModelFactor1<gtsam::Vector3>(noise, key), mx(x), my(y)
    {
    
    }
    virtual ~curvfitFactor(){
    
    }

    // 自定义因子一定要重写evaluateError函数(优化变量, 雅可比矩阵)
    gtsam::Vector evaluateError(const gtsam::Vector3 &p, boost::optional<gtsam::Matrix &> H = boost::none) const
    {
    
    
        auto val = funct(p, mx);
        if (H) // 残差为1维，优化变量为3维，雅可比矩阵为1*3(残差对abc三个变量求导)
        {
    
    
            // f(x) = exp(a x^2 + b x + c) - y
            // f(x)对[a b c]T求导，[x^2 * exp(a x^2 + b x + c) x * exp(a x^2 + b x + c) exp(a x^2 + b x + c)]T
            gtsam::Matrix Jac = gtsam::Matrix::Zero(1, 3);
            Jac << mx * mx * val, mx * val, val;
            (*H) = Jac;
        }
        return gtsam::Vector1(val - my); // 返回值为残差，残差为1维
    }
};

int main()
{
    
    
    // 制造一批带有噪声的数据
    double w_sigma = 1.0;
    double inv_sigma = 1.0;
    cv::RNG rng; // OpenCV随机数产生器
    vector<double> x_data, y_data;
    const gtsam::Vector3 para(1.0, 2.0, 1.0); // a,b,c的真实值
    for (int i = 0; i < 1000; ++i)
    {
    
    
        double mx = i / 1000.0;
        auto my = funct(para, mx) + rng.gaussian(w_sigma * w_sigma); // 加入了噪声数据
        x_data.push_back(mx);
        y_data.push_back(my);
    }

    // 构造因子图
    // Step1: 创建一个非线性优化因子图
    gtsam::NonlinearFactorGraph graph;
    // Step2: 添加一元因子到因子图
    for (int i = 0; i < 1000; i++)
    {
    
    
        // 定义噪声模型
        auto noiseM = gtsam::noiseModel::Isotropic::Sigma(1, w_sigma); // 噪声的维度需要与观测值维度保持一致
        // 添加因子,因为只有一个因子，所以所有的因子都添加到X(0)
        graph.add(boost::make_shared<curvfitFactor>(X(0), noiseM, x_data[i], y_data[i]));
    }
    // Step3: 给变量设置初始值
    gtsam::Values intial;
    intial.insert<gtsam::Vector3>(X(0), gtsam::Vector3(2.0, -1.0, 5.0));

    // Step4: 使用不同的优化器进行优化
    // gtsam::DoglegOptimizer opt(graph, intial); // 使用Dogleg优化
    // gtsam::GaussNewtonOptimizer opt(graph, intial); // 使用高斯牛顿优化
    gtsam::LevenbergMarquardtOptimizer opt(graph, intial); // 使用LM优化

    // Step5: 打印结果
    std::cout << "initial error=" << graph.error(intial) << std::endl;
    auto res = opt.optimize();
    res.print("final res:");
    std::cout << "final error=" << graph.error(res) << std::endl;
    gtsam::Vector3 matX0 = res.at<gtsam::Vector3>(X(0));
    std::cout << "a b c: " << matX0 << "\n";

    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.10)
project(gtsam_test)

set(GTSAM_USE_SYSTEM_EIGEN ON)

find_package(OpenCV REQUIRED)
find_package(GTSAM REQUIRED)

include_directories(
${OpenCV_INCLUDE_DIRS}
${GTSAM_INCLUDE_DIR}
)

add_executable(gtsam_curve_fitting gtsam_curve_fitting.cpp)
target_link_libraries(gtsam_curve_fitting gtsam ${OpenCV_LIBS})

基于gtsam进行曲线拟合

猜你喜欢