Q3D-Calibration/include/GaussNewton.h

#pragma once

#ifndef gauss_newton_h
#define gauss_newton_h

#include <Eigen/Dense>
#include <iostream>

class GaussNewton {
public:
    GaussNewton(int L_, double* parma_, double (*Func_)(double, double*),
                double* (*Gunc_)(double, double*));
    ~GaussNewton();

public:
    void addData(double x, double y);
    double* solve();

public:
    double eps = 1e-5;
    double* parma;
    double (*Func)(double, double*);
    double* (*Gunc)(double, double*);
    int L, maxIter = 10;
    std::vector<Eigen::Vector2d> data;

private:
    void calmJ_vF();
    void calmH_vG();

private:
    Eigen::MatrixXd mJ;  // 雅克比矩阵
    Eigen::MatrixXd mH;  // H矩阵
    Eigen::VectorXd vF;  // 误差向量
    Eigen::Vector3d vG;  // 反馈向量
};

GaussNewton::GaussNewton(int L_, double* parma_, double (*Func_)(double, double*),
                         double* (*Gunc_)(double, double*)) {
    L = L_;
    parma = parma_;
    Func = Func_;
    Gunc = Gunc_;
}

GaussNewton::~GaussNewton() {}

void GaussNewton::addData(double x, double y) { data.push_back(Eigen::Vector2d(x, y)); }

void GaussNewton::calmJ_vF() {
    double x, y;
    double* resJ;

    mJ.resize(data.size(), L);
    vF.resize(data.size());

    for (int i = 0; i < data.size(); i++) {
        Eigen::Vector2d& point = data.at(i);
        x = point(0), y = point(1);
        resJ = (*Gunc)(x, parma);
        for (int j = 0; j < L; j++) mJ(i, j) = resJ[j];
        vF(i) = y - (*Func)(x, parma);
    }
}

void GaussNewton::calmH_vG() {
    mH = mJ.transpose() * mJ;
    vG = -mJ.transpose() * vF;
}

double* GaussNewton::solve() {
    Eigen::VectorXd vX(L);

    for (int k = 0; k < maxIter; k++) {
        calmJ_vF();
        calmH_vG();

        vX = mH.ldlt().solve(vG);
        if (vX.norm() <= eps) return parma;

        for (int i = 0; i < L; i++) parma[i] += vX[i];
    }

    return parma;
}

#endif