# 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 npTWO_PI = 2 * np.pidef neg_pi_to_pi(angle):r"""Normalize ``angle`` to be in :math:`[-\pi, \pi)`.Parameters----------angle : floatAn angle (in radians)Returns-------floatThe angle normalized to :math:`[-\pi, \pi)`"""return (angle + np.pi) % (TWO_PI) - np.pidef 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} = nReturns-------intThe 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.ndarrayThe upper triangular portion of the matrixn : intThe size of the matrixReturns-------mat : np.ndarrayThe full matrix"""triu0 = np.triu_indices(n, 0)triu1 = np.triu_indices(n, 1)tril1 = np.tril_indices(n, -1)mat = np.zeros((n, n), dtype=float)mat[triu0] = arrmat[tril1] = mat[triu1]return mat
此处可能存在不合适展示的内容,页面不予展示。您可通过相关编辑功能自查并修改。
如您确认内容无涉及 不当用语 / 纯广告导流 / 暴力 / 低俗色情 / 侵权 / 盗版 / 虚假 / 无价值内容或违法国家有关法律法规的内容,可点击提交进行申诉,我们将尽快为您处理。