Unit tests for Jacobian of PartialPriorFactor<Pose3> #721

miloknowles · 2021-03-20T21:57:46Z

Adds unit tests for PartialPriorFactor<Pose3> to show that the Jacobians for this factor are incorrect. Specifically, the factor precomputes a Jacobian that is correct for an identity pose, but doesn't take into account the rotation/translation at the current linearization point. See /gtsam_unstable/slam/tests/testPartialPriorFactor.cpp for the relevant tests (the identity case succeeds while the others fail). This is addressing the post here on the Google group.

Haven't contributed to GTSAM before so apologies if I'm missing a part of the workflow. I thought I'd open the issue now, and start working on a fix in the meantime.

dellaert · 2021-03-21T15:53:24Z

Nice. I would put the fix in this same PR, as we can’t merge failing tests :-) do you know how to fix it?

miloknowles · 2021-03-21T19:50:54Z

I think so - the evaluateError function already calls LieGroup::logmapto get the tangent vector, so getting the Jacobian from logmap and then selecting the relevant row(s) seems like an easy solution.

Unrelated, but I'd also like to rename the "mask" argument in the factor to something else, since it's a list of tangent vector indices rather than a binary mask (I found this slightly confusing at first).

miloknowles · 2021-03-21T20:24:44Z

One question: should I prefer localCoordinates or logmap?

…mpatibility

dellaert · 2021-03-21T21:48:48Z

I think so - the evaluateError function already calls LieGroup::logmapto get the tangent vector, so getting the Jacobian from logmap and then selecting the relevant row(s) seems like an easy solution.

Unrelated, but I'd also like to rename the "mask" argument in the factor to something else, since it's a list of tangent vector indices rather than a binary mask (I found this slightly confusing at first).

Ah, unrelated PRs are unrelated :-) Let's keep this one easy to review? If you've already don it no issue.

localcoordinates is indeed preferred as that will allow any manifold type as well.

miloknowles · 2021-03-22T01:27:56Z

Already made the change, apologies - I'll keep my PRs minimal in the future though! :)

Switched to localCoordinates, although I only get the correct Jacobians when I use p.localCoordinates(T::identity()) instead of the other way around, for reasons I don't quite understand...

Based on PoseRotationPrior and PoseTranslationPrior, it seems like the correct way to compute error is prior_.localCoordinates(p). However, since we only store part of the prior tangent vector in this factor, we would have to assume values for the other parameters to follow this pattern. I'm not sure how to do that except for by assuming the identity, in which case we may as well use Logmap or LocalCoordinates. Let me know what you think would be best.

dellaert · 2021-03-25T12:56:20Z

Interesting. I propose you try something that you think is right, but do it in a very test-driven way. Definitely test away from zero, and for non-trivial manifolds, e.g. Pose3.

CI seems to be failing now, BTW

gtsam_unstable/slam/tests/testPartialPriorFactor.cpp

miloknowles · 2021-03-26T14:53:33Z

@dellaert I decided to use T::Logmap since I think most GTSAM users will be using this factor with Lie groups anyways. I've added unit tests for PartialPriorFactor<Pose2/Pose3> at a variety of different linearization points and am pretty confident the Jacobians are correct now. Interestingly, it looks like PR #724 is fixing the exact same Jacobian problem in the UnaryFactor tutorial. Will deal with the CI failures now.

dellaert · 2021-03-26T15:06:52Z

@dellaert I decided to use T::Logmap since I think most GTSAM users will be using this factor with Lie groups anyways. I've added unit tests for PartialPriorFactor<Pose2/Pose3> at a variety of different linearization points and am pretty confident the Jacobians are correct now. Interestingly, it looks like PR #724 is fixing the exact same Jacobian problem in the UnaryFactor tutorial. Will deal with the CI failures now.

Hmmm. What's the problem using T::LocalCoordinates? It seems Logmap is artificially constraining the usefulness. Maybe I don't see the problem.

miloknowles · 2021-03-26T15:21:27Z

@dellaert I decided to use T::Logmap since I think most GTSAM users will be using this factor with Lie groups anyways. I've added unit tests for PartialPriorFactor<Pose2/Pose3> at a variety of different linearization points and am pretty confident the Jacobians are correct now. Interestingly, it looks like PR #724 is fixing the exact same Jacobian problem in the UnaryFactor tutorial. Will deal with the CI failures now.

Hmmm. What's the problem using T::LocalCoordinates? It seems Logmap is artificially constraining the usefulness. Maybe I don't see the problem.

I forgot to mention - PoseRTV does not implement LocalCoordinates, only Logmap, which is why I had ended up using Logmap. There are some some partial prior factors in DynamicsPriors.h that are templated on the PoseRTV type.

Would you like me to add LocalCoordinates in PoseRTV or stick with Logmap?

dellaert · 2021-03-26T15:26:10Z

@dellaert I decided to use T::Logmap since I think most GTSAM users will be using this factor with Lie groups anyways. I've added unit tests for PartialPriorFactor<Pose2/Pose3> at a variety of different linearization points and am pretty confident the Jacobians are correct now. Interestingly, it looks like PR #724 is fixing the exact same Jacobian problem in the UnaryFactor tutorial. Will deal with the CI failures now.

Hmmm. What's the problem using T::LocalCoordinates? It seems Logmap is artificially constraining the usefulness. Maybe I don't see the problem.

I forgot to mention - PoseRTV does not implement LocalCoordinates, only Logmap, which is why I had ended up using Logmap. There are some some partial prior factors in DynamicsPriors.h that are templated on the PoseRTV type.

So, PoseRTV is superseded by NavState, but that's a different PR :-)

But the problem with LocalCoordinates might be easily solved. It might simply be a missing entry at line 78 in PoseRTV.h ?

  /// @name Manifold
  /// @{
  using Base::dimension;
  using Base::dim;
  using Base::Dim;
  using Base::retract;
  using Base::localCoordinates;
  /// @}

miloknowles · 2021-03-26T15:36:36Z

@dellaert I decided to use T::Logmap since I think most GTSAM users will be using this factor with Lie groups anyways. I've added unit tests for PartialPriorFactor<Pose2/Pose3> at a variety of different linearization points and am pretty confident the Jacobians are correct now. Interestingly, it looks like PR #724 is fixing the exact same Jacobian problem in the UnaryFactor tutorial. Will deal with the CI failures now.

Hmmm. What's the problem using T::LocalCoordinates? It seems Logmap is artificially constraining the usefulness. Maybe I don't see the problem.

I forgot to mention - PoseRTV does not implement LocalCoordinates, only Logmap, which is why I had ended up using Logmap. There are some some partial prior factors in DynamicsPriors.h that are templated on the PoseRTV type.

So, PoseRTV is superseded by NavState, but that's a different PR :-)

But the problem with LocalCoordinates might be easily solved. It might simply be a missing entry at line 78 in PoseRTV.h ?
  /// @name Manifold
  /// @{
  using Base::dimension;
  using Base::dim;
  using Base::Dim;
  using Base::retract;
  using Base::localCoordinates;
  /// @}

I think because ProductLieGroup doesn't implement LocalCoordinates (just Logmap), we can't use the simple fix :(
Maybe that's a different PR too?

dellaert · 2021-03-26T16:11:21Z

I think because ProductLieGroup doesn't implement LocalCoordinates (just Logmap), we can't use the simple fix :(
Maybe that's a different PR too?

No, I think that's an oversight and we could put that in this PR. OR switch to a different type for testing this PR. Adding PoseRTV code is probably a mistake, so maybe just switch to NavState which should have all the manifold goodness.

…en/Core> include to reduce compile memory

miloknowles · 2021-03-26T18:44:56Z

I think because ProductLieGroup doesn't implement LocalCoordinates (just Logmap), we can't use the simple fix :(
Maybe that's a different PR too?

No, I think that's an oversight and we could put that in this PR. OR switch to a different type for testing this PR. Adding PoseRTV code is probably a mistake, so maybe just switch to NavState which should have all the manifold goodness.

Sorry for all the back and forth - I opted to just add LocalCoordinates to ProductLieGroup so that we get it in PoseRTV. Unfortunately NavState doesn't have LocalCoordinates either, unless I'm missing some kind of manifold inheritance. Also, replacing PoseRTV with NavState would require a lot of changes in DynamicsPriors and dynamics/tests/testIMUSystem.cpp, and I didn't want to make the review any more involved.

dellaert

Looking good! A few more comments re modern Jacobian style. Re-request review if CI succeeds and I'll approve (or Brice can).

gtsam/base/ProductLieGroup.h

gtsam_unstable/slam/PartialPriorFactor.h

dellaert · 2021-03-29T02:00:52Z

gtsam_unstable/slam/PartialPriorFactor.h

+
+      // If the Rot3 Cayley map is used, Rot3::LocalCoordinates will throw a runtime error
+      // when asked to compute the Jacobian matrix (see Rot3M.cpp).
+      #ifdef GTSAM_ROT3_EXPMAP


Sure, but why would we do this for every possible T?
But I'll let this slip, as (a) the fix is not straightforward, (b) this is not an often used factor, and (c) GTSAM_ROT3_EXPMAP is the default...

Sorry, I'm not sure I follow. Do you mean we just shouldn't use this factor if GTSAM_ROT3_EXPMAP is turned off?

dellaert · 2021-03-29T02:01:18Z

Thanks @miloknowles !!!!

Milo Knowles added 2 commits March 20, 2021 17:44

Three examples of failing PartialPriorFactor<Pose3> Jacobians

075293c

Add successful unit test for identity pose

e6b7d9f

miloknowles changed the title ~~Three examples of failing PartialPriorFactor<Pose3> Jacobians~~ Unit tests for Jacobian of PartialPriorFactor<Pose3> Mar 20, 2021

Milo Knowles added 2 commits March 21, 2021 17:10

Correct Jacobian in PartialPriorFactor, modify derived factors for co…

593e6e9

…mpatibility

Change mask to indices and update factor docstring

43c9f2b

Prefer localCoordinates over logmap

4b29c03

bricerebsamen reviewed Mar 26, 2021

View reviewed changes

gtsam_unstable/slam/tests/testPartialPriorFactor.cpp Outdated Show resolved Hide resolved

gtsam_unstable/slam/tests/testPartialPriorFactor.cpp Outdated Show resolved Hide resolved

Milo Knowles added 3 commits March 26, 2021 08:59

Fix x/y mismatch in unit tests

ef2cd5d

Add constructor tests and extend tests to Pose2

74b92ef

Switch to cleaner T::Logmap

05fad78

Fix incorrect Vector dimension that was causing CI failures

909b550

Add LocalCoordinates() to ProductLieGroup and remove unnecessary <Eig…

a0ff5e3

…en/Core> include to reduce compile memory

dellaert reviewed Mar 26, 2021

View reviewed changes

gtsam/base/ProductLieGroup.h Show resolved Hide resolved

gtsam_unstable/slam/PartialPriorFactor.h Outdated Show resolved Hide resolved

gtsam_unstable/slam/PartialPriorFactor.h Outdated Show resolved Hide resolved

Fix Rot3::LocalCoordinates runtime error when using Cayley map

1f12f82

miloknowles requested a review from dellaert March 27, 2021 02:17

dellaert reviewed Mar 29, 2021

View reviewed changes

dellaert approved these changes Mar 29, 2021

View reviewed changes

dellaert merged commit 8ffad01 into borglab:develop Mar 29, 2021

miloknowles deleted the milo/partial_prior_factor branch March 29, 2021 13:13

miloknowles mentioned this pull request Apr 23, 2021

Fix PartialPriorFactor (again) #755

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Unit tests for Jacobian of PartialPriorFactor<Pose3> #721

Unit tests for Jacobian of PartialPriorFactor<Pose3> #721

miloknowles commented Mar 20, 2021

dellaert commented Mar 21, 2021

miloknowles commented Mar 21, 2021

miloknowles commented Mar 21, 2021 •

edited

dellaert commented Mar 21, 2021

miloknowles commented Mar 22, 2021

dellaert commented Mar 25, 2021

miloknowles commented Mar 26, 2021

dellaert commented Mar 26, 2021

miloknowles commented Mar 26, 2021 •

edited

dellaert commented Mar 26, 2021

miloknowles commented Mar 26, 2021

dellaert commented Mar 26, 2021

miloknowles commented Mar 26, 2021

dellaert left a comment

dellaert Mar 29, 2021

miloknowles Mar 29, 2021

dellaert commented Mar 29, 2021

Unit tests for Jacobian of PartialPriorFactor<Pose3> #721

Unit tests for Jacobian of PartialPriorFactor<Pose3> #721

Conversation

miloknowles commented Mar 20, 2021

dellaert commented Mar 21, 2021

miloknowles commented Mar 21, 2021

miloknowles commented Mar 21, 2021 • edited

dellaert commented Mar 21, 2021

miloknowles commented Mar 22, 2021

dellaert commented Mar 25, 2021

miloknowles commented Mar 26, 2021

dellaert commented Mar 26, 2021

miloknowles commented Mar 26, 2021 • edited

dellaert commented Mar 26, 2021

miloknowles commented Mar 26, 2021

dellaert commented Mar 26, 2021

miloknowles commented Mar 26, 2021

dellaert left a comment

Choose a reason for hiding this comment

dellaert Mar 29, 2021

Choose a reason for hiding this comment

miloknowles Mar 29, 2021

Choose a reason for hiding this comment

dellaert commented Mar 29, 2021

miloknowles commented Mar 21, 2021 •

edited

miloknowles commented Mar 26, 2021 •

edited