Q3D-Calibration/include/LevenbergMarquardt.h

#pragma once

#ifndef LMM_h
#define LMM_h

#include "utils.h"

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


class LevenbergMarquardt {
public:
    LevenbergMarquardt(int L_, double* parma_, double (*Func_)(double, double*),
                       double* (*Gunc_)(double, double*), int type_ = 0);
    ~LevenbergMarquardt();

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

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

private:
    void calmJ_vF();
    void calmH_vG();
    double calMse(double* parma_);

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

LevenbergMarquardt::LevenbergMarquardt(int L_, double* parma_, double (*Func_)(double, double*),
                                       double* (*Gunc_)(double, double*), int type_) {
    L = L_;
    parma = parma_;
    Func = Func_;
    Gunc = Gunc_;
    this->type_ = type_;
}

LevenbergMarquardt::~LevenbergMarquardt() {}

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

void LevenbergMarquardt::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 LevenbergMarquardt::calmH_vG() {
    mH = mJ.transpose() * mJ;
    vG = -mJ.transpose() * vF;
}

double LevenbergMarquardt::calMse(double* parma_) {
    int n = data.size();
    double x, y, mse = 0;

    for (int i = 0; i < n; i++) {
        Eigen::Vector2d& point = data.at(i);
        x = point(0), y = point(1);
        mse += pow(y - (*Func)(x, parma_), 2);
    }

    return mse / n;
}

double* LevenbergMarquardt::solve() {
    double v = 2, cost;
    double* parmaNew;

    Eigen::VectorXd vX;
    Eigen::MatrixXd mT, mD = Eigen::MatrixXd::Zero();
    vX.resize(L);
    mD.resize(L, L);
    for (int i = 0; i < L; i++) mD(i, i) = 1;

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

        if (type_ == 1) {
            mT = mJ * mJ.transpose();
            for (int i = 0; i < L; i++) mD(i, i) = sqrt(mT(i, i));
        }

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

        for (int i = 0; i < L; i++) parmaNew[i] = parma[i] + vX[i];
        cost = calMse(parma) - calMse(parmaNew);
        cost /= vX.transpose() * (mu * vX + vG);
    }

    return parma;
}

#endif