diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp index d7c6e4ecf..9742aefd7 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.cpp @@ -152,6 +152,8 @@ ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilter::update(const No result.linearVariables = isam2_.getFixedVariables().size(); result.nonlinearVariables = isam2_.getLinearizationPoint().size() - result.linearVariables; result.newFactorsIndices = isam2Result.newFactorsIndices; + result.variablesReeliminated = isam2Result.variablesReeliminated; + result.variablesRelinearized = isam2Result.variablesRelinearized; // result.error = isam2_.getFactorsUnsafe().error(isam2_.calculateEstimate()); if(debug) std::cout << "ConcurrentIncrementalFilter::update End" << std::endl; diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h index c534c843e..4f2e1b0aa 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h @@ -31,7 +31,6 @@ class GTSAM_UNSTABLE_EXPORT ConcurrentIncrementalFilter : public virtual Concurr public: - ISAM2 isam2_; ///< The iSAM2 inference engine typedef boost::shared_ptr shared_ptr; typedef ConcurrentFilter Base; ///< typedef for base class @@ -40,6 +39,8 @@ public: size_t iterations; ///< The number of optimizer iterations performed size_t nonlinearVariables; ///< The number of variables that can be relinearized size_t linearVariables; ///< The number of variables that must keep a constant linearization point + size_t variablesReeliminated; + size_t variablesRelinearized; /** The indices of the newly-added factors, in 1-to-1 correspondence with the * factors passed as \c newFactors update(). These indices may be @@ -76,6 +77,11 @@ public: return isam2_.getFactorsUnsafe(); } + /** Access the current linearization point */ + const ISAM2& getISAM2() const { + return isam2_; + } + /** Access the current linearization point */ const Values& getLinearizationPoint() const { return isam2_.getLinearizationPoint(); @@ -160,7 +166,7 @@ public: protected: -// ISAM2 isam2_; ///< The iSAM2 inference engine + ISAM2 isam2_; ///< The iSAM2 inference engine // ??? NonlinearFactorGraph previousSmootherSummarization_; ///< The smoother summarization on the old separator sent by the smoother during the last synchronization