From b4e65e963126b78a830eb6eeda580a7dfbc98f2c Mon Sep 17 00:00:00 2001
From: Michael Kaess <michaelkaess@gmail.com>
Date: Thu, 31 Dec 2009 05:35:08 +0000
Subject: [PATCH] progress in ISAM2, but unit test still disabled

---
 cpp/BayesTree.h           |  3 +-
 cpp/ISAM2-inl.h           | 73 ++++++++++++++++++++++++++++++++++-----
 cpp/testGaussianISAM2.cpp |  4 +--
 3 files changed, 68 insertions(+), 12 deletions(-)

diff --git a/cpp/BayesTree.h b/cpp/BayesTree.h
index 349e889b6..2e52689f7 100644
--- a/cpp/BayesTree.h
+++ b/cpp/BayesTree.h
@@ -186,8 +186,7 @@ namespace gtsam {
 		 * Used for incrementally updating a BayesTree given new measurements (factors).
 		 */
 		template<class Factor>
-		std::pair<FactorGraph<Factor>, Cliques>
-		removeTop(const FactorGraph<Factor>& newFactors);
+		std::pair<FactorGraph<Factor>, Cliques> removeTop(const FactorGraph<Factor>& newFactors);
 
 	}; // BayesTree
 
diff --git a/cpp/ISAM2-inl.h b/cpp/ISAM2-inl.h
index 726b23dc9..9debb7027 100644
--- a/cpp/ISAM2-inl.h
+++ b/cpp/ISAM2-inl.h
@@ -26,12 +26,14 @@ namespace gtsam {
 
 	/** Create a Bayes Tree from a nonlinear factor graph */
 	template<class Conditional, class Config>
-	ISAM2<Conditional, Config>::ISAM2(const NonlinearFactorGraph<Config>& nlfg, const Ordering& ordering, const Config& config) {
-
-		BayesTree<Conditional>(nlfg.linearize(config).eliminate(ordering));
-
-		nonlinearFactors_ = nlfg;
-		config_ = config;
+	ISAM2<Conditional, Config>::ISAM2(const NonlinearFactorGraph<Config>& nlfg, const Ordering& ordering, const Config& config)
+	: BayesTree<Conditional>(nlfg.linearize(config).eliminate(ordering)), nonlinearFactors_(nlfg), config_(config) {
+		// todo - debug only
+		printf("constructor keys:\n");
+		BOOST_FOREACH(string s, nonlinearFactors_.keys()) {
+			printf("%s ", s.c_str());
+		}
+		printf("\n");
 	}
 
 	/* ************************************************************************* */
@@ -55,16 +57,71 @@ namespace gtsam {
 
 		// find the corresponding original nonlinear factors, and relinearize them
 		NonlinearFactorGraph<Config> nonlinearAffectedFactors;
+#if 0
+		// simply wrong................................................................
 		list<string> keys = affectedFactors.keys();
 		for (list<string>::iterator keyIt = keys.begin(); keyIt!=keys.end(); keyIt++) {
+			// affected factors in original factor graph
 			list<int> indices = nonlinearFactors_.factors(*keyIt);
 			for (list<int>::iterator indIt = indices.begin(); indIt!=indices.end(); indIt++) {
-// todo - do we need to check if it already exists? probably...				if (*indIt)
-					nonlinearAffectedFactors.push_back(nonlinearFactors_[*indIt]);
+				// only add factors that have not already been added
+				bool alreadyAdded = false;
+				typename NonlinearFactorGraph<Config>::iterator it;
+				for (it = nonlinearAffectedFactors.begin(); it!=nonlinearAffectedFactors.end(); it++) {
+					if (*it == nonlinearFactors_[*indIt]) alreadyAdded = true;
+				}
+				if (!alreadyAdded) nonlinearAffectedFactors.push_back(nonlinearFactors_[*indIt]);
 			}
 		}
+#else
+		BOOST_FOREACH(FactorGraph<GaussianFactor>::sharedFactor fac, affectedFactors) {
+			printf("XX\n");
+			// retrieve correspondent factor from nonlinearFactors_
+			Ordering keys = fac->keys();
+			list<int> indices = nonlinearFactors_.factors(keys.front());
+			BOOST_FOREACH(int idx, indices) {
+				BOOST_FOREACH(string s, nonlinearFactors_[idx]->keys()) {
+					printf("%s ", s.c_str());
+				}
+				printf(" - versus - ");
+				BOOST_FOREACH(string s, keys) {
+					printf("%s ", s.c_str());
+				}
+				printf("\n");
+				printf("nonlinFac\n");
+				nonlinearFactors_[idx]->print();
+				printf("fac\n");
+				fac->print();
+				// todo: for some reason, nonlinearFactors returns variables in reverse order...
+				Ordering other_keys = nonlinearFactors_[idx]->keys();
+				other_keys.reverse();
+				if (keys.equals(other_keys)) {
+					// todo: can there be duplicates? they would be added multiple times then
+					printf("YY\n");
+					nonlinearAffectedFactors.push_back(nonlinearFactors_[idx]);
+				}
+			}
+		}
+#endif
 		FactorGraph<GaussianFactor> factors = nonlinearAffectedFactors.linearize(config_);
 
+		// todo - debug - test:
+		if (factors.equals(affectedFactors)) {
+			printf("factors equal\n");
+		} else {
+			FactorGraph<GaussianFactor> all = nonlinearFactors_.linearize(config_);
+			printf("=====ALL\n");
+			all.print();
+
+			printf("=====ACTUAL\n");
+			factors.print();
+			printf("=====EXPECTED\n");
+			affectedFactors.print();
+			printf("=====ORPHANS\n");
+			orphans.print();
+			printf("factors NOT equal\n"); exit(1);
+		}
+
 		// add the new factors themselves
 		factors.push_back(newFactorsLinearized);
 
diff --git a/cpp/testGaussianISAM2.cpp b/cpp/testGaussianISAM2.cpp
index 063b29531..3d6f2e4d1 100644
--- a/cpp/testGaussianISAM2.cpp
+++ b/cpp/testGaussianISAM2.cpp
@@ -52,7 +52,7 @@ TEST( ISAM2, ISAM2_smoother )
 	CHECK(assert_equal(e, optimized));
 }
 
-/* ************************************************************************* */
+/* ************************************************************************* *
 TEST( ISAM2, ISAM2_smoother2 )
 {
 	// Create smoother with 7 nodes
@@ -76,7 +76,7 @@ TEST( ISAM2, ISAM2_smoother2 )
 	for (int t = 1; t <= 7; t++) ordering += symbol('x', t);
 	GaussianISAM2 expected(smoother, ordering, poses);
 
-//	CHECK(assert_equal(expected, actual)); // todo: actual is wrong...
+	CHECK(assert_equal(expected, actual));
 }
 
 /* ************************************************************************* */