Incorporating reviewer comments, and tidying tests.

Author: Sammy Guo

gtsam/navigation/PseudorangeFactor.cpp

@@ -33,14 +33,10 @@ PseudorangeFactor::PseudorangeFactor(Key receiverPositionKey,
 //***************************************************************************
 void PseudorangeFactor::print(const std::string& s,
                               const KeyFormatter& keyFormatter) const {
-  std::cout << (s.empty() ? "" : s + " ") << "PseudorangeFactor on "
-            << keyFormatter(this->key<1>()) << ", "
-            << keyFormatter(this->key<2>()) << "\n";
-  std::cout << "  Pseudorange: " << pseudorange_ << " meters\n";
-  std::cout << "  Satellite Position: " << satPos_.transpose()
-            << " meters (ECEF)\n";
-  std::cout << "  Satellite clock bias: " << satClkBias_ << " seconds\n";
-  noiseModel_->print("  noise model: ");
+  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/navigation/PseudorangeFactor.h

@@ -41,10 +41,10 @@ class GTSAM_EXPORT PseudorangeFactor
  private:
   typedef NoiseModelFactorN<Point3, double> Base;
 
-  const double
-      pseudorange_;  ///< Receiver-reported pseudorange measurement in meters.
-  const Point3 satPos_;      ///< Satellite position in WGS84 ECEF meters.
-  const double satClkBias_;  ///< Satellite clock bias in seconds.
+  double
+      pseudorange_;    ///< Receiver-reported pseudorange measurement in meters.
+  Point3 satPos_;      ///< Satellite position in WGS84 ECEF meters.
+  double satClkBias_;  ///< Satellite clock bias in seconds.
 
  public:
   // Provide access to the Matrix& version of evaluateError:
@@ -72,11 +72,11 @@ class GTSAM_EXPORT PseudorangeFactor
    * @param satelliteClockBias Satellite clock bias in seconds.
    * @param model 1-D noise model.
    */
-  PseudorangeFactor(Key receiverPositionKey, Key receiverClockBiasKey,
-                    double measuredPseudorange, const Point3& satellitePosition,
-                    double satelliteClockBias,
-                    const SharedNoiseModel& model =
-                        noiseModel::Diagonal::Sigmas(Vector1(1.0)));
+  PseudorangeFactor(
+      Key receiverPositionKey, Key receiverClockBiasKey,
+      double measuredPseudorange, const Point3& satellitePosition,
+      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 {
@@ -112,4 +112,8 @@ class GTSAM_EXPORT PseudorangeFactor
 #endif
 };
 
+/// traits
+template <>
+struct traits<PseudorangeFactor> : public Testable<PseudorangeFactor> {};
+
 }  // namespace gtsam

gtsam/navigation/navigation.i

@@ -488,20 +488,22 @@ virtual class GPSFactor2ArmCalib : gtsam::NonlinearFactor{
 #include <gtsam/navigation/PseudorangeFactor.h>
 virtual class PseudorangeFactor : gtsam::NonlinearFactor {
   PseudorangeFactor(gtsam::Key receiverPositionKey,
-                    gtsam::Key receiverClockBiasKey,
-                    double measuredPseudorange,
+                    gtsam::Key receiverClockBiasKey, double measuredPseudorange,
                     const gtsam::Point3& satellitePosition,
                     double satelliteClockBias,
                     const gtsam::noiseModel::Base* model);
 
   // Testable
   void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
-                                                         gtsam::DefaultKeyFormatter) const;
+                                gtsam::DefaultKeyFormatter) const;
   bool equals(const gtsam::NonlinearFactor& expected, double tol);
 
   // Standard Interface
   gtsam::Vector evaluateError(const gtsam::Point3& receiverPosition,
                               const double& receiverClock_bias) const;
+
+  // enable serialization functionality
+  void serialize() const;
 };
 
 #include <gtsam/navigation/BarometricFactor.h>

gtsam/navigation/tests/testPseudorangeFactor.cpp

@@ -24,16 +24,39 @@ TEST(TestPseudorangeFactor, Constructor) {
       factor.evaluateError(Point3::Zero(), 0.0, Hpos, Hbias)[0];
   EXPECT_DOUBLES_EQUAL(0.0, error, 1e-9);
 
-  // Jacobians are technically undefined if the receiver and satellite positions
-  // are the same. So make sure this corner-case does not numerically explode:
-  CHECK(!Hpos.array().isNaN().any());
-  CHECK(!Hbias.array().isNaN().any());
+  // Derivatives are technically undefined if the receiver and satellite
+  // positions are the same (hopefully that's never the case in reality). But
+  // for all intents and purposes, zero-valued derivatives can substitute for
+  // undefined gradient at that singularity. So make sure this corner-case does
+  // not numerically explode:
+  EXPECT(!Hpos.array().isNaN().any());
+  EXPECT(!Hbias.array().isNaN().any());
   EXPECT_DOUBLES_EQUAL(Hpos.norm(), 0.0, 1e-9);
-  // Clock bias should always be speed-of-light in vacuum:
+  // Clock bias derivative should always be speed-of-light in vacuum:
   EXPECT_DOUBLES_EQUAL(Hbias.norm(), 299792458.0, 1e-9);
 }
 
-TEST(TestPseudorangeFactor, Jacobians) {
+// *************************************************************************
+TEST(TestPseudorangeFactor, Jacobians1) {
+  // Synthetic example with exact error/derivatives:
+  const auto factor = PseudorangeFactor(
+      Key(0), Key(1),  // Receiver position and clock bias keys.
+      4.0,             // Measured pseudorange.
+      // Satellite position:
+      Vector3(0.0, 0.0, 3.0),
+      0.0  // Sat clock drift bias.
+  );
+  const double error = factor.evaluateError(Vector3::Zero(), 0.0)[0];
+  EXPECT_DOUBLES_EQUAL(-1.0, error, 1e-6);
+
+  Values values;
+  values.insert(Key(0), Vector3(1.0, 2.0, 3.0));
+  values.insert(Key(1), 0.0);
+  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-3, 1e-5);
+}
+
+// *************************************************************************
+TEST(TestPseudorangeFactor, Jacobians2) {
   // Example values borrowed from `SinglePointPositioningExample.ipynb`:
   const auto factor = PseudorangeFactor(
       Key(0), Key(1),  // Receiver position and clock bias keys.
@@ -50,6 +73,30 @@ TEST(TestPseudorangeFactor, Jacobians) {
   EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-3, 1e-5);
 }
 
+// *************************************************************************
+TEST(TestPseudorangeFactor, print) {
+  // Just make sure `print()` doesn't throw errors
+  // since there's no elegant way to check stdout.
+  const auto factor = PseudorangeFactor();
+  factor.print();
+}
+
+// *************************************************************************
+TEST(TestPseudorangeFactor, equals) {
+  const auto factor1 = PseudorangeFactor();
+  const auto factor2 = PseudorangeFactor(1, 2, 0.0, Point3::Zero(), 0.0);
+  const auto factor3 =
+      PseudorangeFactor(1, 2, 10.0, Point3(1.0, 2.0, 3.0), 20.0);
+
+  CHECK(factor1.equals(factor1));
+  CHECK(factor2.equals(factor2));
+  CHECK(!factor1.equals(factor2));
+  CHECK(factor2.equals(factor3, 1e99));
+
+  // Test print:
+  factor2.print("factor2");
+}
+
 // *************************************************************************
 int main() {
   TestResult tr;

python/gtsam/tests/test_PseudorangeFactor.py

@@ -16,10 +16,41 @@
 from gtsam.utils.test_case import GtsamTestCase
 
 
-class TestPriorFactor(GtsamTestCase):
-
-    def test_PseudorangeFactor(self):
-        self.assertEqual(1, 0)
+class TestPseudorangeFactor(GtsamTestCase):
+    def test_singularity(self):
+        pos = np.zeros(3)
+        values = gtsam.Values()
+        values.insert(0, pos)
+        values.insert(1, 0.0)
+
+        model = gtsam.noiseModel.Unit.Create(1)
+        factor = gtsam.PseudorangeFactor(0, 1, 0.0, pos, 0.0, model)
+        self.assertEqual(factor.error(values), 0)
+        # test print:
+        factor.print("factor")
+
+    def test_errors(self):
+        satpos = np.array([0.0, 0.0, 3.0])
+        model = gtsam.noiseModel.Unit.Create(1)
+        factor = gtsam.PseudorangeFactor(0, 1, 4.0, satpos, 0.0, model)
+        error = factor.evaluateError(np.zeros(3), 0.0)
+        self.assertEqual(error[0], -1.0)
+
+    def test_equality(self):
+        satpos = np.array([0.0, 0.0, 3.0])
+        model = gtsam.noiseModel.Unit.Create(1)
+        factor1 = gtsam.PseudorangeFactor(0, 1, 4.0, satpos, 0.0, model)
+        factor2 = gtsam.PseudorangeFactor(2, 1, 4.0, satpos, 0.0, model)
+        factor3 = gtsam.PseudorangeFactor(0, 1, 40.0, satpos, 10.0, model)
+        self.assertTrue(factor1.equals(factor1, 1e-6))
+        self.assertFalse(factor1.equals(factor2, 1e-6))
+        self.assertTrue(factor1.equals(factor3, 1e99))
+
+    def test_serialization(self):
+        satpos = np.array([0.0, 0.0, 3.0])
+        model = gtsam.noiseModel.Unit.Create(1)
+        factor = gtsam.PseudorangeFactor(0, 1, 4.0, satpos, 0.0, model)
+        factor.serialize()
 
 
 if __name__ == "__main__":