Jacobian Calculation Help

[CCInc]

Hi, I'm trying to understand how the jacobians derived from symforce relate to GTSAM and other literature.

For example, for a translational residual residual = T.translation - p, then the jacobian should be defined as [0, T.R], and indeed that's what we see:

>>> T = sf.unsupported.Pose3_SE3(sf.Rot3.symbolic("R"), sf.V3.symbolic("t"))
>>> p = sf.V3.symbolic("p")
>>> (T.t - p).jacobian(T)
[0, 0, 0, 1 - 2*R_y**2 - 2*R_z**2, -2*R_w*R_z + 2*R_x*R_y, 2*R_w*R_y + 2*R_x*R_z]
[0, 0, 0, 2*R_w*R_z + 2*R_x*R_y, 1 - 2*R_x**2 - 2*R_z**2, -2*R_x*R_w + 2*R_z*R_y]
[0, 0, 0, -2*R_w*R_y + 2*R_x*R_z, 2*R_x*R_w + 2*R_z*R_y, 1 - 2*R_x**2 - 2*R_y**2]

>>> (T.t - p).jacobian(T)[:,3:] == T.R.to_rotation_matrix()
True

But once we add the rotational component, I get confused - for a rotation residual = T * p - p_0 I would expect the jacobian w.r.t T to be [R * skew(-p), R]. But I cannot seem to replicate that result in symforce:

>>> p0 = sf.V3.symbolic("p0")
>>> auto_jacob = (T * p - p0).jacobian(T)
>>> auto_jacob[:,3:] == T.R.to_rotation_matrix()
>>> auto_jacob[:,:3] == T.R.to_rotation_matrix() * T.R.hat(-p)
False
>>> (auto_jacob[:,:3] - T.R.to_rotation_matrix() * T.R.hat(-p)).simplify()
symbolic.simplify():495 WARNING -- Converting to sympy to use .simplify
[0, (-1 + R_w**2 + R_x**2 + R_y**2 + R_z**2)*p2, (1 - R_w**2 - R_x**2 - R_y**2 - R_z**2)*p1]
[(1 - R_w**2 - R_x**2 - R_y**2 - R_z**2)*p2, 0, (-1 + R_w**2 + R_x**2 + R_y**2 + R_z**2)*p0]
[(-1 + R_w**2 + R_x**2 + R_y**2 + R_z**2)*p1, (1 - R_w**2 - R_x**2 - R_y**2 - R_z**2)*p0, 0]

I seem to always end up with extra terms. And I see similar behavior when computing the jacobian on Rot3 directly. Sorry for the simple question, if someone can help clear up my understanding I'd appreciate it!

[aaron-skydio]

One of the things that SymPy's simplify doesn't know about is the unit quaternion constraint - the last matrix there has a lot of terms like -1 + R_w**2 + R_x**2 + R_y**2 + R_z**2, which is 0 given that R is a unit quaternion, if you apply that constraint that matrix simplifies down to the zero matrix. In general,

1. this sort of thing is often less noticeable/important in larger expressions, and
2. for targeted cases like this, you can apply targeted simplifications by doing e.g. .subs(T.R.q.squared_norm(), 1) in the right sequence with simplifications - see for example https://github.com/symforce-org/symforce/blob/main/notebooks/tangent_D_storage.ipynb