86 lines
3.8 KiB
C++
86 lines
3.8 KiB
C++
#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_;
|
||
}
|
||
|
||
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
|