Adding lever-arm to GPS factors #1975
Conversation
|
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
|
|
I’ll take a look and then answer your questions. If it can be done easily with another state then probably that is preferred. |
There was a problem hiding this comment.
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.
| } | ||
|
|
||
| //*************************************************************************** | ||
| Vector GPSFactor2::evaluateError(const NavState& p, | ||
| OptionalMatrixType H) const { | ||
| return p.position(H) -nT_; | ||
| const Matrix3 rot = p.attitude().matrix(); |
There was a problem hiding this comment.
Please use aRb naming convention, where R transforms b coordinates into a coordinates.
| @@ -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 | |||
There was a problem hiding this comment.
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.
| @@ -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); | |||
There was a problem hiding this comment.
I would not expect you to change an existing test, but rather add an additional test with a non-zero lever arm.
| } | ||
|
|
||
| //*************************************************************************** | ||
| Vector GPSFactor2::evaluateError(const NavState& p, | ||
| OptionalMatrixType H) const { | ||
| return p.position(H) -nT_; | ||
| const Matrix3 rot = p.attitude().matrix(); | ||
| if (H) { |
There was a problem hiding this comment.
I did not see a numerical derivative test on this Jacobian (right?) so in my book all bets are off :-)
| H->block<3, 3>(0, 6).setZero(); | ||
| } | ||
|
|
||
| return p.position() - (nT_ - rot * B_t_BG_); |
There was a problem hiding this comment.
This formula is a bit counter-intuitive to me. I would expect to see
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.
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
setBodyPSensorbut this comes at the cost of additional linearization error I believe (ref). As a result the performance is actually different.