diff --git a/gtsam/navigation/PseudorangeFactor.cpp b/gtsam/navigation/PseudorangeFactor.cpp index 2ac06cec0c..12d5fa5d55 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): @@ -134,4 +136,145 @@ Vector DifferentialPseudorangeFactor::evaluateError( return Vector1(error); } +//*************************************************************************** +PseudorangeFactorArm::PseudorangeFactorArm( + const Key ecefTbodyKey, const Key receiverClockBiasKey, + const double measuredPseudorange, const Point3& satellitePosition, + const Point3& leverArm, const double satelliteClockBias, + const SharedNoiseModel& model) + : Base(model, ecefTbodyKey, 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& ecef_T_body, const double& receiverClockBias, + OptionalMatrixType H_ecef_T_body, + OptionalMatrixType HreceiverClockBias) const { + // 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_; + const double range = position_difference.norm(); + const double rho = range + CLIGHT * (receiverClockBias - satClkBias_); + const double error = rho - pseudorange_; + + // Compute associated derivatives: + if (H_ecef_T_body) { + H_ecef_T_body->resize(1, 6); + if (range < std::numeric_limits::epsilon()) { + H_ecef_T_body->setZero(); + } else { + // u = unit vector from satellite to antenna + const Matrix u = (position_difference / range).transpose(); // 1x3 + 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; + } + } + + if (HreceiverClockBias) { + *HreceiverClockBias = I_1x1 * CLIGHT; + } + + return Vector1(error); +} + +//*************************************************************************** +DifferentialPseudorangeFactorArm::DifferentialPseudorangeFactorArm( + 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, ecefTbodyKey, 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& ecef_T_body, const double& receiverClockBias, + const double& differentialCorrection, OptionalMatrixType H_ecef_T_body, + OptionalMatrixType HreceiverClockBias, + OptionalMatrixType HdifferentialCorrection) const { + // 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_; + const double range = position_difference.norm(); + const double rho = range + CLIGHT * (receiverClockBias - satClkBias_); + const double error = rho - pseudorange_ - differentialCorrection; + + // Compute associated derivatives: + if (H_ecef_T_body) { + H_ecef_T_body->resize(1, 6); + if (range < std::numeric_limits::epsilon()) { + H_ecef_T_body->setZero(); + } else { + // u = unit vector from satellite to antenna + const Matrix u = (position_difference / range).transpose(); // 1x3 + 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; + } + } + + 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 79fb7a4f53..3c37fbb0b3 100644 --- a/gtsam/navigation/PseudorangeFactor.h +++ b/gtsam/navigation/PseudorangeFactor.h @@ -7,6 +7,7 @@ #pragma once #include +#include #include #include @@ -225,4 +226,210 @@ 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 = 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 + : 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() : PseudorangeBase{0.0, Point3(0, 0, 0), 0.0}, bL_(0, 0, 0) {} + + virtual ~PseudorangeFactorArm() = default; + + /** + * Construct a PseudorangeFactorArm. + * + * @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. + * @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 ecefTbodyKey, 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& ecef_T_body, + const double& receiverClockBias, + OptionalMatrixType H_ecef_T_body, + 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 {}; + +/** + * 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 + * + * @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 + : 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() : PseudorangeBase{0.0, Point3(0, 0, 0), 0.0}, bL_(0, 0, 0) {} + + virtual ~DifferentialPseudorangeFactorArm() = default; + + /** + * Construct a DifferentialPseudorangeFactorArm. + * + * @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. + * @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 ecefTbodyKey, 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& ecef_T_body, + const double& receiverClockBias, + const double& differentialCorrection, + OptionalMatrixType H_ecef_T_body, + 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 12209fb925..9ae6b65f72 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -548,6 +548,54 @@ virtual class DifferentialPseudorangeFactor : gtsam::NonlinearFactor { void serialize() const; }; +virtual class PseudorangeFactorArm : gtsam::NonlinearFactor { + PseudorangeFactorArm(gtsam::Key ecefTbodyKey, + 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& ecef_T_body, + const double& receiverClockBias) const; + const gtsam::Point3& leverArm() const; + + // enable serialization functionality + void serialize() const; +}; + +virtual class DifferentialPseudorangeFactorArm : gtsam::NonlinearFactor { + DifferentialPseudorangeFactorArm(gtsam::Key ecefTbodyKey, + 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& ecef_T_body, + 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 7a558534dd..b8a4de4610 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); } @@ -182,6 +182,289 @@ 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(0, 0), 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, LeverArmZeroError) { + // 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 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 - 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(); + + // 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(ecef_T_body, clockBias)[0]; + EXPECT_DOUBLES_EQUAL(0.0, error, 1e-6); +} + +// ************************************************************************* +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"); +} + +// ************************************************************************* +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(0, 0), 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 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 - 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; + 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(ecef_T_body, 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;