Adding lever-arm to GPS factors #1975

mnissov · 2025-01-13T16:01:43Z

I noticed the GPS factors had no option for a non-zero lever arm, the feature became necessary for me so I figured I would make this PR.

Of course similar behavior is possible with setBodyPSensor but this comes at the cost of additional linearization error I believe (ref). As a result the performance is actually different.

mnissov · 2025-01-14T11:13:50Z

I see the macos-14-xcode-15.4 Release Python 3 check failed. This I have very little experience with, but in hindsight I see the serialization and interfacing were not appropriately updated.

Perhaps I should ask the following questions

Is this of interest for GTSAM?
Would you prefer the extrinsic added as an estimated state?
Do you have any tips for resolving the aforementioned failed check, or how I can test this locally. Because running make check succeeds across the board.

dellaert · 2025-01-14T19:51:06Z

I’ll take a look and then answer your questions. If it can be done easily with another state then probably that is preferred.

dellaert

I think this is great, and I think it should be added to a factor. But I propose to add a GpsFactor2 or GpsFactorArm in the same file, because the Jacobians are more expensive. Adding documentation of the nature I suggested in both the old and the new factor would make it easier to see the difference.

PS we will soon start adding notebooks to document factors in the source tree itself. Consider authoring a small colab that demonstrates both factors that we could include :-) That’s not needed for this PR to merge, though.

dellaert · 2025-01-14T19:54:32Z

gtsam/navigation/GPSFactor.cpp

 }

 //***************************************************************************
 Vector GPSFactor2::evaluateError(const NavState& p,
    OptionalMatrixType H) const {
-  return p.position(H) -nT_;
+  const Matrix3 rot = p.attitude().matrix();


Please use aRb naming convention, where R transforms b coordinates into a coordinates.

dellaert · 2025-01-14T19:57:02Z

gtsam/navigation/GPSFactor.h

@@ -39,6 +39,7 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
  typedef NoiseModelFactorN<Pose3> Base;

  Point3 nT_; ///< Position measurement in cartesian coordinates
+  Point3 B_t_BG_; ///< Lever arm between GPS and BODY frame


The naming convention ‘nT_’ says that T is in the navigation frame. I think if you want to include a lever arm in this factor you need to include more documentation about the frames involved.

dellaert · 2025-01-14T19:58:51Z

gtsam/navigation/tests/testGPSFactor.cpp

@@ -61,10 +64,12 @@ TEST( GPSFactor, Constructor ) {
  // Factor
  Key key(1);
  SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
-  GPSFactor factor(key, Point3(E, N, U), model);
+  GPSFactor factor(key, Point3(E, N, U), model, leverArm);


I would not expect you to change an existing test, but rather add an additional test with a non-zero lever arm.

gtsam/navigation/GPSFactor.cpp

dellaert · 2025-01-14T20:09:12Z

gtsam/navigation/GPSFactor.cpp

+    H->block<3, 3>(0, 6).setZero();
+  }
+
+  return p.position() - (nT_ - rot * B_t_BG_);


This formula is a bit counter-intuitive to me. I would expect to see $h(x)-z$, where $h(x)$ is the measurement prediction and $z$ the measurement, in this case nT_. If I rewrite your equation that way I get

h(x) = p.position() + rot * B_t_BG_

From which I deduce that rot==nRb which matches my definition of NavState, and B_t_BG_==bL_, a lever arm in the body frame. So, I think the documentation/notation I’m looking for is

bL_ is a lever arm in the body frame, denoting the 3D position of the GPS antenna in the body frame. Because the actual location of the antenna depends on both position and attitude, providing a lever arm makes components of the attitude observable in turns.

Lever arm is definitely in body frame, will adapt this to GTSAM format.

mnissov · 2025-01-15T12:34:16Z

I think I have addressed all your points in the review with new factors GPSFactorArm and GPSFactor2Arm mirroring their namesakes but with an argument for lever arm.

I had some follow-up questions

Do you still want the lever-arm coded in as a state instead of as a constant? It would be pretty simple to do so from my side.
Regarding notebooks, is there an example of one I can look at? I assume it wouldn't be so hard for me to make one for this.
I've left out EstimateState when making GPSFactorArm as there is now some inconsistency about the nTb actually being nTg in a way. Of course this is correctable for the pose, less so for the velocity (missing angular rate I think).

dellaert

This looks great ! You could probably save a bit of copy/pasta by inheriting from the non-arm factors, but that's entirely optional.

gtsam/navigation/GPSFactor.h

gtsam/navigation/GPSFactor.cpp

dellaert · 2025-01-16T15:40:28Z

I had some follow-up questions

Do you still want the lever-arm coded in as a state instead of as a constant? It would be pretty simple to do so from my side.

I would say that is a different factor, one that would incur a lot of computation, actually: this new state variable would connect all GPS factors. It would be useful in a calibration setting, but not for everyday use, if you already have a calibrated lever arm.

Regarding notebooks, is there an example of one I can look at? I assume it wouldn't be so hard for me to make one for this.

Sure: https://www.roboticsbook.org/S64_driving_perception.html, open in colab :-)

I've left out EstimateState when making GPSFactorArm as there is now some inconsistency about the nTb actually being nTg in a way. Of course this is correctable for the pose, less so for the velocity (missing angular rate I think).

I don't quite understand this :-)

mnissov · 2025-01-16T17:24:07Z

This looks great ! You could probably save a bit of copy/pasta by inheriting from the non-arm factors, but that's entirely optional.

Thanks! I had considered inheritance, but I had some trouble with base classes and indirect bases. Plus I think inheritance only saves copy-pasting clone and measurementIn, the rest have, albeit, minor changes. So I didn't pursue this much further.

I don't quite understand this :-)

I'll try to explain it differently, suppose aPab is the position of {b} with respect to {a}, expressed in coordinate frame {a} (for coordinate frames {a} and {b}). If there is a lever arm, the measurement is really wPwg instead of wPwb for world frame {w}, GPS antenna {g}, and body frame {b}.

Therefore EstimateState, if there is a nonzero lever arm, calculates not velocity wVwb but instead wVwg. So if the lever arm is large, while rotating there can be a significant difference. In addition, rotating about {b} means that the position of {g} will change, but not the position of {b}. The position difference can be corrected I think, but the velocity not without knowing the angular rate as well.

This is what I was trying to say. I hope this makes a little more sense.

dellaert · 2025-01-16T19:43:37Z

This looks great ! You could probably save a bit of copy/pasta by inheriting from the non-arm factors, but that's entirely optional.

Thanks! I had considered inheritance, but I had some trouble with base classes and indirect bases. Plus I think inheritance only saves copy-pasting clone and measurementIn, the rest have, albeit, minor changes. So I didn't pursue this much further.

Agreed !

I don't quite understand this :-)
This is what I was trying to say. I hope this makes a little more sense.

Yep, I think so :-)

So, this is ready to merge after CI succeeds, yes? I propose we do a notebook in a follow-up PR, if you're game. In fact, we're soon going to add a mechanism to GTSAM to check these in and they will be automatically served. And, we can featsure them as blog posts on gtsam.org, as well.

mnissov · 2025-01-16T20:30:07Z

Sorry about that, I was a bit slow but I added a factor with the lever arm as a state as well. Ended up calling it GPSFactorArmCalib, which is a bit verbose but I'm struggling to find something else :).

So, this is ready to merge after CI succeeds, yes?

Otherwise yes, should be all good from my side.

I propose we do a notebook in a follow-up PR, if you're game. In fact, we're soon going to add a mechanism to GTSAM to check these in and they will be automatically served. And, we can featsure them as blog posts on gtsam.org, as well.

Yeah I am down for that. The model for this is a numerical example and a bit of description/story-telling?

dellaert · 2025-01-17T13:15:24Z

Thanks @mnissov !!! Will be quite useful, makes orientation observable if you have two receivers, as well :-)

mnissov added 3 commits January 13, 2025 16:55

adding lever arm optional argument to gps factors

5254b4f

fix GPSFactor serialization

57b8f6b

update GPSFactor interface

7efcb46

dellaert reviewed Jan 14, 2025

View reviewed changes

mnissov added 2 commits January 15, 2025 11:54

reverting changes to existing factors

af1e6e3

creating GPSFactorArm and GPSFactor2Arm w/ lever arm arguments

b815936

remove EstimateState from GPSFactorArm

94e96ab

dellaert approved these changes Jan 16, 2025

View reviewed changes

mnissov added 2 commits January 16, 2025 17:57

improving comments

4737f0d

improved documentation, params and getters

b5349a7

mnissov added 2 commits January 16, 2025 21:26

typo in print for GPSFactor2Arm

0c13038

adding GPSFactors with lever arm as state

d1106ae

dellaert merged commit 717e3be into borglab:develop Jan 17, 2025
33 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Adding lever-arm to GPS factors #1975

Adding lever-arm to GPS factors #1975

mnissov commented Jan 13, 2025

mnissov commented Jan 14, 2025 •

edited

Loading

dellaert commented Jan 14, 2025

dellaert left a comment •

edited

Loading

dellaert Jan 14, 2025

dellaert Jan 14, 2025

dellaert Jan 14, 2025

dellaert Jan 14, 2025

mnissov Jan 15, 2025

mnissov commented Jan 15, 2025

dellaert left a comment

dellaert commented Jan 16, 2025

mnissov commented Jan 16, 2025 •

edited

Loading

dellaert commented Jan 16, 2025

mnissov commented Jan 16, 2025

dellaert commented Jan 17, 2025

Adding lever-arm to GPS factors #1975

Adding lever-arm to GPS factors #1975

Conversation

mnissov commented Jan 13, 2025

mnissov commented Jan 14, 2025 • edited Loading

dellaert commented Jan 14, 2025

dellaert left a comment • edited Loading

Choose a reason for hiding this comment

dellaert Jan 14, 2025

Choose a reason for hiding this comment

dellaert Jan 14, 2025

Choose a reason for hiding this comment

dellaert Jan 14, 2025

Choose a reason for hiding this comment

dellaert Jan 14, 2025

Choose a reason for hiding this comment

mnissov Jan 15, 2025

Choose a reason for hiding this comment

mnissov commented Jan 15, 2025

dellaert left a comment

Choose a reason for hiding this comment

dellaert commented Jan 16, 2025

mnissov commented Jan 16, 2025 • edited Loading

dellaert commented Jan 16, 2025

mnissov commented Jan 16, 2025

dellaert commented Jan 17, 2025

mnissov commented Jan 14, 2025 •

edited

Loading

dellaert left a comment •

edited

Loading

mnissov commented Jan 16, 2025 •

edited

Loading