曲线方程为 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;
}