Efficient way to convert numpy array to SE2 object

[LukasStu]

Hi, I'm trying to convert an array of x,y,theta values to a SE2 Objekt. I used this function to do so.

def ar2se2(array):
    """Create se2-object from numpy array"""
    poselist = list(array)
    T = SE2().Empty()
    for p in poselist:
        T.append(SE2(p, unit='deg'))
    return T

Is there a more efficient way?

Thank you,

Lukas

[petercorke]

Sorry, no. You could write that as

T = SE2([SE2(p) for p in poselist])

which is shorter, but there's quite a bit of constructor overhead.

What would be better? A list of 3-tuples or an Nx3 array, which can get confused for a 3x3 SE(2) matrix.

[petercorke]

or

from spatialmath.base import xyt2tr
T = SE2([xyt2tr(p) for p in poselist])

which avoids a lot of the constructor overhead, xyt2tr converts (x,y,θ) to an SE(2) matrix.

[LukasStu]

I compared the execution time for trajectories of length 20000 and found out that your first suggestion is a bit faster than the second one. If you don't use SE2 objects it is of course the fastest, but not so comfortable anymore if you want to calculate with the trajectories. For example SE2.inv(T)*T... Thanks for your suggestions

from spatialmath.base import xyt2tr
from spatialmath import SE2
import numpy as np
import time

# execution time Corke 1:  1.115886926651001
# execution time Corke 2:  1.8799126148223877
# execution time Stue 1:   1.2093167304992676
# execution time numpy:    0.9414606094360352

np.random.seed(23)

"""Input"""
l = 20000
rand1 = np.random.random(l)
rand2 = np.random.random(l)
rand3 = np.random.random(l)

gt = np.vstack((rand1,rand2,rand3)).T

t0 = time.time()
T1 = SE2([SE2(p) for p in gt]) 
t1 = time.time()
print("execution time Corke 1:",t1-t0)


t0 = time.time()
T2 = SE2([xyt2tr(p,unit='deg') for p in gt])
t1 = time.time()
print("execution time Corke 2:",t1-t0)


t0 = time.time()
T3 = SE2().Empty()
for p in gt:
    T3.append(SE2(p, unit='deg')) 
    t1 = time.time()
print("execution time Stue 1:",t1-t0)


t0 = time.time()
T4 = np.array([xyt2tr(p,unit='deg') for p in gt])
t1 = time.time()
print("execution time numpy:",t1-t0)