Skip to content

navigation: add non-holonomic constraint (NHC) factors - #2933

Open
inuex35 wants to merge 2 commits into
borglab:developfrom
inuex35:feature/nhc-factor-upstream
Open

inuex35 wants to merge 2 commits into
borglab:developfrom
inuex35:feature/nhc-factor-upstream

Conversation

@inuex35

@inuex35 inuex35 commented Nov 12, 2026 •

Copy link
Copy Markdown
Contributor

I would like to add NhcFactor and NhcFactorCalib for non-holonomic robots. They constrain the lateral and vertical velocity of the vehicle reference point to zero in the vehicle frame, with lever-arm compensation.

  • NhcFactor(pose, velocity): the mounting rotation is a fixed constant.
  • NhcFactorCalib(pose, velocity, mountRotation): the mounting rotation is estimated online.

@inuex35
inuex35 force-pushed the feature/nhc-factor-upstream branch 2 times, most recently from 3543818 to 7bc1317 Compare November 13, 2026 09:04
Add NHC factors for cars and other non-holonomic robots. A wheeled vehicle
that does not slip has no lateral or vertical velocity at its reference
point (e.g. the rear-axle center). The factors express that velocity in
the vehicle frame and constrain its lateral and vertical components:

  v_point   = wRb^T * v_world + omega x leverArm
  v_vehicle = bRv^T * v_point
  error     = [v_vehicle.y, v_vehicle.z]

The forward speed is left unconstrained, so the factors combine with any
wheel-odometry or velocity measurement.

Two variants, matching the GPSFactor / GPSFactorArmCalib convention:
  - NhcFactor      (Pose3, Vector3):       the mounting rotation bRv is a
                                           fixed constant (default identity)
  - NhcFactorCalib (Pose3, Vector3, Rot3): bRv is an estimated variable,
                                           for online calibration

Only the pitch and yaw of bRv are observable; rotation about the forward
axis (roll) is never observable from the NHC alone. This is documented and
checked by a test.

Wrapped in navigation.i. C++ tests cover the error definition, analytic
Jacobians, agreement between the fixed and estimated variants, forward-axis
unobservability, lever-arm compensation on a turning ConstantTwistScenario,
serialization round-trips, and optimization of velocity and mounting yaw.
A Python test exercises the wrapped constructors, accessors, and errors.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
@inuex35
inuex35 force-pushed the feature/nhc-factor-upstream branch from 7bc1317 to a9ec10d Compare November 13, 2026 09:35

@ProfFan ProfFan left a comment

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

some minor comments, should be good to go after them :)

using symbol_shorthand::X;

/* ************************************************************************* */
namespace generic_state {

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The tests miss vertical lever-arm compensation, all unit tests passed without it

const gtsam::Vector3& velocity) const;
const gtsam::Vector3& angularVelocity() const;
const gtsam::Vector3& leverArm() const;
const gtsam::Rot3& mountRotation() const;

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

void serialize() const; for both

np.testing.assert_allclose(aligned.evaluateError(self.pose, velocity),
[0.2, -0.3], atol=1e-9)

def test_calib_matches_fixed_factor(self):

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think there is an ambiguity in initialization (0 vs +pi). If so, document that initialization ambiguity and test a mount with both pitch and yaw differences.

- Test vertical lever-arm compensation: a roof-mounted IMU under a roll
  rate is compensated only by the vertical lever-arm component, and
  dropping it leaves a lateral error of -omega_x * height.
- Wrap serialize() for NhcFactor and NhcFactorCalib, with a Python
  pickle round-trip test.
- Document that the NHC cannot tell the forward axis from the backward
  axis, so NhcFactorCalib has two minima selected by initialization.
  Test a mount differing in both pitch and yaw, recovering its forward
  axis from identity and the flipped axis from a yaw-pi initialization.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01RcxMmH57diwYwoMExn65U7
@inuex35

inuex35 commented Nov 16, 2026

Copy link
Copy Markdown
Contributor Author

Thanks for the review!

  • Vertical lever arm: added VerticalLeverArmCompensatesRoll (roof-mounted IMU under a roll rate). Verified it fails when the z component of the lever arm is dropped from the implementation, which the previous tests did not catch.
  • Added void serialize() const; to both NhcFactor and NhcFactorCalib in navigation.i, with a Python pickle round-trip test.
  • Documented the forward/backward (0 vs pi) ambiguity in NhcFactorCalib, and added RecoverPitchAndYawMountingUpToForwardSign: a mount with both pitch and yaw offsets recovers its forward axis from identity, and converges to the flipped axis from a yaw-pi initialization.

This branch has not been deployed

No deployments
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

3 participants