symforce-org / symforce

Fast symbolic computation, code generation, and nonlinear optimization for robotics
https://symforce.org
Apache License 2.0
1.44k stars 147 forks source link

Computing a matrix determinant and gradient #306

Closed Jaeyoung-Lim closed 1 year ago

Jaeyoung-Lim commented 1 year ago

Is your feature request related to a problem? Please describe. It seems like it is not straightforward to compute a determinant of a matrix, or it is not exposed as part of symforce.

It seems like the determinant computation should be available in sympy, and I can compute it by accessing the matrix.

Computing a determinant can be useful for handling information matrices.

For example, I can compute the

import symforce.symbolic as sf

landmark = sf.V3.symbolic("l")
position = sf.V3.symbolic("p")

bearing = (landmark - position) / (landmark - position).norm()

# Jacobian to landmarks
J_theta = bearing.jacobian(landmark)

# Information Matrix
I_theta = J_theta * J_theta.transpose()
D_optimality = I_theta.mat.det()

However, when I try to differentiate this with respect to position,

D_optimality.jacobian(position)

I get the following error

--------------------------------------------
Traceback (most recent call last):
  File "test.py", line 24, in <module>
    policy = D_optimality.jacobian(position)
AttributeError: 'Add' object has no attribute 'jacobian'

Would fixing this just be a matter of exposing the sympy API to symforce? Or am I missing something?

Describe the solution you'd like Be able to compute the determinant of a matrix similar to the norm() operation for vectors.

Additional context

Jaeyoung-Lim commented 1 year ago

Small update - a workaround is to make the determinant a 1X1 matrix,

D_optimality = Matrix([I_theta.mat.det()])

But the symforce vectors can no longer be passed to the subs but need to provide each key e.g. (l0, l1, l2) to display the value

bresch commented 1 year ago

Maybe cast it in a V1 vector? H = sf.V1(I_theta.mat.det()).jacobian(position)

Jaeyoung-Lim commented 1 year ago

Suggestions from @bresch worked great! Thanks!

aaron-skydio commented 1 year ago

Yeah generally the .jacobian method is only present on matrix (and vector) types and not scalars, although we might add it to scalars for consistency so you can do things like this. Scalars have .diff but that will only differentiate with respect to other scalars

But the symforce vectors can no longer be passed to the subs but need to provide each key e.g. (l0, l1, l2) to display the value

What do you mean here? Do you have an example? This maybe sounds like a bug...

Jaeyoung-Lim commented 1 year ago

@aaron-skydio When I tried to work around the problem by declaring Matrix for the determinant, the symforce vectors were no longer being mapped into the values(e.g. vector l), and I had to manually map individual components into the subs() (e.g. l0, l1,l2`)

However when I use the workaround by @bresch to add it as a sf.V1 vector, then everything worked as expected.

aaron-skydio commented 1 year ago

What was the exact call to subs? E.g. this seems to work as expected for me:

sf.Matrix([D_optimality]).jacobian(position).subs({landmark: sf.V3(1.5, 2.5, 3.5)})