graphslam.edge.base_edge module
A base class for edges.
- class graphslam.edge.base_edge.BaseEdge(vertex_ids, information, estimate, vertices=None)[source]
Bases:
ABC
A class for representing edges in Graph SLAM.
- Parameters:
vertex_ids (list[int]) – The IDs of all vertices constrained by this edge
information (np.ndarray) – The information matrix \(\Omega_j\) associated with the edge
estimate (BasePose, np.ndarray, float) – The expected measurement \(\mathbf{z}_j\)
vertices (list[graphslam.vertex.Vertex], None) – A list of the vertices constrained by the edge
- information
The information matrix \(\Omega_j\) associated with the edge
- Type:
np.ndarray
- vertex_ids
The IDs of all vertices constrained by this edge
- Type:
list[int]
- vertices
A list of the vertices constrained by the edge
- Type:
list[graphslam.vertex.Vertex], None
- _NUMERICAL_DIFFERENTIATION_EPSILON = 1e-06
The difference that will be used for numerical differentiation
- _abc_impl = <_abc._abc_data object>
- _calc_jacobian(err, dim, vertex_index)[source]
Calculate the Jacobian of the edge with respect to the specified vertex’s pose.
- Parameters:
err (np.ndarray) – The current error for the edge (see \(BaseEdge.calc_error\))
dim (int) – The dimensionality of the compact pose representation
vertex_index (int) – The index of the vertex (pose) for which we are computing the Jacobian
- Returns:
The Jacobian of the edge with respect to the specified vertex’s pose
- Return type:
np.ndarray
- _is_valid()[source]
Check some basic criteria for the edge.
- Returns:
Whether the basic validity criteria for the edge are satisfied
- Return type:
bool
- calc_chi2()[source]
Calculate the \(\chi^2\) error for the edge.
\[\mathbf{e}_j^T \Omega_j \mathbf{e}_j\]- Returns:
The \(\chi^2\) error for the edge
- Return type:
float
- calc_chi2_gradient_hessian()[source]
Calculate the edge’s contributions to the graph’s \(\chi^2\) error, gradient (\(\mathbf{b}\)), and Hessian (\(H\)).
- Returns:
float – The \(\chi^2\) error for the edge
list[tuple[int, np.ndarray]] – The edge’s contribution(s) to the gradient
list[tuple[tuple[int, int], np.ndarray]] – The edge’s contribution(s) to the Hessian
- abstract calc_error()[source]
Calculate the error for the edge: \(\mathbf{e}_j \in \mathbb{R}^\bullet\).
- Returns:
The error for the edge
- Return type:
np.ndarray, float
- calc_jacobians()[source]
Calculate the Jacobian of the edge’s error with respect to each constrained pose.
\[\frac{\partial}{\partial \Delta \mathbf{x}^k} \left[ \mathbf{e}_j(\mathbf{x}^k \boxplus \Delta \mathbf{x}^k) \right]\]- Returns:
The Jacobian matrices for the edge with respect to each constrained pose
- Return type:
list[np.ndarray]
- equals(other, tol=1e-06)[source]
Check whether two edges are equal.
- Parameters:
other (BaseEdge) – The edge to which we are comparing
tol (float) – The tolerance
- Returns:
Whether the two edges are equal
- Return type:
bool
- classmethod from_g2o(line, g2o_params_or_none=None)[source]
Load an edge from a line in a .g2o file.
Note
Overload this method to support loading from .g2o files.
- Parameters:
line (str) – The line from the .g2o file
g2o_params_or_none (dict, None) – A dictionary where the values are graphslam.g2o_parameters.BaseG2OParameters objects, or
None
if there are no such parameters
- Returns:
The instantiated edge object, or
None
ifline
does not correspond to this edge type (or if this edge type does not support loading from g2o)- Return type:
BaseEdge, None
- abstract is_valid()[source]
Check that the edge is valid.
The vertices attribute is populated, it is the correct length, and the poses are the correct types
The estimate attribute is the correct type and length
The information attribute is the right shape
Any other checks
- Returns:
Whether the edge is valid
- Return type:
bool