graphslam.util module

Utility functions used throughout the package.

graphslam.util.neg_pi_to_pi(angle)

Normalize angle to be in \([-\pi, \pi)\).

Parameters:

angle (float) – An angle (in radians)

Returns:

The angle normalized to \([-\pi, \pi)\)

Return type:

float

graphslam.util.solve_for_edge_dimensionality(n)

Solve for the dimensionality of an edge.

In a .g2o file, an edge is specified as <estimate> <information matrix>, where only the upper triangular portion of the matrix is provided.

This solves the problem:

\[d + \frac{d (d + 1)}{2} = n\]

Returns:

The dimensionality of the edge

Return type:

int

graphslam.util.upper_triangular_matrix_to_full_matrix(arr, n)

Given an upper triangular matrix, return the full matrix.

Parameters:

• arr (np.ndarray) – The upper triangular portion of the matrix

• n (int) – The size of the matrix

Returns:

mat – The full matrix

Return type:

np.ndarray