121 lines
2.8 KiB
C++
121 lines
2.8 KiB
C++
#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 |