Fix upper_triangular_matrix_to_full_matrix (#29)

* Fix `upper_triangular_matrix_to_full_matrix` * Add test * Update README
JeffLIrion · Mar 17, 2022 · 8aad5e9 · 8aad5e9
1 parent d70618e
commit 8aad5e9
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 20 deletions.
diff --git 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
@@ -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
@@ -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__':