容易写错的地方：
- 注意你的residual定义是 z-h(x)， 还是 h(x) - z，这会影响到Hx=b的符号。（自己推一遍就知道了），我的习惯性定义是z-h(x)
class GaussianNewtonOptimizer {// Observation: [x, y]// y = std::exp(a*x^2 + b*x + c)// state: [a, b, c]// J = std::exp(a*x^2 + b*x + c) * [x^2, x, 1]// residual = y - f(x; a, b, c)public:void Initialize(const Eigen::Vector3d &state, const Eigen::Vector3d &state_gt,int max_iteration) {state_ = state;state_gt_ = state_gt;max_iteration_ = max_iteration;double MeasurementModelFunc(double observation) {return std::exp(state_[0] * observation * observation +state_[1] * observation + state_[2]);}Eigen::Matrix<double, 1, 3> JacobianFunc(double observation) {const double dzdf = MeasurementModelFunc(observation);Eigen::Matrix<double, 1, 3> dfdx;dfdx << observation * observation, observation, 1;return dzdf * dfdx;}void Solve(const std::vector<double> &x_data,const std::vector<double> &y_data) {const int measurement_dim = x_data.size();assert(measurement_dim == y_data.size());for (int iter = 0; iter < max_iteration_; ++iter) {// ------------------- 逐个观测构建H矩阵和b向量（比较常用） -------------------// 1. Compute jacobian and residual// Eigen::Matrix<double, 3, 3> H;// Eigen::Matrix<double, 3, 1> b;// H.setZero();// b.setZero();// double cost = 0.0;// for (int i = 0; i < measurement_dim; ++i) {// const Eigen::Matrix<double, 1, 3> jacobian = JacobianFunc(x_data[i]);// const double residual = y_data[i] - MeasurementModelFunc(x_data[i]);// H += jacobian.transpose() * jacobian;// b += jacobian.transpose() * residual;// cost += residual * residual;// }// 2. Compute delta x// Eigen::Vector3d delta_x = H.ldlt().solve(b);// ------------------- 所有观测一次性构建H矩阵和b向量 -------------------Eigen::MatrixXd jacobians = Eigen::MatrixXd::Zero(measurement_dim, 3);Eigen::MatrixXd residuals = Eigen::MatrixXd::Zero(measurement_dim, 1);double cost = 0.0;for (int i = 0; i < measurement_dim; ++i) {const Eigen::Matrix<double, 1, 3> jacobian = JacobianFunc(x_data[i]);const double residual = y_data[i] - MeasurementModelFunc(x_data[i]);jacobians.row(i) = jacobian;residuals(i, 0) = residual;cost += residual * residual;}// 2. Compute delta xEigen::MatrixXd H = jacobians.transpose() * jacobians;Eigen::MatrixXd b = jacobians.transpose() * residuals;Eigen::Vector3d delta_x = H.ldlt().solve(b);state_ += delta_x;std::cout << "iter: " << iter << ", cost: " << cost<< ", state : " << state_.transpose() << std::endl;}}private:int max_iteration_ = 0;Eigen::Vector3d state_ = Eigen::Vector3d::Zero();Eigen::Vector3d state_gt_ = Eigen::Vector3d::Zero();
};
