mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-12 20:58:21 -05:00
78 lines
1.5 KiB
Python
78 lines
1.5 KiB
Python
# Copyright (c) 2020 Jeff Irion and contributors
|
|
#
|
|
# This file originated from the `graphslam` package:
|
|
#
|
|
# https://github.com/JeffLIrion/python-graphslam
|
|
|
|
"""Utility functions used throughout the package.
|
|
|
|
"""
|
|
|
|
import numpy as np
|
|
|
|
|
|
TWO_PI = 2 * np.pi
|
|
|
|
|
|
def neg_pi_to_pi(angle):
|
|
r"""Normalize ``angle`` to be in :math:`[-\pi, \pi)`.
|
|
|
|
Parameters
|
|
----------
|
|
angle : float
|
|
An angle (in radians)
|
|
|
|
Returns
|
|
-------
|
|
float
|
|
The angle normalized to :math:`[-\pi, \pi)`
|
|
|
|
"""
|
|
return (angle + np.pi) % (TWO_PI) - np.pi
|
|
|
|
|
|
def solve_for_edge_dimensionality(n):
|
|
r"""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:
|
|
|
|
.. math::
|
|
|
|
d + \frac{d (d + 1)}{2} = n
|
|
|
|
Returns
|
|
-------
|
|
int
|
|
The dimensionality of the edge
|
|
|
|
"""
|
|
return int(round(np.sqrt(2 * n + 2.25) - 1.5))
|
|
|
|
|
|
def 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 : np.ndarray
|
|
The full matrix
|
|
|
|
"""
|
|
triu0 = np.triu_indices(n, 0)
|
|
tril1 = np.tril_indices(n, -1)
|
|
|
|
mat = np.zeros((n, n), dtype=float)
|
|
mat[triu0] = arr
|
|
mat[tril1] = mat.T[tril1]
|
|
|
|
return mat
|