上手 GTSAM 4.3.0 求解非线性曲线拟合问题

GTSAM 4.3.0 非线性曲线拟合

---

参考链接:
链接1: 例子由来
链接2:实验室大佬的博客

---

遵循视觉SLAM十四讲的思路, 上手各类求解器都可以用非线性曲线拟合这个例子来做. 假设问题如下:
$$y=a{x}^2+bx$$
待求解变量为 $a$ 和 $b$
它的Jacobian矩阵为:
$$J=\left[\begin{array}{cc}{x}^2& x\end{array}\right]$$
局部线性化的误差方程为:
$$error=\left[\begin{array}{cc}{x}^2& x\end{array}\right]\left[\begin{array}{c}a\\ b\end{array}\right]-y$$

---

C++代码如下, GTSAM版本为4.3.0:

#include 
#include 
#include 
#include 
#include 
#include 
 
#include 
#include 
#include 
 
#include 
 
//  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:
    using NoiseModelFactor1<gtsam::Vector2>::evaluateError;
    typedef std::shared_ptr<curvfitFactor> shared_ptr;
    curvfitFactor(gtsam::Key key, const gtsam::SharedNoiseModel& noise, double x, double y)
        : gtsam::NoiseModelFactor1<gtsam::Vector2>(noise, key), mx(x), my(y){
    }
    //virtual ~curvfitFactor() {
    ~curvfitFactor() override {
    }
    gtsam::Vector evaluateError(const gtsam::Vector2& x, gtsam::OptionalMatrixType H ) 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);
    }
    gtsam::NonlinearFactor::shared_ptr clone() const override{
        return std::static_pointer_cast<gtsam::NonlinearFactor>(
            gtsam::NonlinearFactor::shared_ptr(new curvfitFactor(*this))); }
};
 
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.1);
 
    for (int i = 0; i < 500; ++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.5,0.2));
 
    //gtsam::DoglegOptimizer opt(graph, intial);
    gtsam::LevenbergMarquardtOptimizer optimizer(graph,intial);
    gtsam::Values result = optimizer.optimize();
    std::cout << "initial error=" << graph.error(intial) << std::endl;
    auto res = optimizer.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";
    return 0;
}

CMakeLists.txt如下

cmake_minimum_required(VERSION 3.0.2)
project(my_project)

add_compile_options(-std=c++17)
find_package(Boost REQUIRED COMPONENTS system)
find_package(Eigen3 REQUIRED)
find_package(GTSAM REQUIRED)
include_directories(${GTSAM_INCLUDE_DIR})
include_directories(${EIGEN3_INCLUDE_DIRS})
set(GTSAM_LIBRARIES gtsam)

include_directories(
include
${EIGEN3_INCLUDE_DIRS}
)
link_directories(/usr/local/lib)
add_executable(gtsam_fitting src/gtsam_fitting.cpp)
target_link_libraries(gtsam_fitting gtsam)

---

与链接1,2中最大的不同在于GTSAM版本为4.3.0, 在自定义因子curefitFactor中覆写的evaluateError的Hessian矩阵参数类型不再是boost::optional, 而是gtsam::OptionalMatrixType.