localCoordinates gives incorret Jacobian matrix?

18 views
Skip to first unread message

Xueshen Liu (Xenshinu)

unread,
Jun 23, 2024, 11:32:45 AMJun 23
to gtsam users
The H matrices calculated from `Pose2::localCoordinates(g, H1, H2)` looks incorrect. The returned value is the same as the `Pose2::between(g, H1, H2)`, but the values of H1 and H2 are different.  

## Steps to reproduce
Here is my code
```
p0 = Pose2(1, 2, 0)
p1 = Pose2(0, 2, np.pi/2)
H1 = np.zeros((3,3), dtype=np.float64, order='F')
H2 = np.zeros((3,3), dtype=np.float64, order='F')
lc = p0.localCoordinates(p1, H1, H2)  # p1 - p0
print("local coordinates: ", lc)
print("H1: \n", H1)
print("H2: \n", H2)
bt = p0.between(p1, H1, H2)
print("between: ", bt)
print("H1: \n", H1)
print("H2: \n", H2)
```
The result is like

```
local coordinates:  [-1.          0.          1.57079633]
H1:
 [[-1.  0.  0.]
 [ 0. -1.  1.]
 [ 0. -0. -1.]]
H2:
 [[ 6.123234e-17 -1.000000e+00  0.000000e+00]
 [ 1.000000e+00  6.123234e-17  0.000000e+00]
 [ 0.000000e+00  0.000000e+00  1.000000e+00]]
between:  (-1, 0, 1.5708)

H1:
 [[-6.123234e-17 -1.000000e+00  1.000000e+00]
 [ 1.000000e+00 -6.123234e-17  6.123234e-17]
 [-0.000000e+00 -0.000000e+00 -1.000000e+00]]
H2:
 [[1. 0. 0.]
 [0. 1. 0.]
 [0. 0. 1.]]
```
Since they both calculate `pose1 @ pose0.inv`, it makes sense the result is the same, but the H matrices calculated by `localCoordinates` looks incorrect. 
Reply all
Reply to author
Forward
0 new messages