fixed bearing/range, large speedup for batch; incremental creation of Config works

cpp/ISAM2-inl.h

@@ -119,16 +119,8 @@ namespace gtsam {
 	void ISAM2<Conditional, Config>::update_internal(const NonlinearFactorGraph<Config>& newFactors,
 			const Config& config, Cliques& orphans) {
 
-#if 0 // todo - temporarily disabled --------------------------------------------------------------------------------------------------
-		// copy variables into config_, but don't overwrite existing entries (current linearization point!)
-		for (typename Config::const_iterator it = config.begin(); it!=config.end(); it++) {
-			if (!linPoint_.contains(it->first)) {
-				linPoint_.insert(it->first, it->second);
-			}
-		}
-#else
-		linPoint_ = config;
-#endif
+		// add new variables
+		linPoint_.insert(config);
 
 		FactorGraph<GaussianFactor> newFactorsLinearized = newFactors.linearize(linPoint_);
 

cpp/ISAM2.h

@@ -65,9 +65,11 @@ namespace gtsam {
 		void update_internal(const NonlinearFactorGraph<Config>& newFactors, const Config& config, Cliques& orphans);
 		void update(const NonlinearFactorGraph<Config>& newFactors, const Config& config);
 
-		const Config estimate() {return expmap(linPoint_, delta_);}
+		const Config estimate() const {return expmap(linPoint_, delta_);}
+		const Config linearizationPoint() const {return linPoint_;}
 
 	private:
+
 		FactorGraph<GaussianFactor> relinearizeAffectedFactors(const std::list<Symbol>& affectedKeys);
 		FactorGraph<GaussianFactor> getCachedBoundaryFactors(Cliques& orphans);
 

cpp/TupleConfig-inl.h

@@ -25,4 +25,14 @@ namespace gtsam {
     second.print(s + "Config2: ");
   }
 
+  template<class J1, class X1, class J2, class X2>
+  void PairConfig<J1,X1,J2,X2>::insert(const PairConfig& config) {
+  	for (typename Config1::const_iterator it = config.first.begin(); it!=config.first.end(); it++) {
+  		insert(it->first, it->second);
+  	}
+  	for (typename Config2::const_iterator it = config.second.begin(); it!=config.second.end(); it++) {
+  		insert(it->first, it->second);
+  	}
+  }
+
 }

cpp/TupleConfig.h

@@ -107,6 +107,8 @@ namespace gtsam {
     void insert(const J1& j, const X1& value) { insert_helper(first, j, value); }
     void insert(const J2& j, const X2& value) { insert_helper(second, j, value); }
 
+    void insert(const PairConfig& config);
+
     /**
      * Remove the variable with the given j.  Throws invalid_argument if the
      * j is not present in the config.

cpp/VectorConfig.cpp

@@ -41,6 +41,13 @@ VectorConfig& VectorConfig::insert(const Symbol& name, const Vector& val) {
   return *this;
 }
 
+/* ************************************************************************* */
+void VectorConfig::insert(const VectorConfig& config) {
+	for (const_iterator it = config.begin(); it!=config.end(); it++) {
+		insert(it->first, it->second);
+	}
+}
+
 /* ************************************************************************* */
 void VectorConfig::add(const Symbol& j, const Vector& a) {
 	Vector& vj = values[j];

cpp/VectorConfig.h

@@ -41,6 +41,9 @@ namespace gtsam {
     /** Insert a value into the configuration with a given index */
     VectorConfig& insert(const Symbol& name, const Vector& val);
 
+    /** Insert a config into another config */
+    void insert(const VectorConfig& config);
+
     /** Add to vector at position j */
     void add(const Symbol& j, const Vector& a);