Add serialization/deserialization support to mrob

[nosmokingsurfer]

Serialization support can be added for mrob package using python pickle mechanization if __getstate__ and __setstate__ methods will be defined for mrob classes needed to become pickleable. Here is brief example for mrob.geometry poses that looks like working but I didn't do extensive testing:

import mrob
import pickle
import numpy as np

def __getstate__(self):
    """Used for serializing instances"""
    
    # start with a copy so we don't accidentally modify the object state
    # or cause other conflicts
    if isinstance(self, mrob.SO3):
        state = self.R().copy()
    else:
        state = self.T().copy()

    # remove unpicklable entries
    # del ...
    return state

def __setstate__(self, state):
    """Used for deserializing"""
    # restore the state which was picklable
    if isinstance(self, mrob.SO3):
        self = mrob.SO3.__init__(self, state)
    elif isinstance(self, mrob.SE3tc):
        t = state[4,3]
        self = mrob.SE3tc.__init__(self,state,t)
    elif isinstance(self,mrob.SE3vel):
        R = mrob.SO3(state[:3,:3])
        p = state[:3, 3]
        v = state[:3, 4]
        self = mrob.SE3vel.__init__(self,R,p,v)
    elif isinstance(self, mrob.SE3):
        self = mrob.SE3.__init__(self,state)
    else:
        raise f"__setstate__ not implemented for clas {type(self)}!"

# adding those two functions to selected classes
# it is not the canonical way to add function to class, but it works and object can be pickled into file and
# later unpickled and compared with original and their matrices are the same
for cl in [mrob.SO3, mrob.SE3, mrob.SE3vel, mrob.SE3tc]:
    cl.__getstate__ = __getstate__
    cl.__setstate__ = __setstate__

if __name__ == "__main__":
    if True:
        T = mrob.SO3(np.array([0.1,0.2,0.3])).R()
        dump = mrob.SO3(T)

        print(dump.R())

        pickle.dump(dump, open('so3.pkl','wb'))

        load = pickle.load(open('so3.pkl','rb'))
        print(load)
        print(load.R())
        print(np.allclose(load.R(), dump.R()))

        # print(pickle.dumps(dump))
        # print(pickle.dumps([mrob.SO3(T) for _ in range(10)]))

    if True:
        T = np.eye(4)
        T[:3, :3] = mrob.SO3(np.array([0.1,0.2,0.3])).R()
        T[:3, 3] = np.random.rand(3)
        dump = mrob.SE3(T)

        print(dump.T())

        pickle.dump(dump, open('se3.pkl','wb'))

        load = pickle.load(open('se3.pkl','rb'))
        print(load)
        print(load.T())
        print(np.allclose(load.T(), dump.T()))

        # print(pickle.dumps(dump))
        # print(pickle.dumps([mrob.SE3(T) for _ in range(10)]))

    if True:
        T = np.eye(5)
        T[:3, :3] = mrob.SO3(np.array([0.1,0.2,0.3])).R()
        T[:3, 3] = np.random.rand(3)
        T[:3, 4] = np.random.rand(3)

        dump = mrob.SE3vel(mrob.SO3(T[:3, :3]), T[:3, 3],T[:3, 4])

        print(dump.T())

        pickle.dump(dump, open('se3vel.pkl','wb'))

        load = pickle.load(open('se3vel.pkl','rb'))
        print(load)
        print(load.T())
        print(np.allclose(load.T(), dump.T()))

        # print(pickle.dumps(dump))
        # print(pickle.dumps([mrob.SE3vel(mrob.SO3(T[:3, :3]), T[:3, 3],T[:3, 4]) for _ in range(10)]))


    if True:
        T = np.eye(5)
        T[:3, :3] = mrob.SO3(np.array([0.1,0.2,0.3])).R()
        T[:3, 3] = np.random.rand(3)
        dump = mrob.SE3tc(T, 1.0)

        print(dump.T())

        pickle.dump(dump, open('se3tc.pkl','wb'))

        load = pickle.load(open('se3tc.pkl','rb'))
        print(load)
        print(load.T())
        print(np.allclose(load.T(), dump.T()))

        # print(pickle.dumps(dump))
        # print(pickle.dumps([mrob.SE3tc(T,0.1) for _ in range(10)]))

For more complex objects like FGraph or FGraphSolver looks like similar approach can be applied if set/get methods will be added to have access to class attributes via python bindings (e.g. graph.set_information_matrix(matrix) will set state of information matrix and etc.).

I see the following steps required to have serialization support in mrob:

1. add setters/getters methods for required attributes in parent abstract classes (FGraph, Factor, FGraphSolve) and to basic classes (SE3, SO3 probably should have abstract parent) (C++)
2. force children classes to define setter, getter methods in C++ source code (C++)
3. forward set/get methods in bindings (C++)
4. Iterators for FGraph vertices and factors would be nice to have in python bindings - iterate through vertices list and factors-lists can be handy when performing __setstate__/__getstate__ (C++ and python)
5. Access by index to vertices/factors in graph also will be useful in general (C++ and python)
6. define __setstate__, __getstate__ in python bindings - can write some code in __init__ file of mrob package for example to define on python side when all functions are importing from mrob. __setstate__/__getstate__ will be using setter/getter methods of python bindings (python)
7. write unit tests for serialization/deserialization for pytest with 100% coverage of all classes we want make pickleable to automatically catch all issues (for example with numpy version) (python)
8. get pose/factor dim value with .get_dim() method