correct coefficients of approximated SE3 Q_r #507
Conversation
|
Thanks! A unit test that first fails, then passes with your fix would be great ;-) |
|
Alright. I will add a unit test soon. |
gtsam/geometry/Pose3.h
Outdated
| @@ -181,6 +181,9 @@ class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> { | |||
| static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none); | |||
| }; | |||
|
|
|||
| static Matrix3 computeQforExpmapDerivative( | |||
There was a problem hiding this comment.
static methods should be Capitalized in GTSAM
gtsam/geometry/Pose3.h
Outdated
| @@ -181,6 +181,9 @@ class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> { | |||
| static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none); | |||
| }; | |||
|
|
|||
| static Matrix3 computeQforExpmapDerivative( | |||
There was a problem hiding this comment.
Please move the Doxygen string from .cpp to .h. Our convention is not to duplicate: if a functions is declared in the header, the docs go there, and not in the .cpp.
gtsam/geometry/tests/testPose3.cpp
Outdated
| w.head<3>() = w.head<3>() * 0.09; | ||
| Matrix3 Qexact = Pose3::computeQforExpmapDerivative(w); | ||
| Matrix3 Qapprox = Pose3::computeQforExpmapDerivative(w, 0.1); | ||
| EXPECT(assert_equal(Qapprox, Qexact, 1e-5)); |
There was a problem hiding this comment.
This function is tested against itself. How do we know the new coefficients are correct, vs the old ones?
There was a problem hiding this comment.
If the original implementation is kept but with a different function signature, then it is straightforward to show that the original implementation fails with this unit test. But I don't know if it is proper to keep the original implementation, say, in the unit test file as it looks messy. Do you have an alternative suggestion?
There was a problem hiding this comment.
My suggestion would be to use numerical derivative (there are many examples on how to d- this with our specialized header).
There was a problem hiding this comment.
All right, Sir. As I understand, Q is one block of the Jacobian of the SE3 Expmap. As you hinted, there are two ways to compute it: the analytical one by the function we are to test, and the numeric one by the powerful numericalDerivative in gtsam. By comparing the two results, we can tell if the function is working properly. If this is what you mean, I will go ahead and write up the unit test?
There was a problem hiding this comment.
Yes, a derivative check for Expmap may already exist, If so it’s just a matter of asserting that a test fails before your change and succeeds after your change. If there is no numerical derivative test yet for expmap then it should be added, and we would welcome that addition to GTSam :-)
There was a problem hiding this comment.
Alright. I will add it soon.
|
Don’t you love unit tests?? :-) Is this now ready for merging in your opinion? |
Absolutely, Sir. Unit tests are my faithful company when I am working alone on a repo. I think the pull request is ready to be merged. |
No description provided.