navlie.batch.gaussian_mixtures.GaussianMixtureResidual¶
- class navlie.batch.gaussian_mixtures.GaussianMixtureResidual(errors: List[Residual], weights: List[float])¶
-
A Gaussian mixture residual. Gaussian mixtures can, for instance, model non-Gaussian noise where a mixture has been fit to it, unknown data associations, or loop closures where one component corresponds to a true loop closure (small covariance) and another component corresponds to a false loop closure (large covariance).
Implements problem terms of the form
\[J(\mathbf{x}) = -\log \sum_{k=1}^{K} w_k \det\left(\sqrt{\mathbf{R}_k^{-1}} \right) \exp (\boldsymbol{\eta}_k^T (\mathbf{x}) \mathbf{R}_k^{-1} \boldsymbol{\eta}_k (\mathbf{x}))\]where defining the normalized error \(\mathbf{e}_k (\mathbf{x}=\sqrt{\mathbf{R}_k^{-1}} \boldsymbol{\eta}_k (\mathbf{x})\) yields
\[J(\mathbf{x}) = -\log \sum_{k=1}^{K} w_k \det\left(\sqrt{\mathbf{R}_k^{-1}} \right) \exp (\mathbf{e}_k^T (\mathbf{x}) \mathbf{e}_k (\mathbf{x})).\]The errors argument input to the constructor define the errors \(\mathbf{e}_k (\mathbf{x})\) and the weights define the weights \(w_k\). The error must define the sqrt_info_matrix method that returns the square root of the information matrix \(\sqrt{\mathbf{R}_k^{-1}}\).
Each type of mixture differs in how the overall error (Jacobian) corresponding to the gaussian mixture is constructed from the component errors (component Jacobians). Therefore, each subclass of GaussianMixtureResidual must overwrite the mix_errors and mix_jacobians methods.
- Parameters:
- abstract mix_errors(error_value_list: List[ndarray], sqrt_info_matrix_list: List[ndarray]) → Tuple[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
- abstract 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) → Tuple[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.
- evaluate_component_residuals(states: List[State], compute_jacobians: List[bool] | None = None) → Tuple[List[ndarray], List[List[ndarray]], List[ndarray]]¶
- evaluate(states: List[State], compute_jacobians: List[bool] | None = None) → Tuple[ndarray, List[ndarray]]¶
Evaluates the residual and Jacobians.
- 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]