diff --git a/README.rst b/README.rst index 61ced42..61f5105 100644 --- a/README.rst +++ b/README.rst @@ -46,7 +46,7 @@ SE(3) Dataset >>> g.calc_chi2() - 16720.020602489112 + 16720.02100546733 >>> g.optimize() @@ -59,12 +59,11 @@ SE(3) Dataset Iteration chi^2 rel. change --------- ----- ----------- - 0 16720.0206 - 1 26.5495 -0.998412 - 2 1.2712 -0.952119 - 3 1.2402 -0.024439 - 4 1.2396 -0.000456 - 5 1.2395 -0.000091 + 0 16720.0210 + 1 45.6644 -0.997269 + 2 1.2936 -0.971671 + 3 1.2387 -0.042457 + 4 1.2387 -0.000001 +-----------------------------------------------------------------------------------------------------------------------+---------------------------------------------------------------------------------------------------------------------------------+ @@ -101,11 +100,11 @@ SE(2) Dataset Iteration chi^2 rel. change --------- ----- ----------- 0 7191686.3825 - 1 319915276.1284 43.484042 - 2 124894535.1749 -0.609601 - 3 338185.8171 -0.997292 - 4 734.5142 -0.997828 - 5 215.8405 -0.706145 + 1 319916668.8138 43.484235 + 2 124888469.1437 -0.609622 + 3 338171.6169 -0.997292 + 4 734.5693 -0.997828 + 5 215.8405 -0.706167 6 215.8405 -0.000000 diff --git a/graphslam/util.py b/graphslam/util.py index fd1368b..4054f63 100644 --- a/graphslam/util.py +++ b/graphslam/util.py @@ -64,11 +64,10 @@ def upper_triangular_matrix_to_full_matrix(arr, n): """ triu0 = np.triu_indices(n, 0) - triu1 = np.triu_indices(n, 1) tril1 = np.tril_indices(n, -1) mat = np.zeros((n, n), dtype=np.float64) mat[triu0] = arr - mat[tril1] = mat[triu1] + mat[tril1] = mat.T[tril1] return mat diff --git a/tests/test_util.py b/tests/test_util.py index fe63a25..75183a9 100644 --- a/tests/test_util.py +++ b/tests/test_util.py @@ -42,13 +42,13 @@ def test_upper_triangular_matrix_to_full_matrix(self): """Test the ``upper_triangular_matrix_to_full_matrix()`` function. """ - arr = np.array([1, 2, 3, 4, 5, 6]) + arrays = [np.array([1, 2, 3, 4, 5, 6]), np.array([1, 2, 3, 4, 5, 6, 7, 8, 9, 10])] + answers = [np.array([[1, 2, 3], [2, 4, 5], [3, 5, 6]], dtype=np.float64), np.array([[1, 2, 3, 4], [2, 5, 6, 7], [3, 6, 8, 9], [4, 7, 9, 10]])] + dims = [3, 4] - mat = util.upper_triangular_matrix_to_full_matrix(arr, 3) - - ans = np.array([[1, 2, 3], [2, 4, 5], [3, 5, 6]], dtype=np.float64) - - self.assertAlmostEqual(np.linalg.norm(mat - ans), 0.) + for array, answer, dim in zip(arrays, answers, dims): + mat = util.upper_triangular_matrix_to_full_matrix(array, dim) + self.assertAlmostEqual(np.linalg.norm(mat - answer), 0.) if __name__ == '__main__':