search

terzakig / sqpnp

C++ Implementation of the SQPnP algorithm
BSD 3-Clause "New" or "Revised" License
150 stars 26 forks source link

Constraints in paper vs. in code #23

Closed AstroTheKat closed 7 months ago

AstroTheKat commented 7 months ago

Hello! Thanks for the great paper and the accompanying code! I'm currently working through the paper and the code side-by-side to understand everything better.

I noticed that the constraint function h(x) in the paper differs a bit from the one used in the code (called g(x) in sqpnp.cpp). While h(x) has det(mat(x)) - 1 as an entry (presumably to enforce special orthogonality), in the code g(x) instead enforces the unit length constraint on the last three elements of x (i.e., the third row of the rotation matrix).

Is there a reason for this difference? I'm a bit confused, considering we want special orthogonality (so I'd expect one would prefer det(mat(x)) - 1) and the comment below from page 6 of the paper:

Note that the unit norm constraint for x_7:9 is redundant and therefore omitted from the components of h.

terzakig commented 7 months ago

Hi there, thank you for your comments and the interest in our work!

Yes, that's a fair question! You are essentially reading between the lines. To begin with, technically, the determinant is a cubic constraint in the elements of the rotation matrix, so it has no place in a quadratically constrained program, but there are ways to work around it, so it's not a major violation of the approach in the paper in practice. So, if we were to be theoretically concerete in our definitions, we should have omitted it anyway.

However, the main reason we skip the constraint from the method that solves the QP in each step of the SQP iteration and use the third row unit-norm instead, is that the Jacobian of the constraints becomes more tractable symbolically and we can actually compute its row and null space analytically and subsequently use the expressions directly in the code. This allows for a very fast solution of the linearized QP without inverting a 15x15 matrix, but rather a 3x3 one (which is done analytically). The other issue you have observed is that we are solving in O(3) instead of SO(3), but that's not a problem, because any PnP solution in O(3) yields a solution in SO(3) (if it's a rotation, then it is accepted; if not, then it will have a -1 determinant, and we obtain a rotation matrix by negating it).

AstroTheKat commented 7 months ago

Ok makes sense, thank you!