(一）使用gtsam做曲线拟合

曲线方程为 y = ax^2 + bx ;

生成样本，利用gtsam拟合曲线的参数 a,b

#include <gtsam/base/Vector.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/DoglegOptimizer.h>

#include <gtsam/inference/Key.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/Values.h>

#include <random>

//  y = a x^2 + b x  
double funct(const gtsam::Vector2& p, const double x) {
    return (p(0)*x*x + p(1)*x);
}

class curvfitFactor :public  gtsam::NoiseModelFactor1<gtsam::Vector2> {
    double mx, my;
public:
    curvfitFactor(gtsam::Key key, const gtsam::SharedNoiseModel& noise, double x, double y)
        : gtsam::NoiseModelFactor1<gtsam::Vector2>(noise, key), mx(x), my(y){
    }
    virtual ~curvfitFactor() {
    }
    gtsam::Vector evaluateError(const gtsam::Vector2& x, boost::optional<gtsam::Matrix&> H = boost::none) const {
        auto val = funct(x, mx);
        if (H) {
            gtsam::Matrix Jac = gtsam::Matrix::Zero(1, 2);
            Jac << mx * mx, mx;
            (*H) = Jac;
        }
        return gtsam::Vector1(val - my);
    }
};

int main() {
    using  gtsam::symbol_shorthand::X;
    const double sig = 0.05;
    std::random_device rd;
    std::default_random_engine generator_(rd());
    std::normal_distribution<double> noise(0, sig);
    gtsam::NonlinearFactorGraph graph;
    const gtsam::Vector2 para(0.2, 0.3);

    for (int i = 0; i < 100; ++i) {
        double mx = i;
        auto my = funct(para, mx) + noise(generator_);
        auto noiseM = gtsam::noiseModel::Isotropic::Sigma(1, sig);
        graph.emplace_shared<curvfitFactor>(X(0), noiseM, mx, my);
    }

    gtsam::Values intial;
    intial.insert<gtsam::Vector2>(X(0), gtsam::Vector2(0.1,0.1));

    gtsam::DoglegOptimizer opt(graph, intial);
    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::Vector2 matX0 = res.at<gtsam::Vector2>(X(0));
    std::cout << matX0 << "\n";
    system("pause");
    return 0;
}