Function levenberg_marquardt::optimize

pub fn optimize<N, P, S, J, PS, RS, JS, IJ>(
    config: Config<N>, 
    init: Vector<N, P, PS>, 
    normalize: impl Fn(Vector<N, P, PS>) -> Vector<N, P, PS>, 
    residuals: impl Fn(&Vector<N, P, PS>) -> Matrix<N, J, S, RS>, 
    jacobians: impl Fn(&Vector<N, P, PS>) -> IJ
) -> Vector<N, P, PS> where
    N: RealField + FromPrimitive,
    P: DimMin<P> + DimName,
    S: Dim,
    J: DimName,
    PS: ContiguousStorageMut<N, P> + Clone,
    RS: Storage<N, J, S>,
    JS: Storage<N, P, J>,
    IJ: Iterator<Item = Matrix<N, P, J, JS>>,
    DefaultAllocator: Allocator<N, J, P>,
    DefaultAllocator: Allocator<N, P, P>,
    DefaultAllocator: Allocator<N, P, Buffer = PS>,
    ShapeConstraint: DimEq<DimMinimum<P, P>, P>, 

Note that the differentials and state vector are represented with column vectors. This is atypical from the normal way it is done in mathematics. This is done because nalgebra is column-major. A nalgebra Vector is a column vector.

Make sure that you create your Jacobian such that it is several fixed length column vectors rather than several row vectors as per normal. If you have already computed it with row vectors, then you can take the transpose.

It is recommended to make the number of columns dynamic unless you have a small fixed number of data-points.

max_iterations limits the number of times the initial guess will be updated.

consecutive_divergence_limit limits the number of times that lambda can diverge consecutively from Gauss-Newton due to a failed improvement. Once the solution is as good as possible, it will begin regressing to gradient descent. This limit prevents it from wasting the remaining cycles of the algorithm.

initial_lambda defines the initial lambda value. As lambda grows higher, Levenberg-Marquardt approaches gradient descent, which is better at converging to a distant minima. As lambda grows lower, Levenberg-Marquardt approaches Gauss-Newton, which allows faster convergence closer to the minima. A lambda of 0.0 would imply that it is purely based on Gauss-Newton approximation. Please do not set lambda to exactly 0.0 or the lambda_scale will be unable to increase lambda since it does so through multiplication.

lambda_converge must be set to a value below 1.0. On each iteration of Levenberg-Marquardt, the lambda is used as-is and multiplied by lambda_converge. If the original lambda or the new lambda is better, that lambda becomes the new lambda. If neither are better than the previous sum-of-squares, then lambda is multiplied by lambda_diverge.

lambda_diverge must be set to a value above 1.0 and highly recommended to set it above lambda_converge^-1 (it will re-test an already-used lambda otherwise). On each iteration, if the sum-of-squares regresses, then lambda is multiplied by lambda_diverge to move closer to gradient descent in hopes that it will cause it to converge.

threshold is the point at which the average-of-squares is low enough that the algorithm can terminate. This exists so that the algorithm can short-circuit and exit early if the solution was easy to find. Set this to 0.0 if you want it to continue for all max_iterations. You might do that if you always have a fixed amount of time per optimization, such as when processing live video frames.

init is the initial parameter guess. Make sure to set init close to the actual solution. It is recommended to use a sample consensus algorithm to get a close initial approximation.

normalize allows the parameter guess to be normalized on each iteration. It can be pushed into a slightly incorrect state on each iteration and this can be used to correct it. This might be something like an angle which exceeds 2 * pi. It might technically be correct, but you want to wrap it back around. This is also useful when a normal vector or unit quaternion is involved since those need to be kept normalized throughout the optimization procedure.

residuals must return the difference between the expected value and the output of the function being optimized. This is returned as a matrix where the number of residuals (rows) that are present in each column must correspond to the number of columns in each Jacobian returned by jacobians.

jacobians is a function that takes in the current guess and produces all the Jacobian matrices of the negative residuals in respect to the parameter. Each row should correspond to a dimension of the parameter vector and each column should correspond to a row in the residual matrix. You can pass in the Jacobian of as many residuals as you would like on each iteration, so long as the residuals returned by residuals has the same number of residuals per column. Only the Jacobian and the residuals are required to perform Levenberg-Marquardt optimization. You may need to caputure your observances in the closure to compute the Jacobian, but they are not arguments since they are constants to Levenberg-Marquardt.

N is the type parameter of the data type that is stored in the matrix (f32).

P is the number of parameter variables being optimized.

S is the number of samples used in optimization.

J is the number of rows per sample returned and the number of columns in the Jacobian.

PS is the nalgebra storage used for the parameter vector.

RS is the nalgebra storage used for the residual matrix.

JS is the nalgebra storage used for the Jacobian matrix.

IJ is the iterator over the Jacobian matrices of each sample.