Fix OrientedPlane3Factor jacobian #362
Conversation
There was a problem hiding this comment.
Awesome. I will think about the analytical error. Maybe @akshay-krishnan would not mind thinking about it, either: he just went deep into this for lines - I'm a bit rusty.
In the meantime I do have a number of style comments (and performance!) that will need to be addressed in the PR before we could merge. Mainly: move as much as possible in cpp files, and move the responsibility of derivatives from the factor to the OrientedPane itself. Finally, use fixed-size matrices allocated on the stack as malloc is evil :-)
| @@ -114,6 +114,10 @@ class GTSAM_EXPORT OrientedPlane3 { | |||
| */ | |||
| Vector3 error(const OrientedPlane3& plane) const; | |||
|
|
|||
| static Vector3 Error(const OrientedPlane3& plane1, const OrientedPlane3& plane2) { | |||
There was a problem hiding this comment.
I don't think we should add this. Instead, add 2 OptionalJacobian arguments to method above (H1 for this, H2 for plane)
gtsam/slam/OrientedPlane3Factor.h
Outdated
| OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1_1, H2_1); | ||
| err << predicted_plane.error(measured_p_); | ||
| // Numerically calculate the derivative since this function doesn't provide one. | ||
| auto f = boost::bind(&OrientedPlane3::Error, _1, _2); |
There was a problem hiding this comment.
move this logic into OrientedPlane.cpp, to calculate the optional derivatives of the error method, then call
Matrix33 error_H_predicted, error_H_measured;
err << predicted.error(measured_p_, error_H_predicted, error_H_measured);
|
Yes, I can help with the analytical derivative. Looks like we will need to define the derivatives for Unit3’s localCoordinates, since OrientedPlane3’s error function makes use of it. About the error method in OrientedPlane3, I’m not sure what the norm 1 error is, but if this is the L1 norm, should it not be the absolute value? Are we missing an std::abs here? |
|
Thanks for all the detailed feedback @dellaert! I've incorporated almost all the changes you mentioned into the latest commit. Once we fix the derivatives we can remove @akshay-krishnan - Thanks for your help :) The call to Also, I'm not sure about the comment about the L1 norm.. |
|
I updated the style (as suggested by Frank) and changed from E.g. Here is the error and derivatives for And here is the error and derivatives for I'm not sure where to go from here - @akshay-krishnan any ideas? |
|
The error and errorVector functions do not do the same thing. One is the negative of the other. I believe this was designed to be this way. If you look at the source, errorVector computes (this - other) where as error computes (other - this) in tangential space (like localCoordinates). I am still confused about the fact that neither of them have an absolute value. |
|
So, any factor calculates a vector of doubles, which GTSAM then takes the norm of to calculate the error. So, the sign of the individual error entries does not matter. |
|
My two cents: negative numbers are perfectly OK in an error vector. But I think both formulas, the one used to error evaluation and the one used to derive the Jacobians, should be the same (with same sign: both "a-b", not "a-b" vs "b-a"), or the nonlinear optimizer may be misled... |
|
Hmmm. The way you used errorVector seems correct. Can you confirm that (a) the errorVector derivative unit test succeeds, and (b) print out the output of the failing evaluateError test? |
I think David is doing this. Is there a specific line you think is wrong? |
@dellaert : Honestly I didn't reviewed the code, I was just answering to:
which seemed suspicious at first sight :-) |
|
So, what's the status on this now? @dwisth @akshay-krishnan |
Sorry, I have not found time to work on the Jacobians for Unit3::error. Could @dwisth confirm if the errorVector unit tests worked? |
|
If you want this in 4.0.3, last checkpoint before 4.1, we need to clean up this PR now. It seems that switching to the other error made things work @dwisth ? |
|
Yes, seemed that switching error terms made it work. I'm going to take another look at this today / tomorrow and try to finish it off. |
…r Unit3::localCoordinates.
|
Thanks for your latest commit @dwisth . But now that you switched error functions, do we still need the analytical Jacobian for localCoordinates? If not I'd rather remove the numerical versions and their unit tests in this PR. |
|
To summarise the issue:
I'm a bit stuck on this last point and if anyone can help or give me some pointers it would be greatly appreciated. |
Awesome. But, again: now that you switched error functions, do we still need the analytical Jacobian for localCoordinates? If not I'd rather remove the numerical versions and their unit tests in this PR. |
Yes we do. In the end I found the the original error function worked best ( |
|
So here are my thoughts from looking into this issue:
|
|
I believe if we can figure out Also, |
|
This was never merged. I propose I merge it into the other branch and do all plane-related stuff at once. |
|
@dellaert be sure to keep track of my comments. We need the Unit3 jacobians implemented. :) |
Maybe. But I'll keep your comments in mind. |
See #283.
This PR fixes the derivative of the
OrientedPlane3Factor. Previously, the calculated Jacobians didn't account for the second function inevaluateErrorwhich meant that the Jacobian was not correct.I have added a
numericalDerivativeto calculate the Jacobian of the second calculation and then applied the chain rule. I have also added a unit test to check that this is correct.TODO: Ideally
OrientedPlane3::error()should calculate analytical derivatives itself instead of usingnumericalDerivatives. Would anyone be able to help with this?@mcamurri
This change is