手写高斯牛顿求解非线性最小二乘问题

容易写错的地方：

• 注意你的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 x
      Eigen::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();
};