[Optimization] Gauss–Newton for Nonlinear Least Squares

Many problems ultimately reduce to least-squares problems, including bundle adjustment and pose-graph optimization in SLAM. Gauss–Newton is one method for solving them.

Derivation

For a nonlinear least-squares problem,

$$x=\operatorname*{arg\,min}_x\frac{1}{2}\lVert f(x)\rVert^2. \tag{1}$$

Gauss–Newton approximates $f(x)$ with the first-order term of its Taylor expansion:

$$f(x+\Delta x)=f(x)+f'(x)\Delta x=f(x)+J(x)\Delta x. \tag{2}$$

Substituting (2) into (1) gives

$$\frac{1}{2}\lVert f(x+\Delta x)\rVert^2= \frac{1}{2}\left\{f(x)^\mathsf{T}f(x)+2f(x)^\mathsf{T}J(x)\Delta x+ \Delta x^\mathsf{T}J(x)^\mathsf{T}J(x)\Delta x\right\}. \tag{3}$$

Differentiate this expression and set the derivative to zero:

$$J(x)^\mathsf{T}J(x)\Delta x=-J(x)^\mathsf{T}f(x). \tag{4}$$

Let $H=J^\mathsf{T}J$ and $B=-J^\mathsf{T}f$. Equation (4) becomes

$$H\Delta x=B. \tag{5}$$

Solving (5) gives the increment $\Delta x$. This requires $H$ to be invertible (positive definite), which is not always true; the method can consequently diverge. An overly large step $\Delta x$ can also cause divergence.

The Gauss–Newton procedure is therefore:

1. Choose an initial value $x_0$.
2. At iteration $k$, calculate $J(x_k)$, form $H=J^\mathsf{T}J$ and $B=-J^\mathsf{T}f$, then solve $H\Delta x=B$.
3. Stop when $\lVert\Delta x\rVert$ is sufficiently small; otherwise set $x_{k+1}=x_k+\Delta x$.
4. Repeat steps 2–3 until the iteration limit or a stopping condition is reached.

---

Implementation

Problem. For the nonlinear equation $y=\exp(ax^2+bx+c)$, given $N$ observations $\{x,y\}$, estimate $X=[a,b,c]^\mathsf{T}$.

Analysis. Let $f(X)=y-\exp(ax^2+bx+c)$. The $N$ observations form one nonlinear system:

$$F(X)= \begin{bmatrix} y_1-\exp(ax_1^2+bx_1+c)\\ \vdots\\ y_N-\exp(ax_N^2+bx_N+c) \end{bmatrix}.$$

We can construct the least-squares objective

$$x=\operatorname*{arg\,min}_x\frac{1}{2}\lVert F(X)\rVert^2.$$

Following the derivation, we need its Jacobian:

$$J(X)= \begin{bmatrix} -x_1^2\exp(ax_1^2+bx_1+c) & -x_1\exp(ax_1^2+bx_1+c) & -\exp(ax_1^2+bx_1+c)\\ \vdots & \vdots & \vdots\\ -x_N^2\exp(ax_N^2+bx_N+c) & -x_N\exp(ax_N^2+bx_N+c) & -\exp(ax_N^2+bx_N+c) \end{bmatrix}.$$

The preceding steps can then solve the problem. Implementation:

ydsf16/Gauss_Newton_solver

/**
 * This file is part of Gauss-Newton Solver.
 *
 * Copyright (C) 2018-2020 Dongsheng Yang <ydsf16@buaa.edu.cn> (Beihang University)
 * For more information see <https://github.com/ydsf16/Gauss_Newton_solver>
 *
 * Gauss_Newton_solver is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Gauss_Newton_solver is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Gauss_Newton_solver. If not, see <http://www.gnu.org/licenses/>.
 */

#include <iostream>
#include <eigen3/Eigen/Core>
#include <vector>
#include <opencv2/opencv.hpp>
#include <eigen3/Eigen/Cholesky>
#include <eigen3/Eigen/QR>
#include <eigen3/Eigen/SVD>
#include <chrono>

/* Timer */
class Runtimer{
public:
    inline void start() { t_s_ = std::chrono::steady_clock::now(); }
    inline void stop() { t_e_ = std::chrono::steady_clock::now(); }
    inline double duration() {
        return std::chrono::duration_cast<std::chrono::duration<double>>(t_e_ - t_s_).count() * 1000.0;
    }
private:
    std::chrono::steady_clock::time_point t_s_; // start time point
    std::chrono::steady_clock::time_point t_e_; // end time point
};

/* Optimization equation */
class CostFunction{
public:
    CostFunction(double* a, double* b, double* c, int max_iter, double min_step, bool is_out):
        a_(a), b_(b), c_(c), max_iter_(max_iter), min_step_(min_step), is_out_(is_out) {}

    void addObservation(double x, double y) { obs_.push_back({x, y}); }

    void calcJ_fx() {
        J_.resize(obs_.size(), 3);
        fx_.resize(obs_.size(), 1);
        for (size_t i = 0; i < obs_.size(); i++) {
            std::vector<double>& ob = obs_.at(i);
            double& x = ob.at(0);
            double& y = ob.at(1);
            double j1 = -x*x*exp(*a_ * x*x + *b_*x + *c_);
            double j2 = -x*exp(*a_ * x*x + *b_*x + *c_);
            double j3 = -exp(*a_ * x*x + *b_*x + *c_);
            J_(i, 0) = j1; J_(i, 1) = j2; J_(i, 2) = j3;
            fx_(i, 0) = y - exp(*a_ *x*x + *b_*x +*c_);
        }
    }
    void calcH_b() { H_ = J_.transpose() * J_; B_ = -J_.transpose() * fx_; }
    void calcDeltax() { deltax_ = H_.ldlt().solve(B_); }
    void updateX() { *a_ += deltax_(0); *b_ += deltax_(1); *c_ += deltax_(2); }
    double getCost() { Eigen::MatrixXd cost = fx_.transpose() * fx_; return cost(0, 0); }

    void solveByGaussNewton() {
        double sumt = 0;
        bool is_conv = false;
        for (size_t i = 0; i < max_iter_; i++) {
            Runtimer t; t.start();
            calcJ_fx(); calcH_b(); calcDeltax();
            double delta = deltax_.transpose() * deltax_;
            t.stop();
            if (is_out_) {
                std::cout << "Iter: " << std::left << std::setw(3) << i << " Result: "
                          << std::left << std::setw(10) << *a_ << " " << std::left << std::setw(10) << *b_ << " "
                          << std::left << std::setw(10) << *c_ << " step: " << std::left << std::setw(14) << delta
                          << " cost: " << std::left << std::setw(14) << getCost() << " time: " << std::left
                          << std::setw(14) << t.duration() << " total_time: " << std::left << std::setw(14)
                          << (sumt += t.duration()) << std::endl;
            }
            if (delta < min_step_) { is_conv = true; break; }
            updateX();
        }
        std::cout << (is_conv ? "\nConverged\n" : "\nDiverged\n\n");
    }

    Eigen::MatrixXd fx_;
    Eigen::MatrixXd J_; // Jacobian matrix
    Eigen::Matrix3d H_; // H matrix
    Eigen::Vector3d B_, deltax_;
    std::vector<std::vector<double>> obs_; // observations
    double* a_; double* b_; double* c_;
    int max_iter_; double min_step_; bool is_out_;
};

int main(int argc, char **argv) {
    const double aa = 0.1, bb = 0.5, cc = 2; // parameters of the ground-truth equation
    double a = 0.0, b = 0.0, c = 0.0; // initial value
    CostFunction cost_func(&a, &b, &c, 50, 1e-10, true); // construct the problem
    const size_t N = 100; // number of observations
    cv::RNG rng(cv::getTickCount());
    for (size_t i = 0; i < N; i++) {
        double x = rng.uniform(0.0, 1.0);
        double y = exp(aa*x*x + bb*x + cc) + rng.gaussian(0.05); // generate data with Gaussian noise
        cost_func.addObservation(x, y);
    }
    cost_func.solveByGaussNewton(); // solve with Gauss–Newton
    return 0;
}