| #include "common/math/levenberg_marquardt.h" |
| |
| #include <stdbool.h> |
| #include <stdio.h> |
| #include <string.h> |
| |
| #include "common/math/macros.h" |
| #include "common/math/mat.h" |
| #include "common/math/vec.h" |
| |
| // FORWARD DECLARATIONS |
| //////////////////////////////////////////////////////////////////////// |
| static bool checkRelativeStepSize(const float *step, const float *state, |
| size_t dim, float relative_error_threshold); |
| |
| static bool computeResidualAndGradients(ResidualAndJacobianFunction func, |
| const float *state, const void *f_data, |
| float *jacobian, |
| float gradient_threshold, |
| size_t state_dim, size_t meas_dim, |
| float *residual, float *gradient, |
| float *hessian); |
| |
| static bool computeStep(const float *gradient, float *hessian, float *L, |
| float damping_factor, size_t dim, float *step); |
| |
| const static float kEps = 1e-10f; |
| |
| // FUNCTION IMPLEMENTATIONS |
| //////////////////////////////////////////////////////////////////////// |
| void lmSolverInit(struct LmSolver *solver, const struct LmParams *params, |
| ResidualAndJacobianFunction func) { |
| ASSERT_NOT_NULL(solver); |
| ASSERT_NOT_NULL(params); |
| ASSERT_NOT_NULL(func); |
| memset(solver, 0, sizeof(struct LmSolver)); |
| memcpy(&solver->params, params, sizeof(struct LmParams)); |
| solver->func = func; |
| solver->num_iter = 0; |
| } |
| |
| void lmSolverDestroy(struct LmSolver *solver) { |
| (void)solver; |
| } |
| |
| void lmSolverSetData(struct LmSolver *solver, struct LmData *data) { |
| ASSERT_NOT_NULL(solver); |
| ASSERT_NOT_NULL(data); |
| solver->data = data; |
| } |
| |
| enum LmStatus lmSolverSolve(struct LmSolver *solver, const float *initial_state, |
| void *f_data, size_t state_dim, size_t meas_dim, |
| float *state) { |
| // Initialize parameters. |
| float damping_factor = 0.0f; |
| float v = 2.0f; |
| |
| // Check dimensions. |
| if (meas_dim > MAX_LM_MEAS_DIMENSION || state_dim > MAX_LM_STATE_DIMENSION) { |
| return INVALID_DATA_DIMENSIONS; |
| } |
| |
| // Check pointers (note that f_data can be null if no additional data is |
| // required by the error function). |
| ASSERT_NOT_NULL(solver); |
| ASSERT_NOT_NULL(initial_state); |
| ASSERT_NOT_NULL(state); |
| ASSERT_NOT_NULL(solver->data); |
| |
| // Allocate memory for intermediate variables. |
| float state_new[MAX_LM_STATE_DIMENSION]; |
| struct LmData *data = solver->data; |
| |
| // state = initial_state, num_iter = 0 |
| memcpy(state, initial_state, sizeof(float) * state_dim); |
| solver->num_iter = 0; |
| |
| // Compute initial cost function gradient and return if already sufficiently |
| // small to satisfy solution. |
| if (computeResidualAndGradients(solver->func, state, f_data, data->temp, |
| solver->params.gradient_threshold, state_dim, |
| meas_dim, data->residual, |
| data->gradient, |
| data->hessian)) { |
| return GRADIENT_SUFFICIENTLY_SMALL; |
| } |
| |
| // Initialize damping parameter. |
| damping_factor = solver->params.initial_u_scale * |
| matMaxDiagonalElement(data->hessian, state_dim); |
| |
| // Iterate solution. |
| for (solver->num_iter = 0; |
| solver->num_iter < solver->params.max_iterations; |
| ++solver->num_iter) { |
| |
| // Compute new solver step. |
| if (!computeStep(data->gradient, data->hessian, data->temp, damping_factor, |
| state_dim, data->step)) { |
| return CHOLESKY_FAIL; |
| } |
| |
| // If the new step is already sufficiently small, we have a solution. |
| if (checkRelativeStepSize(data->step, state, state_dim, |
| solver->params.relative_step_threshold)) { |
| return RELATIVE_STEP_SUFFICIENTLY_SMALL; |
| } |
| |
| // state_new = state + step. |
| vecAdd(state_new, state, data->step, state_dim); |
| |
| // Compute new cost function residual. |
| solver->func(state_new, f_data, data->residual_new, NULL); |
| |
| // Compute ratio of expected to actual cost function gain for this step. |
| const float gain_ratio = computeGainRatio(data->residual, |
| data->residual_new, |
| data->step, data->gradient, |
| damping_factor, state_dim, |
| meas_dim); |
| |
| // If gain ratio is positive, the step size is good, otherwise adjust |
| // damping factor and compute a new step. |
| if (gain_ratio > 0.0f) { |
| // Set state to new state vector: state = state_new. |
| memcpy(state, state_new, sizeof(float) * state_dim); |
| |
| // Check if cost function gradient is now sufficiently small, |
| // in which case we have a local solution. |
| if (computeResidualAndGradients(solver->func, state, f_data, data->temp, |
| solver->params.gradient_threshold, |
| state_dim, meas_dim, data->residual, |
| data->gradient, data->hessian)) { |
| return GRADIENT_SUFFICIENTLY_SMALL; |
| } |
| |
| // Update damping factor based on gain ratio. |
| // Note, this update logic comes from Equation 2.21 in the following: |
| // [Madsen, Kaj, Hans Bruun Nielsen, and Ole Tingleff. |
| // "Methods for non-linear least squares problems." (2004)]. |
| const float tmp = 2.f * gain_ratio - 1.f; |
| damping_factor *= NANO_MAX(0.33333f, 1.f - tmp * tmp * tmp); |
| v = 2.f; |
| } else { |
| // Update damping factor and try again. |
| damping_factor *= v; |
| v *= 2.f; |
| } |
| } |
| |
| return HIT_MAX_ITERATIONS; |
| } |
| |
| float computeGainRatio(const float *residual, const float *residual_new, |
| const float *step, const float *gradient, |
| float damping_factor, size_t state_dim, |
| size_t meas_dim) { |
| // Compute true_gain = residual' residual - residual_new' residual_new. |
| const float true_gain = vecDot(residual, residual, meas_dim) |
| - vecDot(residual_new, residual_new, meas_dim); |
| |
| // predicted gain = 0.5 * step' * (damping_factor * step + gradient). |
| float tmp[MAX_LM_STATE_DIMENSION]; |
| vecScalarMul(tmp, step, damping_factor, state_dim); |
| vecAddInPlace(tmp, gradient, state_dim); |
| const float predicted_gain = 0.5f * vecDot(step, tmp, state_dim); |
| |
| // Check that we don't divide by zero! If denominator is too small, |
| // set gain_ratio = 1 to use the current step. |
| if (predicted_gain < kEps) { |
| return 1.f; |
| } |
| |
| return true_gain / predicted_gain; |
| } |
| |
| /* |
| * Tests if a solution is found based on the size of the step relative to the |
| * current state magnitude. Returns true if a solution is found. |
| * |
| * TODO(dvitus): consider optimization of this function to use squared norm |
| * rather than norm for relative error computation to avoid square root. |
| */ |
| bool checkRelativeStepSize(const float *step, const float *state, |
| size_t dim, float relative_error_threshold) { |
| // r = eps * (||x|| + eps) |
| const float relative_error = relative_error_threshold * |
| (vecNorm(state, dim) + relative_error_threshold); |
| |
| // solved if ||step|| <= r |
| // use squared version of this compare to avoid square root. |
| return (vecNormSquared(step, dim) <= relative_error * relative_error); |
| } |
| |
| /* |
| * Computes the residual, f(x), as well as the gradient and hessian of the cost |
| * function for the given state. |
| * |
| * Returns a boolean indicating if the computed gradient is sufficiently small |
| * to indicate that a solution has been found. |
| * |
| * INPUTS: |
| * state: state estimate (x) for which to compute the gradient & hessian. |
| * f_data: pointer to parameter data needed for the residual or jacobian. |
| * jacobian: pointer to temporary memory for storing jacobian. |
| * Must be at least MAX_LM_STATE_DIMENSION * MAX_LM_MEAS_DIMENSION. |
| * gradient_threshold: if gradient is below this threshold, function returns 1. |
| * |
| * OUTPUTS: |
| * residual: f(x). |
| * gradient: - J' f(x), where J = df(x)/dx |
| * hessian: df^2(x)/dx^2 = J' J |
| */ |
| bool computeResidualAndGradients(ResidualAndJacobianFunction func, |
| const float *state, const void *f_data, |
| float *jacobian, float gradient_threshold, |
| size_t state_dim, size_t meas_dim, |
| float *residual, float *gradient, |
| float *hessian) { |
| // Compute residual and Jacobian. |
| ASSERT_NOT_NULL(state); |
| ASSERT_NOT_NULL(residual); |
| ASSERT_NOT_NULL(gradient); |
| ASSERT_NOT_NULL(hessian); |
| func(state, f_data, residual, jacobian); |
| |
| // Compute the cost function hessian = jacobian' jacobian and |
| // gradient = -jacobian' residual |
| matTransposeMultiplyMat(hessian, jacobian, meas_dim, state_dim); |
| matTransposeMultiplyVec(gradient, jacobian, residual, meas_dim, state_dim); |
| vecScalarMulInPlace(gradient, -1.f, state_dim); |
| |
| // Check if solution is found (cost function gradient is sufficiently small). |
| return (vecMaxAbsoluteValue(gradient, state_dim) < gradient_threshold); |
| } |
| |
| /* |
| * Computes the Levenberg-Marquardt solver step to satisfy the following: |
| * (J'J + uI) * step = - J' f |
| * |
| * INPUTS: |
| * gradient: -J'f |
| * hessian: J'J |
| * L: temp memory of at least MAX_LM_STATE_DIMENSION * MAX_LM_STATE_DIMENSION. |
| * damping_factor: u |
| * dim: state dimension |
| * |
| * OUTPUTS: |
| * step: solution to the above equation. |
| * Function returns false if the solution fails (due to cholesky failure), |
| * otherwise returns true. |
| * |
| * Note that the hessian is modified in this function in order to reduce |
| * local memory requirements. |
| */ |
| bool computeStep(const float *gradient, float *hessian, float *L, |
| float damping_factor, size_t dim, float *step) { |
| |
| // 1) A = hessian + damping_factor * Identity. |
| matAddConstantDiagonal(hessian, damping_factor, dim); |
| |
| // 2) Solve A * step = gradient for step. |
| // a) compute cholesky decomposition of A = L L^T. |
| if (!matCholeskyDecomposition(L, hessian, dim)) { |
| return false; |
| } |
| |
| // b) solve for step via back-solve. |
| return matLinearSolveCholesky(step, L, gradient, dim); |
| } |
