navlie.batch.gaussian_mixtures.HessianSumMixtureResidual¶

class navlie.batch.gaussian_mixtures.HessianSumMixtureResidual(errors: List[Residual], weights, no_use_complex_numbers=True)¶

Bases: GaussianMixtureResidual

The Hessian-Sum-Mixture method patched for compatibility with nonlinear least squares solvers. Based on the following reference: @misc{korotkine2024hessian, title={A Hessian for Gaussian Mixture Likelihoods in Nonlinear Least Squares}, author={Vassili Korotkine and Mitchell Cohen and James Richard Forbes}, year={2024}, eprint={2404.05452}, archivePrefix={arXiv}, primaryClass={cs.RO} }

Parameters:

errors (List[Residual]) – List of Residuals, each of which must implement the sqrt_info_matrix method.
weights (List[float]) – Weights of the Gaussian mixture.

no_use_complex_numbers: bool¶

static get_normalization_constant(alphas: List[float])¶

evaluate(states: List[State], compute_jacobians: List[bool] | None = None) → Tuple[ndarray, List[ndarray]]¶

Evaluates the residual and Jacobians.

Parameters:

states (List[State]) – List of states for optimization.
compute_jacobian (List[bool], optional) – optional flag to compute Jacobians, by default None

Returns:

Returns the error and a list of Jacobians.

Return type:

Tuple[np.ndarray, List[np.ndarray]]

evaluate_component_residuals(states: List[State], compute_jacobians: List[bool] | None = None) → Tuple[List[ndarray], List[List[ndarray]], List[ndarray]]¶

jacobian_fd(states: List[State], step_size=1e-06) → List[ndarray]¶

Calculates the model jacobian with finite difference.

Parameters:: states (List[State]) – Evaluation point of Jacobians, a list of states that the residual is a function of.
Returns:: A list of Jacobians of the measurement model with respect to each of the input states. For example, the first element of the return list is the Jacobian of the residual w.r.t states[0], the second element is the Jacobian of the residual w.r.t states[1], etc.
Return type:: List[np.ndarray]

mix_errors(error_value_list: List[ndarray], sqrt_info_matrix_list: List[ndarray]) → List[ndarray]¶

Each mixture must implement this method.. Compute the factor error from the errors corresponding to each component

All errors are assumed to be normalized and have identity covariance.

Parameters:: error_value_list (List[np.ndarray],) – List of errors corresponding to each component

sqrt_info_matrix(states: List[State])¶: Returns the information matrix

mix_jacobians(error_value_list: List[ndarray], jacobian_list_of_lists: List[List[ndarray]], sqrt_info_matrix_list: List[ndarray], reused_values: Dict | None = None) → List[ndarray]¶

Each mixture must implement this method. For every state, compute Jacobian of the Gaussian mixture w.r.t. that state

Parameters:

error_value_list (List[np.ndarray],) – List of errors corresponding to each component
jacobian_list (List[List[np.ndarray]]) – Outer list corresponds to each component, for each of which the inner list contains the component Jacobians w.r.t. every state.

navlie.batch.gaussian_mixtures.GaussianMixtureResidual

navlie.batch.gaussian_mixtures.MaxMixtureResidual