se2_localization example displaying trajectory in xy

[mnissov]

Output from running se2_localization.py is:

X STATE     :    X      Y      Z    TH_x   TH_y   TH_z 
-------------------------------------------------------
X initial   :  [0. 0. 0.]
-------------------------------------------------------
X simulated :  [0.1  0.   0.05]
X estimated :  [ 0.097 -0.011  0.053]
X unfilterd :  [0.136 0.054 0.147]
-------------------------------------------------------
X simulated :  [ 0.2 -0.   0.1]
X estimated :  [ 0.197 -0.005  0.098]
X unfilterd :  [0.316 0.136 0.211]
-------------------------------------------------------
X simulated :  [ 0.3  -0.    0.15]
X estimated :  [ 0.297 -0.001  0.148]
X unfilterd :  [0.474 0.133 0.333]
-------------------------------------------------------
X simulated :  [ 0.4 -0.   0.2]
X estimated :  [ 0.401 -0.01   0.201]
X unfilterd :  [0.654 0.178 0.456]
-------------------------------------------------------
X simulated :  [0.5  0.   0.25]
X estimated :  [ 0.495 -0.01   0.252]
X unfilterd :  [0.786 0.267 0.569]
-------------------------------------------------------
X simulated :  [ 0.6 -0.   0.3]
X estimated :  [ 0.603 -0.011  0.301]
X unfilterd :  [0.917 0.294 0.711]
-------------------------------------------------------
X simulated :  [ 0.7  -0.    0.35]
X estimated :  [ 0.695 -0.011  0.353]
X unfilterd :  [1.028 0.308 0.842]
-------------------------------------------------------
X simulated :  [ 0.8 -0.   0.4]
X estimated :  [ 0.804 -0.007  0.4  ]
X unfilterd :  [1.147 0.386 0.984]
-------------------------------------------------------
X simulated :  [ 0.9  -0.    0.45]
X estimated :  [ 0.897 -0.01   0.452]
X unfilterd :  [1.308 0.42  1.064]
-------------------------------------------------------
X simulated :  [1.  0.  0.5]
X estimated :  [ 0.998 -0.008  0.501]
X unfilterd :  [1.429 0.447 1.124]
-------------------------------------------------------

if the input u_nom = Vector([0.1, 0.0, 0.05]) this means driving straightforward and rotating (if I'm not mistaken). Shouldn't this then mean that the object is driving in a circle? Because the printed statements show only a straight line.

I think I may be misunderstanding something.

[artivis]

Hi @mnissov,

This demo (and likely all the other) is not displaying the actual state but the state log. Remove the .log() in the prints to see the full states printed out. Mind that in that case they are [x, y, real, imag].

X initial   :  [0. 0. 0.]
-------------------------------------------------------
X simulated :  [0.1   0.002 0.999 0.05 ]
X estimated :  [ 0.098 -0.004  0.999  0.05 ]
X unfilterd :  [0.189 0.051 0.994 0.112]
-------------------------------------------------------
X simulated :  [0.2   0.01  0.995 0.1  ]
X estimated :  [0.196 0.007 0.995 0.098]
X unfilterd :  [0.381 0.083 0.985 0.173]
-------------------------------------------------------
X simulated :  [0.299 0.022 0.989 0.149]
X estimated :  [0.299 0.014 0.989 0.15 ]
X unfilterd :  [0.487 0.162 0.952 0.308]
-------------------------------------------------------
X simulated :  [0.397 0.04  0.98  0.199]
X estimated :  [0.395 0.031 0.98  0.2  ]
X unfilterd :  [0.591 0.21  0.929 0.369]
-------------------------------------------------------
X simulated :  [0.495 0.062 0.969 0.247]
X estimated :  [0.491 0.053 0.968 0.25 ]
X unfilterd :  [0.655 0.327 0.905 0.426]
-------------------------------------------------------
X simulated :  [0.591 0.089 0.955 0.296]
X estimated :  [0.589 0.082 0.955 0.296]
X unfilterd :  [0.771 0.415 0.85  0.526]
-------------------------------------------------------
X simulated :  [0.686 0.121 0.939 0.343]
X estimated :  [0.683 0.114 0.939 0.343]
X unfilterd :  [0.87  0.516 0.786 0.619]
-------------------------------------------------------
X simulated :  [0.779 0.158 0.921 0.389]
X estimated :  [0.775 0.15  0.921 0.389]
X unfilterd :  [0.983 0.665 0.715 0.699]
-------------------------------------------------------
X simulated :  [0.87  0.199 0.9   0.435]
X estimated :  [0.869 0.19  0.9   0.435]
X unfilterd :  [1.033 0.755 0.646 0.763]
-------------------------------------------------------
X simulated :  [0.959 0.245 0.878 0.479]
X estimated :  [0.959 0.237 0.877 0.48 ]
X unfilterd :  [1.117 0.938 0.544 0.839]
-------------------------------------------------------

[mnissov]

Maybe it's just a misunderstanding from my side then, I figured applying the Log map would retract what is essentially just a vector of [x, y, theta]. The R^3 components one applies the generators on to form the component in the algebra.

[joansola]

The Log_SE3 is not the same as the Log_SO3 and Log_R3. This is explained in Example 7 in the paper.

The log_SE3 is such that is the inverse of exp_SE3. And exp_SE3 takes a point in the Lie algebra, say (dx,dy,dz, droll,dpitch,dyaw)=(1,0,0, 0,0,1), with displacement on the X-axis, and "rolls it over" the manifold. In my example, this is a curve of length 1 and arc PI, and so you end up at (x,y,x,roll,pitch,yaw) = (sqrt3/2, 1/2, 0, 0,0,PI), that is, out of the X-axis. Remember that the Lie vector is only tangent to the group at one point. After that point, it departs from the Lie algebra. So the trajectory departs from the X/axis. But if you take the Log_SE3 then, you return to the X axis!

Maybe you want to display SE3 elements in vector form with M=(T,R) --> (T, Log_SO3(R)) instead of Log_SE3(M)

The results in the example log are of the same type. The

Missatge de Morten @.***> del dia dj., 26 de gen. 2023 a les 9:21:

Maybe it's just a misunderstanding from my side then, I figured applying the Log map would retract what is essentially just a vector of [x, y, theta]. The R^3 components one applies the generators on to form the component in the algebra.

— Reply to this email directly, view it on GitHub https://github.com/artivis/manif/issues/262#issuecomment-1404680003, or unsubscribe https://github.com/notifications/unsubscribe-auth/AAS2LPNQEANDIUGNXTW2LJ3WUIXZDANCNFSM6AAAAAAUGNIOPU . You are receiving this because you are subscribed to this thread.Message ID: @.***>