Nonlinear Optimization Least Squares

Non-Linear Least Squares

A non-linear version of the least squares problem, where the error term $e(x)$ is non-linear.

Options to solve:

• Ceres Solver http://ceres-solver.org/
• G2o https://github.com/RainerKuemmerle/g2o
• Eigen?
• Sophus
• CuSOLVER https://docs.nvidia.com/cuda/cusolver/index.html#using-the-cusolver-api
• Found from a quick google search https://github.com/Rookfighter/least-squares-cpp

Notes from Cyrill Stachniss course.

I find the notes from the SLAM Textbook to be really good.

The goal is to minimize the function ${\min }_{x}F(x)=\frac{1}{2}\Vert f(x){\Vert }_2^2$

We solve it iteratively in the following steps:

Abstract

1. Give an initial value $x_0$.
2. For $k$-th iteration, we find an incremental value of$\Delta x_k$, such that the object function $\Vert f(x_k+\Delta x_k){\Vert }_2^2$ reaches a smaller value.
3. If $\Delta x_k$ is small enough, stop the algorithm.
4. Otherwise, let $x_{k+1}=x_k+\Delta x_k$ and return to step 2.

• This feels very much like Gradient Descent. How do we find $\Delta x_k$? In gradient descent, it’s just some step size multiplied by the gradient of the function

Take the Taylor Polynomial expansion of $F$: $F(x_k+\Delta x_k)=F(x_k)+J(x_k{)}^T\Delta x_k+\frac{1}{2}\Delta x_k^{T}H(x_k)\Delta x_k$

• $J(x_k)$ is the first derivative of $F(x)$ (called the Jacobian Matrix)
• $H(x_k)$ is the second derivative of $F(x)$ (called the Hessian Matrix)

Note

Depending on whether you take $J$ or $H$, you end up with first-order method (Gradient Descent) or second-order method (Newton’s Method).

• Gradient descent is volatile due to having to determine the step size correctly, does not guarantee convergence
• Newton’s method is computationally expensive

The book introduces two quasi-Newton methods to avoid calculating $H$ directly:

• Gauss-Newton Method
• Levernberg-Marquatdt Method

Practical advice

We usually choose one of the Gauss-Newton or Levenberg-Marquardt methods as the gradient descent strategy in practical problems.

• When the problem is well-formed, Gauss-Newton is used
• Otherwise, in ill-formed problems, we use the Levenberg-Marquardt method

In SLAM Context

In the context of SLAM, we want to minimize the error between the current observation and the prediction.

So how is F(x) defined? $F(x)={\sum }_i{e}_i(x)$

where $e_i(x)=z_i-f_i(x)$

• so how are those values actually computed?

• $z_i$ is the 3d coordinate of the point point example

• $f_i(x)$ is the project of the 2d coordinate onto the 3d plane

What is this equation? $e_i(x)=e_i(x{)}^T{\text{ohm}}_i{e}_i(x)$

• Depends only on the state