diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 52f5901ee..fedb28226 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -314,6 +314,14 @@ class DSFMapIndexPair { gtsam::IndexPairSetMap sets(); }; +#include +template +class FastList {}; + +#include +template +class FastMap {}; + #include bool linear_independent(Matrix A, Matrix B, double tol); @@ -2567,9 +2575,10 @@ class ISAM2 { gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices); gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys); - // TODO: wrap the full version of update - //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap& constrainedKeys); - //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap& constrainedKeys, bool force_relinearize); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, gtsam::FastMap& constrainedKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, gtsam::FastMap& constrainedKeys, const gtsam::FastList& noRelinKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, gtsam::FastMap& constrainedKeys, const gtsam::FastList& noRelinKeys, const gtsam::FastList& extraReelimKeys); + gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, gtsam::FastMap& constrainedKeys, const gtsam::FastList& noRelinKeys, const gtsam::FastList& extraReelimKeys, bool force_relinearize); gtsam::Values getLinearizationPoint() const; gtsam::Values calculateEstimate() const; @@ -2701,8 +2710,7 @@ class BearingRange { BearingRange(const BEARING& b, const RANGE& r); BEARING bearing() const; RANGE range() const; - // TODO(frank): can't class instance itself? - // static gtsam::BearingRange Measure(const POSE& pose, const POINT& point); + static gtsam::BearingRange Measure(const POSE& pose, const POINT& point); static BEARING MeasureBearing(const POSE& pose, const POINT& point); static RANGE MeasureRange(const POSE& pose, const POINT& point); void print(string s) const;