From Hmf6_unordered: getISAM2 and variables Reeliminated/Relinearized (d97dcacf186900d12003c399e7fe55e03ca34e3f)

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;

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<ConcurrentIncrementalFilter> 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