diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index d519557ead..27f2aa9245 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2091,8 +2091,14 @@ class NonlinearFactorGraph { gtsam::KeySet keys() const; gtsam::KeyVector keyVector() const; - template, gtsam::imuBias::ConstantBias}> - void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); + template , + gtsam::imuBias::ConstantBias}> + void addPrior(size_t key, const T& prior, + const gtsam::noiseModel::Base* noiseModel); // NonlinearFactorGraph void printErrors(const gtsam::Values& values) const; @@ -2561,7 +2567,26 @@ class NonlinearISAM { #include #include -template}> +template }> virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; @@ -2573,7 +2598,6 @@ virtual class PriorFactor : gtsam::NoiseModelFactor { void pickle() const; }; - #include template virtual class BetweenFactor : gtsam::NoiseModelFactor { @@ -3194,6 +3218,9 @@ class NavState { gtsam::Point3 position() const; Vector velocity() const; gtsam::Pose3 pose() const; + + gtsam::NavState retract(const Vector& x) const; + Vector localCoordinates(const gtsam::NavState& g) const; }; #include