Matrix multiplication of numpy array and SO/SE object

[stefangachter]

I got stuck with the following. I read the documentation and study the source code. The later, up to a certain degree.
I have a covariance matrix A that I would like to rotate by C. The covariance and rotation matrices are given by 2x2 numpy arrays:

>>> import numpy as np
>>> A = np.array([[10,0],[0,1]])
>>> alpha=0.5
>>> C = np.array([[np.cos(alpha), -np.sin(alpha)],[np.sin(alpha), np.cos(alpha)]])
>>> C
array([[ 0.87758256, -0.47942554],
       [ 0.47942554,  0.87758256]])
>>> B = C @ A @ np.transpose(C)
>>> B
array([[7.93136038, 3.78661943],
       [3.78661943, 3.06863962]])

If the rotation is given by SO2, then the following happens:

>>> from spatialmath import SO2
>>> Csm = SO2(0.5)
>>> Csm
SO2(array([[ 0.87758256, -0.47942554],
           [ 0.47942554,  0.87758256]]))
>>> Bsm = Csm * A * Csm.inv()
>>> Bsm
array([[1.00000000e+01, 2.58022041e-17],
       [3.59774358e-17, 1.00000000e+00]])

If I am not mistaken, the multiplication of the SO2 by the 2x2 numpy array is "column-wise". The operator * acts per column and not as a matrix multiplication. I can understand this behavior if one wants to transform a sequence of vectors but this can be a source of nasty errors.
I am wondering then how to best represent a "covariance matrix". I could not find a solution in the documentation.
One solution could be

>>> Csm.A @ A @ np.transpose(Csm.A)
array([[7.93136038, 3.78661943],
       [3.78661943, 3.06863962]])

But is there a better way? Is it possible to give the covariance a "matrix property" such that SE2 understands it?

[stefangachter]

If I am not mistaken, the multiplication of SO2 by 2x2 matrix was considered in past, see baseposematrix.py, __mul__ operator:

            elif (
                len(left) == 1
                and isinstance(right, np.ndarray)
                and left.isSO
                and right.shape[0] == left.N
            ):
                # SO(n) x matrix
                return left.A @ right

but is ignored now, because this condition is never met.

[stefangachter]

Ignored, because the case would be ambiguous.

[petercorke]

Hi @stefangachter thanks for your interest and good questions. Consider C * A * C.inv(), then the Python is going to compute (C * A) * C.inv(). The result of the first expression is a NumPy array, based on matrix multiplication as desired. The second * will do an element-wise product because I'd not considered the case of premultiplication by a matrix, as the doc strings for __rmul__ says, it's for scalar multiplication.

>>> C*A@(C.A.T)
array([[   7.931,    3.787],
       [   3.787,    3.069]])

works but is pretty ugly.

There are a few options that I'd be interested in your comments on.

• Make a Covariance matrix class that encapsulates a NumPy array. They are important objects in robotics.
• Overload a different operator, but there are not many good ones to choose from, perhaps C >> A ??
• Add a method to SO3, I think this operation is a conjugation, so C.conjugation(A)
• Change __rmul__ to handle this case, smallish change, not yet on GH (~ line 1282)

        if left.shape == right.A.shape:
            return left @ right.A
        else:
            return right.__mul__(left)

which leads to

>>> C * A * C.inv()
array([[   7.931,    3.787],
       [   3.787,    3.069]])

I'd be happy to implement the third and/or fourth options, straightforward and a win for users.

[stefangachter]

Thanks, Peter, for taking a closer look. I am thinking of making a covariance class because covariances are important objects as you point out. If I am not mistaken, spatialmath does not yet offer "covariances". Only a function to plot them in 2D. Though, it will take me a while to come up with a covariance class.
In any case, I would add option 4 (rmul) and, if time permits, option 3 (conjugation). Option 4 would add a "low-level" functionality. (I am not a mathematician. Conjugation seems to be the correct term to me, though: https://mathworld.wolfram.com/ConjugateMatrix.html)

[petercorke]

ok, let me do this for the 1.1.6 push. If/when the Covariance class comes together I'd be happy to include it in SMTB. Would you do the 3D case as well? And the pose cases in 2D and 3D?