From a964316056384df770c4a388efc3222472766c4a Mon Sep 17 00:00:00 2001 From: inuex35 Date: Mon, 2 Mar 2026 22:04:14 +0900 Subject: [PATCH 1/5] add leverarm for pseudorange --- gtsam/navigation/PseudorangeFactor.cpp | 67 ++++++++++ gtsam/navigation/PseudorangeFactor.h | 100 +++++++++++++++ gtsam/navigation/navigation.i | 23 ++++ .../tests/testPseudorangeFactor.cpp | 114 ++++++++++++++++++ 4 files changed, 304 insertions(+) diff --git a/gtsam/navigation/PseudorangeFactor.cpp b/gtsam/navigation/PseudorangeFactor.cpp index 2ac06cec0c..d96aedf8e1 100644 --- a/gtsam/navigation/PseudorangeFactor.cpp +++ b/gtsam/navigation/PseudorangeFactor.cpp @@ -134,4 +134,71 @@ Vector DifferentialPseudorangeFactor::evaluateError( return Vector1(error); } +//*************************************************************************** +PseudorangeFactorArm::PseudorangeFactorArm( + const Key nTbKey, const Key receiverClockBiasKey, + const double measuredPseudorange, const Point3& satellitePosition, + const Point3& leverArm, const double satelliteClockBias, + const SharedNoiseModel& model) + : Base(model, nTbKey, receiverClockBiasKey), + PseudorangeBase{measuredPseudorange, satellitePosition, + satelliteClockBias}, + bL_(leverArm) {} + +//*************************************************************************** +void PseudorangeFactorArm::print(const std::string& s, + const KeyFormatter& keyFormatter) const { + Base::print(s, keyFormatter); + gtsam::print(pseudorange_, "pseudorange (m): "); + gtsam::print(Vector(satPos_), "sat position (ECEF meters): "); + gtsam::print(satClkBias_, "sat clock bias (s): "); + gtsam::print(Vector(bL_), "lever arm (body frame meters): "); +} + +//*************************************************************************** +bool PseudorangeFactorArm::equals(const NonlinearFactor& expected, + double tol) const { + const This* e = dynamic_cast(&expected); + return e != nullptr && Base::equals(*e, tol) && + traits::Equals(pseudorange_, e->pseudorange_, tol) && + traits::Equals(satPos_, e->satPos_, tol) && + traits::Equals(satClkBias_, e->satClkBias_, tol) && + traits::Equals(bL_, e->bL_, tol); +} + +//*************************************************************************** +Vector PseudorangeFactorArm::evaluateError( + const Pose3& nTb, const double& receiverClockBias, + OptionalMatrixType H_nTb, + OptionalMatrixType HreceiverClockBias) const { + // Compute antenna position in the navigation frame: + const Matrix3 nRb = nTb.rotation().matrix(); + const Point3 antennaPos = nTb.translation() + nRb * bL_; + + // Apply pseudorange equation: rho = range + c*[dt_u - dt^s] + const Vector3 position_difference = antennaPos - satPos_; + const double range = position_difference.norm(); + const double rho = range + CLIGHT * (receiverClockBias - satClkBias_); + const double error = rho - pseudorange_; + + // Compute associated derivatives: + if (H_nTb) { + if (range < std::numeric_limits::epsilon()) { + *H_nTb = Matrix16::Zero(); + } else { + // u = unit vector from satellite to antenna + const RowVector3d u = (position_difference / range).transpose(); + H_nTb->resize(1, 6); + H_nTb->block<1, 3>(0, 0) = u * (-nRb * skewSymmetric(bL_)); + H_nTb->block<1, 3>(0, 3) = u * nRb; + } + } + + if (HreceiverClockBias) { + *HreceiverClockBias = I_1x1 * CLIGHT; + } + + return Vector1(error); +} + } // namespace gtsam diff --git a/gtsam/navigation/PseudorangeFactor.h b/gtsam/navigation/PseudorangeFactor.h index 79fb7a4f53..a64c4d3f0d 100644 --- a/gtsam/navigation/PseudorangeFactor.h +++ b/gtsam/navigation/PseudorangeFactor.h @@ -7,6 +7,7 @@ #pragma once #include +#include #include #include @@ -225,4 +226,103 @@ template <> struct traits : public Testable {}; +/** + * GNSS pseudorange factor with lever arm correction. + * + * Like PseudorangeFactor, but uses a Pose3 (position + attitude) as the + * receiver state variable, allowing compensation for a lever arm offset + * between the body frame origin and the GNSS antenna location. + * + * The antenna position is computed as: + * antenna_pos = nTb.translation() + nRb * bL_ + * + * where bL_ is the lever arm from the body origin to the antenna in the body + * frame. The error model is: + * error = ||antenna_pos - satPos|| + c*(dt_u - dt_s) - pseudorange + * + * @ingroup navigation + */ +class GTSAM_EXPORT PseudorangeFactorArm + : public NoiseModelFactorN, + private PseudorangeBase { + private: + typedef NoiseModelFactorN Base; + + Point3 bL_; ///< Lever arm from body origin to antenna in body frame. + + public: + // Provide access to the Matrix& version of evaluateError: + using Base::evaluateError; + + /// shorthand for a smart pointer to a factor + typedef std::shared_ptr shared_ptr; + + /// Typedef to this class + typedef PseudorangeFactorArm This; + + /** default constructor - only use for serialization */ + PseudorangeFactorArm() : bL_(0, 0, 0) {} + + virtual ~PseudorangeFactorArm() = default; + + /** + * Construct a PseudorangeFactorArm. + * + * @param nTbKey Receiver gtsam::Pose3 key (body pose in navigation frame). + * @param receiverClockBiasKey Receiver clock bias node. + * @param measuredPseudorange Receiver-measured pseudorange in meters. + * @param satellitePosition Satellite ECEF position in meters. + * @param leverArm Translation from body origin to antenna in body frame. + * @param satelliteClockBias Satellite clock bias in seconds. + * @param model 1-D pseudorange noise model. + */ + PseudorangeFactorArm( + Key nTbKey, Key receiverClockBiasKey, + double measuredPseudorange, const Point3& satellitePosition, + const Point3& leverArm, double satelliteClockBias = 0.0, + const SharedNoiseModel& model = noiseModel::Unit::Create(1)); + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// print + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; + + /// equals + bool equals(const NonlinearFactor& expected, + double tol = 1e-9) const override; + + /// vector of errors + Vector evaluateError(const Pose3& nTb, + const double& receiverClockBias, + OptionalMatrixType H_nTb, + OptionalMatrixType HreceiverClockBias) const override; + + /// return the lever arm, a position in the body frame + inline const Point3& leverArm() const { return bL_; } + + private: +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PseudorangeFactorArm::Base); + ar& BOOST_SERIALIZATION_NVP(pseudorange_); + ar& BOOST_SERIALIZATION_NVP(satPos_); + ar& BOOST_SERIALIZATION_NVP(satClkBias_); + ar& BOOST_SERIALIZATION_NVP(bL_); + } +#endif +}; + +/// traits +template <> +struct traits + : public Testable {}; + } // namespace gtsam diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 3530d0e62c..89ec61b07a 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -548,6 +548,29 @@ virtual class DifferentialPseudorangeFactor : gtsam::NonlinearFactor { void serialize() const; }; +virtual class PseudorangeFactorArm : gtsam::NonlinearFactor { + PseudorangeFactorArm(gtsam::Key nTbKey, + gtsam::Key receiverClockBiasKey, + double measuredPseudorange, + const gtsam::Point3& satellitePosition, + const gtsam::Point3& leverArm, + double satelliteClockBias, + const gtsam::noiseModel::Base* model); + + // Testable + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::NonlinearFactor& expected, double tol); + + // Standard Interface + gtsam::Vector evaluateError(const gtsam::Pose3& nTb, + const double& receiverClockBias) const; + const gtsam::Point3& leverArm() const; + + // enable serialization functionality + void serialize() const; +}; + #include virtual class BarometricFactor : gtsam::NonlinearFactor { BarometricFactor(); diff --git a/gtsam/navigation/tests/testPseudorangeFactor.cpp b/gtsam/navigation/tests/testPseudorangeFactor.cpp index 7a558534dd..f2df6d532b 100644 --- a/gtsam/navigation/tests/testPseudorangeFactor.cpp +++ b/gtsam/navigation/tests/testPseudorangeFactor.cpp @@ -182,6 +182,120 @@ TEST(TestDifferentialPseudorangeFactor, equals) { factor2.print("factor2"); } +// ************************************************************************* +TEST(TestPseudorangeFactorArm, Constructor) { + const Point3 leverArm(0.1, 0.2, 0.3); + const auto factor = PseudorangeFactorArm( + Key(0), Key(1), 0.0, Point3::Zero(), leverArm, 0.0, + noiseModel::Isotropic::Sigma(1, 1.0)); + + Matrix Hpose, Hbias; + const double error = + factor.evaluateError(Pose3::Identity(), 0.0, Hpose, Hbias)[0]; + + // With identity pose and zero satellite position, the antenna is at the + // lever arm position, so range = ||leverArm|| = sqrt(0.01+0.04+0.09) + const double expectedRange = leverArm.norm(); + EXPECT_DOUBLES_EQUAL(expectedRange, error, 1e-9); + + // Check Jacobians are not NaN: + EXPECT(!Hpose.array().isNaN().any()); + EXPECT(!Hbias.array().isNaN().any()); + // Clock bias derivative should always be speed-of-light in vacuum: + EXPECT_DOUBLES_EQUAL(Hbias.norm(), 299792458.0, 1e-9); +} + +// ************************************************************************* +TEST(TestPseudorangeFactorArm, Jacobians1) { + const Point3 leverArm(0.5, -0.3, 1.0); + const auto factor = PseudorangeFactorArm( + Key(0), Key(1), // Receiver pose and clock bias keys. + 4.0, // Measured pseudorange. + Vector3(0.0, 0.0, 3.0), // Satellite position. + leverArm, + 0.0 // Sat clock drift bias. + ); + + Values values; + values.insert(Key(0), Pose3(Rot3::RzRyRx(0.1, 0.2, 0.3), + Point3(1.0, 2.0, 3.0))); + values.insert(Key(1), 0.0); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5); +} + +// ************************************************************************* +TEST(TestPseudorangeFactorArm, Jacobians2) { + // Example values adapted from SinglePointPositioningExample: + const Point3 leverArm(0.1, 0.0, -0.5); + const auto factor = PseudorangeFactorArm( + Key(0), Key(1), // Receiver pose and clock bias keys. + 24874028.989, // Measured pseudorange. + Vector3(-5824269.46342, -22935011.26952, -12195522.22428), + leverArm, + -0.00022743876852667193 // Sat clock drift bias. + ); + + Values values; + values.insert(Key(0), + Pose3(Rot3::RzRyRx(0.05, -0.03, 0.1), + Point3(-2684418.91084688, -4293361.08683296, + 3865365.45451951))); + values.insert(Key(1), 5.377885093511699e-07); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-3, 1e-5); +} + +// ************************************************************************* +TEST(TestPseudorangeFactorArm, ZeroLeverArm) { + // With zero lever arm, PseudorangeFactorArm should produce the same error + // as PseudorangeFactor at the same position: + const Point3 satPos(100.0, 200.0, 300.0); + const Point3 receiverPos(1.0, 2.0, 3.0); + const double pseudorange = 350.0; + const double clockBias = 1e-8; + const double satClkBias = 1e-9; + + const auto factorPoint = PseudorangeFactor( + Key(0), Key(1), pseudorange, satPos, satClkBias); + const auto factorArm = PseudorangeFactorArm( + Key(0), Key(1), pseudorange, satPos, Point3::Zero(), satClkBias); + + const double errorPoint = + factorPoint.evaluateError(receiverPos, clockBias)[0]; + const double errorArm = + factorArm.evaluateError(Pose3(Rot3::Identity(), receiverPos), + clockBias)[0]; + EXPECT_DOUBLES_EQUAL(errorPoint, errorArm, 1e-9); +} + +// ************************************************************************* +TEST(TestPseudorangeFactorArm, print) { + const auto factor = PseudorangeFactorArm(); + factor.print(); +} + +// ************************************************************************* +TEST(TestPseudorangeFactorArm, equals) { + const Point3 leverArm(0.1, 0.2, 0.3); + const auto factor1 = PseudorangeFactorArm(); + const auto factor2 = + PseudorangeFactorArm(1, 2, 0.0, Point3::Zero(), leverArm, 0.0); + const auto factor3 = + PseudorangeFactorArm(1, 2, 10.0, Point3(1.0, 2.0, 3.0), leverArm, 20.0); + + CHECK(factor1.equals(factor1)); + CHECK(factor2.equals(factor2)); + CHECK(!factor1.equals(factor2)); + CHECK(factor2.equals(factor3, 1e99)); + + // Different lever arm should not be equal: + const auto factor4 = + PseudorangeFactorArm(1, 2, 0.0, Point3::Zero(), + Point3(9.0, 8.0, 7.0), 0.0); + CHECK(!factor2.equals(factor4)); + + factor2.print("factor2"); +} + // ************************************************************************* int main() { TestResult tr; From d65e9892ea207f2246afe921f8a6cbc5d9b21d5c Mon Sep 17 00:00:00 2001 From: inuex35 Date: Mon, 2 Mar 2026 22:52:08 +0900 Subject: [PATCH 2/5] add differential --- gtsam/navigation/PseudorangeFactor.cpp | 72 ++++++++ gtsam/navigation/PseudorangeFactor.h | 98 ++++++++++ gtsam/navigation/navigation.i | 25 +++ .../tests/testPseudorangeFactor.cpp | 169 ++++++++++++++++++ 4 files changed, 364 insertions(+) diff --git a/gtsam/navigation/PseudorangeFactor.cpp b/gtsam/navigation/PseudorangeFactor.cpp index d96aedf8e1..67b8fdfad1 100644 --- a/gtsam/navigation/PseudorangeFactor.cpp +++ b/gtsam/navigation/PseudorangeFactor.cpp @@ -201,4 +201,76 @@ Vector PseudorangeFactorArm::evaluateError( return Vector1(error); } +//*************************************************************************** +DifferentialPseudorangeFactorArm::DifferentialPseudorangeFactorArm( + const Key nTbKey, const Key receiverClockBiasKey, + const Key differentialCorrectionKey, const double measuredPseudorange, + const Point3& satellitePosition, const Point3& leverArm, + const double satelliteClockBias, const SharedNoiseModel& model) + : Base(model, nTbKey, receiverClockBiasKey, differentialCorrectionKey), + PseudorangeBase{measuredPseudorange, satellitePosition, + satelliteClockBias}, + bL_(leverArm) {} + +//*************************************************************************** +void DifferentialPseudorangeFactorArm::print( + const std::string& s, const KeyFormatter& keyFormatter) const { + Base::print(s, keyFormatter); + gtsam::print(pseudorange_, "pseudorange (m): "); + gtsam::print(Vector(satPos_), "sat position (ECEF meters): "); + gtsam::print(satClkBias_, "sat clock bias (s): "); + gtsam::print(Vector(bL_), "lever arm (body frame meters): "); +} + +//*************************************************************************** +bool DifferentialPseudorangeFactorArm::equals( + const NonlinearFactor& expected, double tol) const { + const This* e = dynamic_cast(&expected); + return e != nullptr && Base::equals(*e, tol) && + traits::Equals(pseudorange_, e->pseudorange_, tol) && + traits::Equals(satPos_, e->satPos_, tol) && + traits::Equals(satClkBias_, e->satClkBias_, tol) && + traits::Equals(bL_, e->bL_, tol); +} + +//*************************************************************************** +Vector DifferentialPseudorangeFactorArm::evaluateError( + const Pose3& nTb, const double& receiverClockBias, + const double& differentialCorrection, OptionalMatrixType H_nTb, + OptionalMatrixType HreceiverClockBias, + OptionalMatrixType HdifferentialCorrection) const { + // Compute antenna position in the navigation frame: + const Matrix3 nRb = nTb.rotation().matrix(); + const Point3 antennaPos = nTb.translation() + nRb * bL_; + + // Apply pseudorange equation: rho = range + c*[dt_u - dt^s] + const Vector3 position_difference = antennaPos - satPos_; + const double range = position_difference.norm(); + const double rho = range + CLIGHT * (receiverClockBias - satClkBias_); + const double error = rho - pseudorange_ - differentialCorrection; + + // Compute associated derivatives: + if (H_nTb) { + if (range < std::numeric_limits::epsilon()) { + *H_nTb = Matrix16::Zero(); + } else { + // u = unit vector from satellite to antenna + const RowVector3d u = (position_difference / range).transpose(); + H_nTb->resize(1, 6); + H_nTb->block<1, 3>(0, 0) = u * (-nRb * skewSymmetric(bL_)); + H_nTb->block<1, 3>(0, 3) = u * nRb; + } + } + + if (HreceiverClockBias) { + *HreceiverClockBias = I_1x1 * CLIGHT; + } + + if (HdifferentialCorrection) { + *HdifferentialCorrection = -I_1x1; + } + + return Vector1(error); +} + } // namespace gtsam diff --git a/gtsam/navigation/PseudorangeFactor.h b/gtsam/navigation/PseudorangeFactor.h index a64c4d3f0d..bd1e25bfd3 100644 --- a/gtsam/navigation/PseudorangeFactor.h +++ b/gtsam/navigation/PseudorangeFactor.h @@ -325,4 +325,102 @@ template <> struct traits : public Testable {}; +/** + * Differentially-corrected pseudorange factor with lever arm correction. + * + * Combines the differential correction model of DifferentialPseudorangeFactor + * with the lever arm compensation of PseudorangeFactorArm. Uses a Pose3 + * (position + attitude) as the receiver state variable. + * + * The error model is: + * error = ||antenna_pos - satPos|| + c*(dt_u - dt_s) - pseudorange - correction + * + * @ingroup navigation + */ +class GTSAM_EXPORT DifferentialPseudorangeFactorArm + : public NoiseModelFactorN, + private PseudorangeBase { + private: + typedef NoiseModelFactorN Base; + + Point3 bL_; ///< Lever arm from body origin to antenna in body frame. + + public: + // Provide access to the Matrix& version of evaluateError: + using Base::evaluateError; + + /// shorthand for a smart pointer to a factor + typedef std::shared_ptr shared_ptr; + + /// Typedef to this class + typedef DifferentialPseudorangeFactorArm This; + + /** default constructor - only use for serialization */ + DifferentialPseudorangeFactorArm() : bL_(0, 0, 0) {} + + virtual ~DifferentialPseudorangeFactorArm() = default; + + /** + * Construct a DifferentialPseudorangeFactorArm. + * + * @param nTbKey Receiver gtsam::Pose3 key (body pose in navigation frame). + * @param receiverClockBiasKey Receiver clock bias node. + * @param differentialCorrectionKey Differential correction node. + * @param measuredPseudorange Receiver-measured pseudorange in meters. + * @param satellitePosition Satellite ECEF position in meters. + * @param leverArm Translation from body origin to antenna in body frame. + * @param satelliteClockBias Satellite clock bias in seconds. + * @param model 1-D pseudorange noise model. + */ + DifferentialPseudorangeFactorArm( + Key nTbKey, Key receiverClockBiasKey, Key differentialCorrectionKey, + double measuredPseudorange, const Point3& satellitePosition, + const Point3& leverArm, double satelliteClockBias = 0.0, + const SharedNoiseModel& model = noiseModel::Unit::Create(1)); + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /// print + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; + + /// equals + bool equals(const NonlinearFactor& expected, + double tol = 1e-9) const override; + + /// vector of errors + Vector evaluateError(const Pose3& nTb, const double& receiverClockBias, + const double& differentialCorrection, + OptionalMatrixType H_nTb, + OptionalMatrixType HreceiverClockBias, + OptionalMatrixType HdifferentialCorrection) const override; + + /// return the lever arm, a position in the body frame + inline const Point3& leverArm() const { return bL_; } + + private: +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP( + DifferentialPseudorangeFactorArm::Base); + ar& BOOST_SERIALIZATION_NVP(pseudorange_); + ar& BOOST_SERIALIZATION_NVP(satPos_); + ar& BOOST_SERIALIZATION_NVP(satClkBias_); + ar& BOOST_SERIALIZATION_NVP(bL_); + } +#endif +}; + +/// traits +template <> +struct traits + : public Testable {}; + } // namespace gtsam diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 89ec61b07a..78609b4b5d 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -571,6 +571,31 @@ virtual class PseudorangeFactorArm : gtsam::NonlinearFactor { void serialize() const; }; +virtual class DifferentialPseudorangeFactorArm : gtsam::NonlinearFactor { + DifferentialPseudorangeFactorArm(gtsam::Key nTbKey, + gtsam::Key receiverClockBiasKey, + gtsam::Key differentialCorrectionKey, + double measuredPseudorange, + const gtsam::Point3& satellitePosition, + const gtsam::Point3& leverArm, + double satelliteClockBias, + const gtsam::noiseModel::Base* model); + + // Testable + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::NonlinearFactor& expected, double tol); + + // Standard Interface + gtsam::Vector evaluateError(const gtsam::Pose3& nTb, + const double& receiverClockBias, + const double& differentialCorrection) const; + const gtsam::Point3& leverArm() const; + + // enable serialization functionality + void serialize() const; +}; + #include virtual class BarometricFactor : gtsam::NonlinearFactor { BarometricFactor(); diff --git a/gtsam/navigation/tests/testPseudorangeFactor.cpp b/gtsam/navigation/tests/testPseudorangeFactor.cpp index f2df6d532b..f39ca6dffc 100644 --- a/gtsam/navigation/tests/testPseudorangeFactor.cpp +++ b/gtsam/navigation/tests/testPseudorangeFactor.cpp @@ -244,6 +244,36 @@ TEST(TestPseudorangeFactorArm, Jacobians2) { EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-3, 1e-5); } +// ************************************************************************* +TEST(TestPseudorangeFactorArm, LeverArmZeroError) { + // If the body position is set so that antenna_pos = body_pos + nRb * leverArm + // gives the correct range, the error should be zero. + // This mirrors the GPSFactorArm test pattern. + const Point3 leverArm(0.5, -0.3, 1.0); + const Rot3 nRb = Rot3::RzRyRx(0.15, -0.30, 0.45); + const Point3 satPos(1000.0, 2000.0, 3000.0); + + // Choose antenna position, then back-compute body position: + const Point3 antennaPos(10.0, 20.0, 30.0); + const Point3 bodyPos = antennaPos - nRb.matrix() * leverArm; + const Pose3 nTb(nRb, bodyPos); + + // The true range from antenna to satellite: + const double trueRange = (antennaPos - satPos).norm(); + + // Set pseudorange = trueRange + c * (clockBias - satClkBias) + // so that the error is exactly zero: + const double clockBias = 1e-7; + const double satClkBias = 2e-8; + const double pseudorange = trueRange + 299792458.0 * (clockBias - satClkBias); + + const auto factor = PseudorangeFactorArm( + Key(0), Key(1), pseudorange, satPos, leverArm, satClkBias); + + const double error = factor.evaluateError(nTb, clockBias)[0]; + EXPECT_DOUBLES_EQUAL(0.0, error, 1e-6); +} + // ************************************************************************* TEST(TestPseudorangeFactorArm, ZeroLeverArm) { // With zero lever arm, PseudorangeFactorArm should produce the same error @@ -296,6 +326,145 @@ TEST(TestPseudorangeFactorArm, equals) { factor2.print("factor2"); } +// ************************************************************************* +TEST(TestDifferentialPseudorangeFactorArm, Constructor) { + const Point3 leverArm(0.1, 0.2, 0.3); + const auto factor = DifferentialPseudorangeFactorArm( + Key(0), Key(1), Key(2), 0.0, Point3::Zero(), leverArm, 0.0, + noiseModel::Isotropic::Sigma(1, 1.0)); + + Matrix Hpose, Hbias, Hcorrection; + const double error = factor.evaluateError(Pose3::Identity(), 0.0, 0.0, Hpose, + Hbias, Hcorrection)[0]; + + const double expectedRange = leverArm.norm(); + EXPECT_DOUBLES_EQUAL(expectedRange, error, 1e-9); + + EXPECT(!Hpose.array().isNaN().any()); + EXPECT(!Hbias.array().isNaN().any()); + EXPECT(!Hcorrection.array().isNaN().any()); + EXPECT_DOUBLES_EQUAL(Hbias.norm(), 299792458.0, 1e-9); + EXPECT_DOUBLES_EQUAL(Hcorrection(0, 0), -1.0, 1e-9); +} + +// ************************************************************************* +TEST(TestDifferentialPseudorangeFactorArm, Jacobians1) { + const Point3 leverArm(0.5, -0.3, 1.0); + const auto factor = DifferentialPseudorangeFactorArm( + Key(0), Key(1), Key(2), + 4.0, + Vector3(0.0, 0.0, 3.0), + leverArm, + 0.0 + ); + + Values values; + values.insert(Key(0), Pose3(Rot3::RzRyRx(0.1, 0.2, 0.3), + Point3(1.0, 2.0, 3.0))); + values.insert(Key(1), 0.0); + values.insert(Key(2), 0.0); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5); +} + +// ************************************************************************* +TEST(TestDifferentialPseudorangeFactorArm, Jacobians2) { + const Point3 leverArm(0.1, 0.0, -0.5); + const auto factor = DifferentialPseudorangeFactorArm( + Key(0), Key(1), Key(2), + 24874028.989, + Vector3(-5824269.46342, -22935011.26952, -12195522.22428), + leverArm, + -0.00022743876852667193 + ); + + Values values; + values.insert(Key(0), + Pose3(Rot3::RzRyRx(0.05, -0.03, 0.1), + Point3(-2684418.91084688, -4293361.08683296, + 3865365.45451951))); + values.insert(Key(1), 5.377885093511699e-07); + values.insert(Key(2), 10.0); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-3, 1e-5); +} + +// ************************************************************************* +TEST(TestDifferentialPseudorangeFactorArm, LeverArmZeroError) { + const Point3 leverArm(0.5, -0.3, 1.0); + const Rot3 nRb = Rot3::RzRyRx(0.15, -0.30, 0.45); + const Point3 satPos(1000.0, 2000.0, 3000.0); + + const Point3 antennaPos(10.0, 20.0, 30.0); + const Point3 bodyPos = antennaPos - nRb.matrix() * leverArm; + const Pose3 nTb(nRb, bodyPos); + + const double trueRange = (antennaPos - satPos).norm(); + const double clockBias = 1e-7; + const double satClkBias = 2e-8; + const double correction = 5.0; + // pseudorange = trueRange + c*(clockBias - satClkBias) - correction + // so that error = 0 + const double pseudorange = + trueRange + 299792458.0 * (clockBias - satClkBias) - correction; + + const auto factor = DifferentialPseudorangeFactorArm( + Key(0), Key(1), Key(2), pseudorange, satPos, leverArm, satClkBias); + + const double error = + factor.evaluateError(nTb, clockBias, correction)[0]; + EXPECT_DOUBLES_EQUAL(0.0, error, 1e-6); +} + +// ************************************************************************* +TEST(TestDifferentialPseudorangeFactorArm, ZeroLeverArm) { + // With zero lever arm, should produce the same error as + // DifferentialPseudorangeFactor at the same position: + const Point3 satPos(100.0, 200.0, 300.0); + const Point3 receiverPos(1.0, 2.0, 3.0); + const double pseudorange = 350.0; + const double clockBias = 1e-8; + const double satClkBias = 1e-9; + const double correction = 5.0; + + const auto factorPoint = DifferentialPseudorangeFactor( + Key(0), Key(1), Key(2), pseudorange, satPos, satClkBias); + const auto factorArm = DifferentialPseudorangeFactorArm( + Key(0), Key(1), Key(2), pseudorange, satPos, Point3::Zero(), satClkBias); + + const double errorPoint = + factorPoint.evaluateError(receiverPos, clockBias, correction)[0]; + const double errorArm = + factorArm.evaluateError(Pose3(Rot3::Identity(), receiverPos), + clockBias, correction)[0]; + EXPECT_DOUBLES_EQUAL(errorPoint, errorArm, 1e-9); +} + +// ************************************************************************* +TEST(TestDifferentialPseudorangeFactorArm, print) { + const auto factor = DifferentialPseudorangeFactorArm(); + factor.print(); +} + +// ************************************************************************* +TEST(TestDifferentialPseudorangeFactorArm, equals) { + const Point3 leverArm(0.1, 0.2, 0.3); + const auto factor1 = DifferentialPseudorangeFactorArm(); + const auto factor2 = DifferentialPseudorangeFactorArm( + 1, 2, 3, 0.0, Point3::Zero(), leverArm, 0.0); + const auto factor3 = DifferentialPseudorangeFactorArm( + 1, 2, 3, 10.0, Point3(1.0, 2.0, 3.0), leverArm, 20.0); + + CHECK(factor1.equals(factor1)); + CHECK(factor2.equals(factor2)); + CHECK(!factor1.equals(factor2)); + CHECK(factor2.equals(factor3, 1e99)); + + const auto factor4 = DifferentialPseudorangeFactorArm( + 1, 2, 3, 0.0, Point3::Zero(), Point3(9.0, 8.0, 7.0), 0.0); + CHECK(!factor2.equals(factor4)); + + factor2.print("factor2"); +} + // ************************************************************************* int main() { TestResult tr; From b5e4a93ef1e39efa773aba2a3f8fb9a4bb23c242 Mon Sep 17 00:00:00 2001 From: inuex35 Date: Sat, 7 Mar 2026 14:38:17 +0900 Subject: [PATCH 3/5] fix --- gtsam/navigation/PseudorangeFactor.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/navigation/PseudorangeFactor.cpp b/gtsam/navigation/PseudorangeFactor.cpp index 67b8fdfad1..61075467f3 100644 --- a/gtsam/navigation/PseudorangeFactor.cpp +++ b/gtsam/navigation/PseudorangeFactor.cpp @@ -183,12 +183,12 @@ Vector PseudorangeFactorArm::evaluateError( // Compute associated derivatives: if (H_nTb) { + H_nTb->resize(1, 6); if (range < std::numeric_limits::epsilon()) { - *H_nTb = Matrix16::Zero(); + H_nTb->setZero(); } else { // u = unit vector from satellite to antenna - const RowVector3d u = (position_difference / range).transpose(); - H_nTb->resize(1, 6); + const Matrix u = (position_difference / range).transpose(); // 1x3 H_nTb->block<1, 3>(0, 0) = u * (-nRb * skewSymmetric(bL_)); H_nTb->block<1, 3>(0, 3) = u * nRb; } @@ -251,12 +251,12 @@ Vector DifferentialPseudorangeFactorArm::evaluateError( // Compute associated derivatives: if (H_nTb) { + H_nTb->resize(1, 6); if (range < std::numeric_limits::epsilon()) { - *H_nTb = Matrix16::Zero(); + H_nTb->setZero(); } else { // u = unit vector from satellite to antenna - const RowVector3d u = (position_difference / range).transpose(); - H_nTb->resize(1, 6); + const Matrix u = (position_difference / range).transpose(); // 1x3 H_nTb->block<1, 3>(0, 0) = u * (-nRb * skewSymmetric(bL_)); H_nTb->block<1, 3>(0, 3) = u * nRb; } From 6833d66080dc77bcfa662264665f37167fab4c06 Mon Sep 17 00:00:00 2001 From: Claude Date: Sat, 7 Mar 2026 14:56:11 +0000 Subject: [PATCH 4/5] Address PR #2447 review feedback from Copilot - Initialize PseudorangeBase in default constructors of PseudorangeFactorArm and DifferentialPseudorangeFactorArm to avoid undefined behavior - Replace Hbias.norm() with Hbias(0, 0) in tests to avoid floating-point issues with sqrt on large values like the speed of light - Add explicit #include in PseudorangeFactor.cpp for std::numeric_limits usage https://claude.ai/code/session_01WwBj5et3656PootjzHLNEJ --- gtsam/navigation/PseudorangeFactor.cpp | 2 ++ gtsam/navigation/PseudorangeFactor.h | 4 ++-- gtsam/navigation/tests/testPseudorangeFactor.cpp | 8 ++++---- 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/gtsam/navigation/PseudorangeFactor.cpp b/gtsam/navigation/PseudorangeFactor.cpp index 61075467f3..5ddabf1c05 100644 --- a/gtsam/navigation/PseudorangeFactor.cpp +++ b/gtsam/navigation/PseudorangeFactor.cpp @@ -7,6 +7,8 @@ #include "PseudorangeFactor.h" +#include + namespace { /// Speed of light in a vacuum (m/s): diff --git a/gtsam/navigation/PseudorangeFactor.h b/gtsam/navigation/PseudorangeFactor.h index bd1e25bfd3..5f06b8ec24 100644 --- a/gtsam/navigation/PseudorangeFactor.h +++ b/gtsam/navigation/PseudorangeFactor.h @@ -261,7 +261,7 @@ class GTSAM_EXPORT PseudorangeFactorArm typedef PseudorangeFactorArm This; /** default constructor - only use for serialization */ - PseudorangeFactorArm() : bL_(0, 0, 0) {} + PseudorangeFactorArm() : PseudorangeBase{0.0, Point3(0, 0, 0), 0.0}, bL_(0, 0, 0) {} virtual ~PseudorangeFactorArm() = default; @@ -356,7 +356,7 @@ class GTSAM_EXPORT DifferentialPseudorangeFactorArm typedef DifferentialPseudorangeFactorArm This; /** default constructor - only use for serialization */ - DifferentialPseudorangeFactorArm() : bL_(0, 0, 0) {} + DifferentialPseudorangeFactorArm() : PseudorangeBase{0.0, Point3(0, 0, 0), 0.0}, bL_(0, 0, 0) {} virtual ~DifferentialPseudorangeFactorArm() = default; diff --git a/gtsam/navigation/tests/testPseudorangeFactor.cpp b/gtsam/navigation/tests/testPseudorangeFactor.cpp index f39ca6dffc..708077a541 100644 --- a/gtsam/navigation/tests/testPseudorangeFactor.cpp +++ b/gtsam/navigation/tests/testPseudorangeFactor.cpp @@ -33,7 +33,7 @@ TEST(TestPseudorangeFactor, Constructor) { EXPECT(!Hbias.array().isNaN().any()); EXPECT_DOUBLES_EQUAL(Hpos.norm(), 0.0, 1e-9); // Clock bias derivative should always be speed-of-light in vacuum: - EXPECT_DOUBLES_EQUAL(Hbias.norm(), 299792458.0, 1e-9); + EXPECT_DOUBLES_EQUAL(Hbias(0, 0), 299792458.0, 1e-9); } // ************************************************************************* @@ -118,7 +118,7 @@ TEST(TestDifferentialPseudorangeFactor, Constructor) { EXPECT(!Hcorrection.array().isNaN().any()); EXPECT_DOUBLES_EQUAL(Hpos.norm(), 0.0, 1e-9); // Clock bias derivative should always be speed-of-light in vacuum: - EXPECT_DOUBLES_EQUAL(Hbias.norm(), 299792458.0, 1e-9); + EXPECT_DOUBLES_EQUAL(Hbias(0, 0), 299792458.0, 1e-9); // Correction derivative should be constant -1: EXPECT_DOUBLES_EQUAL(Hcorrection(0, 0), -1.0, 1e-9); } @@ -202,7 +202,7 @@ TEST(TestPseudorangeFactorArm, Constructor) { EXPECT(!Hpose.array().isNaN().any()); EXPECT(!Hbias.array().isNaN().any()); // Clock bias derivative should always be speed-of-light in vacuum: - EXPECT_DOUBLES_EQUAL(Hbias.norm(), 299792458.0, 1e-9); + EXPECT_DOUBLES_EQUAL(Hbias(0, 0), 299792458.0, 1e-9); } // ************************************************************************* @@ -343,7 +343,7 @@ TEST(TestDifferentialPseudorangeFactorArm, Constructor) { EXPECT(!Hpose.array().isNaN().any()); EXPECT(!Hbias.array().isNaN().any()); EXPECT(!Hcorrection.array().isNaN().any()); - EXPECT_DOUBLES_EQUAL(Hbias.norm(), 299792458.0, 1e-9); + EXPECT_DOUBLES_EQUAL(Hbias(0, 0), 299792458.0, 1e-9); EXPECT_DOUBLES_EQUAL(Hcorrection(0, 0), -1.0, 1e-9); } From c792c0597c7282d6646e05460bb70a011cfa0914 Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 12 Mar 2026 00:40:23 +0000 Subject: [PATCH 5/5] Rename frame variables from nTb/nRb to ecef_T_body/ecef_R_body Address PR review feedback from dellaert: rename frame variables to make explicit that the body pose and satellite position are in ECEF, not a generic navigation frame. Also update documentation to clarify that local frame (NED/ENU) conversion is the caller's responsibility. https://claude.ai/code/session_01GqWXxUkLXHV89MnocPt1ws --- gtsam/navigation/PseudorangeFactor.cpp | 50 ++++++++++--------- gtsam/navigation/PseudorangeFactor.h | 27 ++++++---- gtsam/navigation/navigation.i | 8 +-- .../tests/testPseudorangeFactor.cpp | 18 +++---- 4 files changed, 57 insertions(+), 46 deletions(-) diff --git a/gtsam/navigation/PseudorangeFactor.cpp b/gtsam/navigation/PseudorangeFactor.cpp index 5ddabf1c05..12d5fa5d55 100644 --- a/gtsam/navigation/PseudorangeFactor.cpp +++ b/gtsam/navigation/PseudorangeFactor.cpp @@ -138,11 +138,11 @@ Vector DifferentialPseudorangeFactor::evaluateError( } //*************************************************************************** PseudorangeFactorArm::PseudorangeFactorArm( - const Key nTbKey, const Key receiverClockBiasKey, + const Key ecefTbodyKey, const Key receiverClockBiasKey, const double measuredPseudorange, const Point3& satellitePosition, const Point3& leverArm, const double satelliteClockBias, const SharedNoiseModel& model) - : Base(model, nTbKey, receiverClockBiasKey), + : Base(model, ecefTbodyKey, receiverClockBiasKey), PseudorangeBase{measuredPseudorange, satellitePosition, satelliteClockBias}, bL_(leverArm) {} @@ -170,12 +170,12 @@ bool PseudorangeFactorArm::equals(const NonlinearFactor& expected, //*************************************************************************** Vector PseudorangeFactorArm::evaluateError( - const Pose3& nTb, const double& receiverClockBias, - OptionalMatrixType H_nTb, + const Pose3& ecef_T_body, const double& receiverClockBias, + OptionalMatrixType H_ecef_T_body, OptionalMatrixType HreceiverClockBias) const { - // Compute antenna position in the navigation frame: - const Matrix3 nRb = nTb.rotation().matrix(); - const Point3 antennaPos = nTb.translation() + nRb * bL_; + // Compute antenna position in the ECEF frame: + const Matrix3 ecef_R_body = ecef_T_body.rotation().matrix(); + const Point3 antennaPos = ecef_T_body.translation() + ecef_R_body * bL_; // Apply pseudorange equation: rho = range + c*[dt_u - dt^s] const Vector3 position_difference = antennaPos - satPos_; @@ -184,15 +184,16 @@ Vector PseudorangeFactorArm::evaluateError( const double error = rho - pseudorange_; // Compute associated derivatives: - if (H_nTb) { - H_nTb->resize(1, 6); + if (H_ecef_T_body) { + H_ecef_T_body->resize(1, 6); if (range < std::numeric_limits::epsilon()) { - H_nTb->setZero(); + H_ecef_T_body->setZero(); } else { // u = unit vector from satellite to antenna const Matrix u = (position_difference / range).transpose(); // 1x3 - H_nTb->block<1, 3>(0, 0) = u * (-nRb * skewSymmetric(bL_)); - H_nTb->block<1, 3>(0, 3) = u * nRb; + H_ecef_T_body->block<1, 3>(0, 0) = + u * (-ecef_R_body * skewSymmetric(bL_)); + H_ecef_T_body->block<1, 3>(0, 3) = u * ecef_R_body; } } @@ -205,11 +206,11 @@ Vector PseudorangeFactorArm::evaluateError( //*************************************************************************** DifferentialPseudorangeFactorArm::DifferentialPseudorangeFactorArm( - const Key nTbKey, const Key receiverClockBiasKey, + const Key ecefTbodyKey, const Key receiverClockBiasKey, const Key differentialCorrectionKey, const double measuredPseudorange, const Point3& satellitePosition, const Point3& leverArm, const double satelliteClockBias, const SharedNoiseModel& model) - : Base(model, nTbKey, receiverClockBiasKey, differentialCorrectionKey), + : Base(model, ecefTbodyKey, receiverClockBiasKey, differentialCorrectionKey), PseudorangeBase{measuredPseudorange, satellitePosition, satelliteClockBias}, bL_(leverArm) {} @@ -237,13 +238,13 @@ bool DifferentialPseudorangeFactorArm::equals( //*************************************************************************** Vector DifferentialPseudorangeFactorArm::evaluateError( - const Pose3& nTb, const double& receiverClockBias, - const double& differentialCorrection, OptionalMatrixType H_nTb, + const Pose3& ecef_T_body, const double& receiverClockBias, + const double& differentialCorrection, OptionalMatrixType H_ecef_T_body, OptionalMatrixType HreceiverClockBias, OptionalMatrixType HdifferentialCorrection) const { - // Compute antenna position in the navigation frame: - const Matrix3 nRb = nTb.rotation().matrix(); - const Point3 antennaPos = nTb.translation() + nRb * bL_; + // Compute antenna position in the ECEF frame: + const Matrix3 ecef_R_body = ecef_T_body.rotation().matrix(); + const Point3 antennaPos = ecef_T_body.translation() + ecef_R_body * bL_; // Apply pseudorange equation: rho = range + c*[dt_u - dt^s] const Vector3 position_difference = antennaPos - satPos_; @@ -252,15 +253,16 @@ Vector DifferentialPseudorangeFactorArm::evaluateError( const double error = rho - pseudorange_ - differentialCorrection; // Compute associated derivatives: - if (H_nTb) { - H_nTb->resize(1, 6); + if (H_ecef_T_body) { + H_ecef_T_body->resize(1, 6); if (range < std::numeric_limits::epsilon()) { - H_nTb->setZero(); + H_ecef_T_body->setZero(); } else { // u = unit vector from satellite to antenna const Matrix u = (position_difference / range).transpose(); // 1x3 - H_nTb->block<1, 3>(0, 0) = u * (-nRb * skewSymmetric(bL_)); - H_nTb->block<1, 3>(0, 3) = u * nRb; + H_ecef_T_body->block<1, 3>(0, 0) = + u * (-ecef_R_body * skewSymmetric(bL_)); + H_ecef_T_body->block<1, 3>(0, 3) = u * ecef_R_body; } } diff --git a/gtsam/navigation/PseudorangeFactor.h b/gtsam/navigation/PseudorangeFactor.h index 5f06b8ec24..3c37fbb0b3 100644 --- a/gtsam/navigation/PseudorangeFactor.h +++ b/gtsam/navigation/PseudorangeFactor.h @@ -234,12 +234,16 @@ struct traits * between the body frame origin and the GNSS antenna location. * * The antenna position is computed as: - * antenna_pos = nTb.translation() + nRb * bL_ + * antenna_pos = ecef_T_body.translation() + ecef_R_body * bL_ * * where bL_ is the lever arm from the body origin to the antenna in the body * frame. The error model is: * error = ||antenna_pos - satPos|| + c*(dt_u - dt_s) - pseudorange * + * @note Both the body pose and satellite position are expected in the ECEF + * frame. Any conversion to/from a local navigation frame (NED/ENU) is + * the caller's responsibility. + * * @ingroup navigation */ class GTSAM_EXPORT PseudorangeFactorArm @@ -268,7 +272,7 @@ class GTSAM_EXPORT PseudorangeFactorArm /** * Construct a PseudorangeFactorArm. * - * @param nTbKey Receiver gtsam::Pose3 key (body pose in navigation frame). + * @param ecefTbodyKey Receiver gtsam::Pose3 key (body pose in ECEF frame). * @param receiverClockBiasKey Receiver clock bias node. * @param measuredPseudorange Receiver-measured pseudorange in meters. * @param satellitePosition Satellite ECEF position in meters. @@ -277,7 +281,7 @@ class GTSAM_EXPORT PseudorangeFactorArm * @param model 1-D pseudorange noise model. */ PseudorangeFactorArm( - Key nTbKey, Key receiverClockBiasKey, + Key ecefTbodyKey, Key receiverClockBiasKey, double measuredPseudorange, const Point3& satellitePosition, const Point3& leverArm, double satelliteClockBias = 0.0, const SharedNoiseModel& model = noiseModel::Unit::Create(1)); @@ -297,9 +301,9 @@ class GTSAM_EXPORT PseudorangeFactorArm double tol = 1e-9) const override; /// vector of errors - Vector evaluateError(const Pose3& nTb, + Vector evaluateError(const Pose3& ecef_T_body, const double& receiverClockBias, - OptionalMatrixType H_nTb, + OptionalMatrixType H_ecef_T_body, OptionalMatrixType HreceiverClockBias) const override; /// return the lever arm, a position in the body frame @@ -335,6 +339,10 @@ struct traits * The error model is: * error = ||antenna_pos - satPos|| + c*(dt_u - dt_s) - pseudorange - correction * + * @note Both the body pose and satellite position are expected in the ECEF + * frame. Any conversion to/from a local navigation frame (NED/ENU) is + * the caller's responsibility. + * * @ingroup navigation */ class GTSAM_EXPORT DifferentialPseudorangeFactorArm @@ -363,7 +371,7 @@ class GTSAM_EXPORT DifferentialPseudorangeFactorArm /** * Construct a DifferentialPseudorangeFactorArm. * - * @param nTbKey Receiver gtsam::Pose3 key (body pose in navigation frame). + * @param ecefTbodyKey Receiver gtsam::Pose3 key (body pose in ECEF frame). * @param receiverClockBiasKey Receiver clock bias node. * @param differentialCorrectionKey Differential correction node. * @param measuredPseudorange Receiver-measured pseudorange in meters. @@ -373,7 +381,7 @@ class GTSAM_EXPORT DifferentialPseudorangeFactorArm * @param model 1-D pseudorange noise model. */ DifferentialPseudorangeFactorArm( - Key nTbKey, Key receiverClockBiasKey, Key differentialCorrectionKey, + Key ecefTbodyKey, Key receiverClockBiasKey, Key differentialCorrectionKey, double measuredPseudorange, const Point3& satellitePosition, const Point3& leverArm, double satelliteClockBias = 0.0, const SharedNoiseModel& model = noiseModel::Unit::Create(1)); @@ -393,9 +401,10 @@ class GTSAM_EXPORT DifferentialPseudorangeFactorArm double tol = 1e-9) const override; /// vector of errors - Vector evaluateError(const Pose3& nTb, const double& receiverClockBias, + Vector evaluateError(const Pose3& ecef_T_body, + const double& receiverClockBias, const double& differentialCorrection, - OptionalMatrixType H_nTb, + OptionalMatrixType H_ecef_T_body, OptionalMatrixType HreceiverClockBias, OptionalMatrixType HdifferentialCorrection) const override; diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 18bb49ee07..9ae6b65f72 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -549,7 +549,7 @@ virtual class DifferentialPseudorangeFactor : gtsam::NonlinearFactor { }; virtual class PseudorangeFactorArm : gtsam::NonlinearFactor { - PseudorangeFactorArm(gtsam::Key nTbKey, + PseudorangeFactorArm(gtsam::Key ecefTbodyKey, gtsam::Key receiverClockBiasKey, double measuredPseudorange, const gtsam::Point3& satellitePosition, @@ -563,7 +563,7 @@ virtual class PseudorangeFactorArm : gtsam::NonlinearFactor { bool equals(const gtsam::NonlinearFactor& expected, double tol); // Standard Interface - gtsam::Vector evaluateError(const gtsam::Pose3& nTb, + gtsam::Vector evaluateError(const gtsam::Pose3& ecef_T_body, const double& receiverClockBias) const; const gtsam::Point3& leverArm() const; @@ -572,7 +572,7 @@ virtual class PseudorangeFactorArm : gtsam::NonlinearFactor { }; virtual class DifferentialPseudorangeFactorArm : gtsam::NonlinearFactor { - DifferentialPseudorangeFactorArm(gtsam::Key nTbKey, + DifferentialPseudorangeFactorArm(gtsam::Key ecefTbodyKey, gtsam::Key receiverClockBiasKey, gtsam::Key differentialCorrectionKey, double measuredPseudorange, @@ -587,7 +587,7 @@ virtual class DifferentialPseudorangeFactorArm : gtsam::NonlinearFactor { bool equals(const gtsam::NonlinearFactor& expected, double tol); // Standard Interface - gtsam::Vector evaluateError(const gtsam::Pose3& nTb, + gtsam::Vector evaluateError(const gtsam::Pose3& ecef_T_body, const double& receiverClockBias, const double& differentialCorrection) const; const gtsam::Point3& leverArm() const; diff --git a/gtsam/navigation/tests/testPseudorangeFactor.cpp b/gtsam/navigation/tests/testPseudorangeFactor.cpp index 708077a541..b8a4de4610 100644 --- a/gtsam/navigation/tests/testPseudorangeFactor.cpp +++ b/gtsam/navigation/tests/testPseudorangeFactor.cpp @@ -246,17 +246,17 @@ TEST(TestPseudorangeFactorArm, Jacobians2) { // ************************************************************************* TEST(TestPseudorangeFactorArm, LeverArmZeroError) { - // If the body position is set so that antenna_pos = body_pos + nRb * leverArm + // If the body position is set so that antenna_pos = body_pos + ecef_R_body * leverArm // gives the correct range, the error should be zero. // This mirrors the GPSFactorArm test pattern. const Point3 leverArm(0.5, -0.3, 1.0); - const Rot3 nRb = Rot3::RzRyRx(0.15, -0.30, 0.45); + const Rot3 ecef_R_body = Rot3::RzRyRx(0.15, -0.30, 0.45); const Point3 satPos(1000.0, 2000.0, 3000.0); // Choose antenna position, then back-compute body position: const Point3 antennaPos(10.0, 20.0, 30.0); - const Point3 bodyPos = antennaPos - nRb.matrix() * leverArm; - const Pose3 nTb(nRb, bodyPos); + const Point3 bodyPos = antennaPos - ecef_R_body.matrix() * leverArm; + const Pose3 ecef_T_body(ecef_R_body, bodyPos); // The true range from antenna to satellite: const double trueRange = (antennaPos - satPos).norm(); @@ -270,7 +270,7 @@ TEST(TestPseudorangeFactorArm, LeverArmZeroError) { const auto factor = PseudorangeFactorArm( Key(0), Key(1), pseudorange, satPos, leverArm, satClkBias); - const double error = factor.evaluateError(nTb, clockBias)[0]; + const double error = factor.evaluateError(ecef_T_body, clockBias)[0]; EXPECT_DOUBLES_EQUAL(0.0, error, 1e-6); } @@ -390,12 +390,12 @@ TEST(TestDifferentialPseudorangeFactorArm, Jacobians2) { // ************************************************************************* TEST(TestDifferentialPseudorangeFactorArm, LeverArmZeroError) { const Point3 leverArm(0.5, -0.3, 1.0); - const Rot3 nRb = Rot3::RzRyRx(0.15, -0.30, 0.45); + const Rot3 ecef_R_body = Rot3::RzRyRx(0.15, -0.30, 0.45); const Point3 satPos(1000.0, 2000.0, 3000.0); const Point3 antennaPos(10.0, 20.0, 30.0); - const Point3 bodyPos = antennaPos - nRb.matrix() * leverArm; - const Pose3 nTb(nRb, bodyPos); + const Point3 bodyPos = antennaPos - ecef_R_body.matrix() * leverArm; + const Pose3 ecef_T_body(ecef_R_body, bodyPos); const double trueRange = (antennaPos - satPos).norm(); const double clockBias = 1e-7; @@ -410,7 +410,7 @@ TEST(TestDifferentialPseudorangeFactorArm, LeverArmZeroError) { Key(0), Key(1), Key(2), pseudorange, satPos, leverArm, satClkBias); const double error = - factor.evaluateError(nTb, clockBias, correction)[0]; + factor.evaluateError(ecef_T_body, clockBias, correction)[0]; EXPECT_DOUBLES_EQUAL(0.0, error, 1e-6); }