Xueshen Liu (Xenshinu)
unread,Jun 23, 2024, 11:32:45 AMJun 23Sign in to reply to author
Sign in to forward
You do not have permission to delete messages in this group
Either email addresses are anonymous for this group or you need the view member email addresses permission to view the original message
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.