Collecting more statistics in isam2, logmap for configs

cpp/ISAM2-inl.h

@@ -201,8 +201,14 @@ namespace gtsam {
 			// todo - remerge in keys of new factors
 			affectedKeys.insert(newKeys.begin(), newKeys.end());
 
+			// Save number of affected variables
+			lastAffectedVariableCount = affectedKeys.size();
+
 			factors = relinearizeAffectedFactors(affectedKeys);
 
+			// Save number of affected factors
+			lastAffectedFactorCount = factors.size();
+
 			// add the cached intermediate results from the boundary of the orphans ...
 			FactorGraph<GaussianFactor> cachedBoundary = getCachedBoundaryFactors(orphans);
 			factors.push_back(cachedBoundary);
@@ -229,6 +235,9 @@ namespace gtsam {
 		for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
 			this->insert(*rit, index);
 
+		// Save number of affectedCliques
+		lastAffectedCliqueCount = this->size();
+
 		// add orphans to the bottom of the new tree
 		BOOST_FOREACH(sharedClique orphan, orphans) {
 			Symbol parentRepresentative = findParentClique(orphan->separator_, index);

cpp/ISAM2.h

@@ -81,7 +81,17 @@ namespace gtsam {
 		// estimate based on full delta (note that this is based on the actual current linearization point)
 		const Config calculateBestEstimate() const {return expmap(theta_, optimize2(*this, 0.));}
 
-		const std::list<Symbol>& getMarked() const { return marked_; }
+		const std::list<Symbol>& getMarkedUnsafe() const { return marked_; }
+
+		const NonlinearFactorGraph<Config>& getFactorsUnsafe() const { return nonlinearFactors_; }
+
+		const Config& getThetaUnsafe() const { return theta_; }
+
+		const VectorConfig& getDeltaUnsafe() const { return delta_; }
+
+		size_t lastAffectedVariableCount;
+		size_t lastAffectedFactorCount;
+		size_t lastAffectedCliqueCount;
 
 	private:
 

cpp/LieConfig-inl.h

@@ -20,6 +20,7 @@
   /*INSTANTIATE_LIE(T);*/ \
   template LieConfig<J,T> expmap(const LieConfig<J,T>&, const VectorConfig&); \
   template LieConfig<J,T> expmap(const LieConfig<J,T>&, const Vector&); \
+  template VectorConfig logmap(const LieConfig<J,T>&, const LieConfig<J,T>&); \
   template class LieConfig<J,T>;
 
 using namespace std;
@@ -107,6 +108,20 @@ namespace gtsam {
     }
     return newConfig;
   }
+
+  // todo: insert for every element is inefficient
+  // todo: currently only logmaps elements in both configs
+  template<class J, class T>
+  VectorConfig logmap(const LieConfig<J,T>& c0, const LieConfig<J,T>& cp) {
+  	VectorConfig delta;
+  	typedef pair<J,T> Value;
+  	BOOST_FOREACH(const Value& value, cp) {
+  		if(c0.exists(value.first))
+  			delta.insert(value.first,
+  					logmap(c0[value.first], value.second));
+  	}
+  	return delta;
+  }
 }
 
 

cpp/LieConfig.h

@@ -115,5 +115,9 @@ namespace gtsam {
   /** Add a delta vector, uses iterator order */
   template<class J, class T>
   LieConfig<J,T> expmap(const LieConfig<J,T>& c, const Vector& delta);
+
+  /** Get a delta config about a linearization point c0 */
+  template<class J, class T>
+  VectorConfig logmap(const LieConfig<J,T>& c0, const LieConfig<J,T>& cp);
 }
 

cpp/TupleConfig.h

@@ -6,6 +6,7 @@
  */
 
 #include "LieConfig.h"
+#include "VectorConfig.h"
 
 #pragma once
 
@@ -98,9 +99,18 @@ namespace gtsam {
     /**
      * expmap each element
      */
-    PairConfig<J1,X1,J2,X2> expmap(const VectorConfig& delta) {
+    PairConfig<J1,X1,J2,X2> expmap(const VectorConfig& delta) const {
       return PairConfig(gtsam::expmap(first, delta), gtsam::expmap(second, delta)); }
 
+    /**
+     * logmap each element
+     */
+    VectorConfig logmap(const PairConfig<J1,X1,J2,X2>& cp) const {
+    	VectorConfig ret(gtsam::logmap(first, cp.first));
+    	ret.insert(gtsam::logmap(second, cp.second));
+    	return ret;
+    }
+
     /**
      * Insert a variable with the given j
      */
@@ -126,7 +136,9 @@ namespace gtsam {
   };
 
   template<class J1, class X1, class J2, class X2>
-  inline PairConfig<J1,X1,J2,X2> expmap(PairConfig<J1,X1,J2,X2> c, const VectorConfig& delta) { return c.expmap(delta); }
+  inline PairConfig<J1,X1,J2,X2> expmap(const PairConfig<J1,X1,J2,X2> c, const VectorConfig& delta) { return c.expmap(delta); }
 
+  template<class J1, class X1, class J2, class X2>
+  inline VectorConfig logmap(const PairConfig<J1,X1,J2,X2> c0, const PairConfig<J1,X1,J2,X2>& cp) { return c0.logmap(cp); }
 
 }