diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index ceeab3b35..7300f87ba 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -329,7 +329,8 @@ virtual class Pose3AttitudeFactor : gtsam::NoiseModelFactor { #include virtual class GPSFactor : gtsam::NonlinearFactor{ GPSFactor(size_t key, const gtsam::Point3& gpsIn, - const gtsam::noiseModel::Base* model); + const gtsam::noiseModel::Base* model, + const gtsam::Point3& leverArm); // Testable void print(string s = "", const gtsam::KeyFormatter& keyFormatter = @@ -338,6 +339,7 @@ virtual class GPSFactor : gtsam::NonlinearFactor{ // Standard Interface gtsam::Point3 measurementIn() const; + gtsam::Point3 leverArm() const; // enable serialization functionality void serialize() const; @@ -345,7 +347,8 @@ virtual class GPSFactor : gtsam::NonlinearFactor{ virtual class GPSFactor2 : gtsam::NonlinearFactor { GPSFactor2(size_t key, const gtsam::Point3& gpsIn, - const gtsam::noiseModel::Base* model); + const gtsam::noiseModel::Base* model, + const gtsam::Point3& leverArm); // Testable void print(string s = "", const gtsam::KeyFormatter& keyFormatter = @@ -354,6 +357,7 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor { // Standard Interface gtsam::Point3 measurementIn() const; + gtsam::Point3 leverArm() const; // enable serialization functionality void serialize() const;