navlie.batch.gaussian_mixtures.SumMixtureResidual¶

class navlie.batch.gaussian_mixtures.SumMixtureResidual(errors: List[Residual], weights: List[float])¶

Bases: GaussianMixtureResidual

For details see the reference @ARTICLE{9381625, author={Pfeifer, Tim and Lange, Sven and Protzel, Peter}, journal={IEEE Robotics and Automation Letters}, title={Advancing Mixture Models for Least Squares Optimization}, year={2021}, volume={6}, number={2}, pages={3941-3948}, doi={10.1109/LRA.2021.3067307}}

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.

mix_errors(error_value_list: List[ndarray], sqrt_info_matrix_list: List[ndarray]) → Tuple[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

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[ndarray, 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(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]

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

navlie.batch.gaussian_mixtures.MaxSumMixtureResidual

navlie.batch.losses