Repository: dfki-ric/pytransform3d (language: python)

Issue:
Feature request: serialization of TransformManager
It would be great if TransformManager had a method to serialize/load TransfromManager objects into/from files. We are currently using pickle but it sometimes generate errors when the module versions (e.g., SciPy) differ.


Discussion / hints:
Hi @ljmanso ,

thanks for the feedback. I was about to suggest using pickle. Do you have a format suggestion? The problem with text-based representations is that you lose precision in floats, but it would be the easiest to export to json without any additional dependencies.
Thanks for the quick reply.

An option would be to serialize arrays to strings (e.g., using numpy.array2string) and then have them into the JSON file as a strings so that JSON does not mess with the precision. I don't think that JSON would be able to serialize np arrays anyway.

The following shows how it would work: 

```
import json
import numpy as np

# This structure cannot be directly serialized
some_structure = {'RF1': np.array([1.1234567890123456789,np.pi])}
#dump = json.dumps(some_structure)
#print(dump)

# We need to make it "json-compatible"
json_ok = {}
for k, v in some_structure.items():
    v2 = np.array2string(v, precision=32)
    json_ok[k] = v2
dump = json.dumps(json_ok)
print(dump)
```

I am not familiar with the internals of pytransform3d, but please let me know if I can help with this.


Let me add the de-serialization to the previous example:

```
import json
import numpy as np

np.set_printoptions(precision=32)

# From https://stackoverflow.com/questions/35750639/how-can-a-string-representation-of-a-numpy-array-be-converted-to-a-numpy-array
def string_to_numpy(text, dtype=None):
    """
    Convert text into 1D or 2D arrays using np.matrix().
    The result is returned as an np.ndarray.
    """
    import re
    text = text.strip()
    # Using a regexp, decide whether the array is flat or not.
    # The following matches either: "[1 2 3]" or "1 2 3"
    is_flat = bool(re.match(r"^(\[[^\[].+[^\]]\]|[^\[].+[^\]])$",
                            text, flags=re.S))
    # Replace newline characters with semicolons.
    text = text.replace("]\n", "];")
    # Prepare the result.
    result = np.asarray(np.matrix(text, dtype=dtype))
    return result.flatten() if is_flat else result


# This structure cannot be directly serialized
some_structure = {'RF1': np.array([1.1234567890123456789,np.pi])}
print(some_structure)
#dump = json.dumps(some_structure)
#print(dump)

# We need to make it "json-compatible"
json_ok = {}
for k, v in some_structure.items():
    v2 = np.array_str(v, precision=32)
    json_ok[k] = v2
serialised = json.dumps(json_ok)
print(serialised)


# We can convert it back from the string, but we still need to convert those strings into ndarrays
read_from_json = json.loads(serialised)
print(read_from_json)
deserialised = {}
for k, v in read_from_json.items():
    v2 = string_to_numpy(v)
    deserialised[k] = v2

print(deserialised)

```






Fix patch:
diff --git a/pytransform3d/transform_manager.py b/pytransform3d/transform_manager.py
index b4aa61952..9cca0bcb0 100644
--- a/pytransform3d/transform_manager.py
+++ b/pytransform3d/transform_manager.py
@@ -38,6 +38,13 @@ class TransformManager(object):
         for large graphs. You can check the consistency explicitly with
         :func:`TransformManager.check_consistency`.
 
+    The TransformManager does not directly support serialization because
+    we don't want to decide for a specific format. However, it allows
+    conversion to a dict with only primitive types that is serializable,
+    for instance, as JSON. If a more compact format is required, binary
+    formats like msgpack can be used. Use :func:`TransformManager.to_dict`
+    and :func:`TransformManager.from_dict` for this purpose.
+
     Parameters
     ----------
     strict_check : bool, optional (default: True)
@@ -62,13 +69,13 @@ def __init__(self, strict_check=True, check=True):
         # We have to store the index n associated to a transformation to be
         # able to remove the transformation later
         self.transform_to_ij_index = {}
-        # Same information as sparse matrix
+        # Connection information as sparse matrix
         self.connections = sp.csr_matrix((0, 0))
 
         # Result of shortest path algorithm:
         # distance matrix (distance is the number of transformations)
         self.dist = np.empty(0)
-        self.predecessors = np.empty(0)
+        self.predecessors = np.empty(0, dtype=np.int32)
 
         self._cached_shortest_paths = {}
 
@@ -485,6 +492,79 @@ def write_png(self, filename, prog=None):  # pragma: no cover
 
         graph.write_png(filename, prog=prog)
 
+    def to_dict(self):
+        """Convert the transform manager to a dict that is serializable.
+
+        Returns
+        -------
+        tm_dict : dict
+            Serializable dict.
+        """
+        return {
+            "class": self.__class__.__name__,
+            "strict_check": self.strict_check,
+            "check": self.check,
+            "transforms": [(k, v.ravel().tolist())
+                           for k, v in self.transforms.items()],
+            "nodes": self.nodes,
+            "i": self.i,
+            "j": self.j,
+            "transform_to_ij_index": list(self.transform_to_ij_index.items()),
+            "connections": {
+                "data": self.connections.data.tolist(),
+                "indices": self.connections.indices.tolist(),
+                "indptr": self.connections.indptr.tolist()
+            },
+            "dist": self.dist.tolist(),
+            "predecessors": self.predecessors.tolist()
+        }
+
+    @staticmethod
+    def from_dict(tm_dict):
+        """Create transform manager from dict.
+
+        Parameters
+        ----------
+        tm_dict : dict
+            Serializable dict.
+
+        Returns
+        -------
+        tm : TransformManager
+            Deserialized transform manager.
+        """
+        strict_check = tm_dict.get("strict_check")
+        check = tm_dict.get("check")
+        tm = TransformManager(strict_check=strict_check, check=check)
+        tm.set_transform_manager_state(tm_dict)
+        return tm
+
+    def set_transform_manager_state(self, tm_dict):
+        """Set state of transform manager from dict.
+
+        Parameters
+        ----------
+        tm_dict : dict
+            Serializable dict.
+        """
+        transforms = tm_dict.get("transforms")
+        self.transforms = dict([
+            (tuple(k), np.array(v).reshape(4, 4)) for k, v in transforms])
+        self.nodes = tm_dict.get("nodes")
+        self.i = tm_dict.get("i")
+        self.j = tm_dict.get("j")
+        self.transform_to_ij_index = dict(
+            [(tuple(k), v) for k, v in tm_dict.get("transform_to_ij_index")])
+        connections = tm_dict.get("connections")
+        self.connections = sp.csr_matrix((
+            connections["data"], connections["indices"],
+            connections["indptr"]))
+        n_nodes = len(self.nodes)
+        dist = np.array(tm_dict.get("dist"))
+        self.dist = dist.reshape(n_nodes, n_nodes)
+        predecessors = np.array(tm_dict.get("predecessors"), dtype=np.int32)
+        self.predecessors = predecessors.reshape(n_nodes, n_nodes)
+
 
 def _dot_display_name(name):  # pragma: no cover
     return name.replace("/", "")
diff --git a/pytransform3d/transform_manager.pyi b/pytransform3d/transform_manager.pyi
index 9f167b57a..9fa354ae6 100644
--- a/pytransform3d/transform_manager.pyi
+++ b/pytransform3d/transform_manager.pyi
@@ -2,7 +2,7 @@ import numpy as np
 import numpy.typing as npt
 import scipy.sparse as sp
 from mpl_toolkits.mplot3d import Axes3D
-from typing import Dict, Tuple, List, Union, Set, Hashable
+from typing import Dict, Tuple, List, Union, Set, Hashable, Any
 
 
 class TransformManager(object):
@@ -57,3 +57,10 @@ class TransformManager(object):
     def _shortest_path(self, i: int, j: int) -> List[str]: ...
 
     def _path_transform(self, path: List[Hashable]) -> np.ndarray: ...
+
+    def to_dict(self) -> Dict[str, Any]: ...
+
+    @staticmethod
+    def from_dict(tm_dict: Dict[str, Any]) -> "TransformManager": ...
+
+    def set_transform_manager_state(self, tm_dict: Dict[str, Any]): ...
diff --git a/pytransform3d/urdf.pyi b/pytransform3d/urdf.pyi
index f36c85571..6906b7dc6 100644
--- a/pytransform3d/urdf.pyi
+++ b/pytransform3d/urdf.pyi
@@ -9,8 +9,8 @@ from typing import Dict, Tuple, List, Any, Union, Type, Hashable
 class UrdfTransformManager(TransformManager):
     _joints: Dict[str, Tuple[Hashable, Hashable, np.ndarray, np.ndarray,
                              Tuple[float, float], str]]
-    collision_objects: List[Any]
-    visuals: List[Any]
+    collision_objects: List[Geometry]
+    visuals: List[Geometry]
 
     def __init__(self, strict_check: bool = ..., check: bool = ...): ...
 


Test patch:
diff --git a/pytransform3d/test/test_transform_manager.py b/pytransform3d/test/test_transform_manager.py
index 72b5f6e57..2692cc94f 100644
--- a/pytransform3d/test/test_transform_manager.py
+++ b/pytransform3d/test/test_transform_manager.py
@@ -289,3 +289,20 @@ def test_remove_transform():
         KeyError, "Cannot compute path", tm.get_transform, "A", "D")
     tm.get_transform("B", "A")
     tm.get_transform("D", "C")
+
+
+def test_from_to_dict():
+    random_state = np.random.RandomState(2323)
+    tm = TransformManager()
+    A2B = random_transform(random_state)
+    tm.add_transform("A", "B", A2B)
+    B2C = random_transform(random_state)
+    tm.add_transform("B", "C", B2C)
+    C2D = random_transform(random_state)
+    tm.add_transform("C", "D", C2D)
+
+    tm_dict = tm.to_dict()
+    tm2 = TransformManager.from_dict(tm_dict)
+
+    assert_array_almost_equal(tm.get_transform("D", "A"),
+                              tm2.get_transform("D", "A"))
diff --git a/pytransform3d/test/test_urdf.py b/pytransform3d/test/test_urdf.py
index 4949f6635..cc111ce51 100644
--- a/pytransform3d/test/test_urdf.py
+++ b/pytransform3d/test/test_urdf.py
@@ -3,6 +3,8 @@
     matplotlib_available = True
 except ImportError:
     matplotlib_available = False
+import os
+import json
 import numpy as np
 import warnings
 from pytransform3d.urdf import (
@@ -1160,3 +1162,26 @@ def test_load_urdf_functional():
                   [0, 0, 1, 1.124],
                   [0, 0, 0, 1]])
     )
+
+
+def test_serialization():
+    tm = UrdfTransformManager()
+    tm.load_urdf(COMPI_URDF)
+    for i in range(1, 7):
+        tm.set_joint("joint%d" % i, 0.1)
+
+    filename = "compi.json"
+    try:
+        tm_dict = tm.to_dict()
+        with open(filename, "w") as f:
+            json.dump(tm_dict, f)
+        with open(filename, "r") as f:
+            tm_dict2 = json.load(f)
+        tm2 = UrdfTransformManager.from_dict(tm_dict2)
+
+        tcp2mount = tm.get_transform("tcp", "linkmount")
+        tcp2mount2 = tm2.get_transform("tcp", "linkmount")
+        assert_array_almost_equal(tcp2mount, tcp2mount2)
+    finally:
+        if os.path.exists(filename):
+            os.remove(filename)