From 6c4c4f150edc1cdae67a5bb379733e8db842bf3a Mon Sep 17 00:00:00 2001
From: Kai Ni <nikai97@gmail.com>
Date: Tue, 25 May 2010 08:16:04 +0000
Subject: [PATCH] solved the issue with empty rows

---
 cpp/FactorGraph-inl.h | 14 +++++++++-----
 cpp/Ordering.cpp      | 13 +++++++++++++
 cpp/Ordering.h        | 23 +++++++++++++++++++++++
 3 files changed, 45 insertions(+), 5 deletions(-)

diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h
index 9934f80ed..b99391b5c 100644
--- a/cpp/FactorGraph-inl.h
+++ b/cpp/FactorGraph-inl.h
@@ -222,19 +222,23 @@ void FactorGraph<Factor>::getOrdering(Ordering& ordering, boost::optional<const
 	// Below, we compute a symbolic matrix stored in sparse columns.
 	map<Symbol, vector<int> > columns; // map from keys to a sparse column of non-zero row indices
 	int nrNonZeros = 0;             // number of non-zero entries
-	int n_row = factors_.size();    /* colamd arg 1: number of rows in A */
+	int n_row = 0;                  /* colamd arg 1: number of rows in A */
 
 	// loop over all factors = rows
 	bool hasInterested = interested.is_initialized();
-	for (int i = 0; i < n_row; i++) {
-		if (factors_[i]==NULL) continue;
-		list<Symbol> keys = factors_[i]->keys();
+	bool inserted;
+	BOOST_FOREACH(const sharedFactor& factor, factors_) {
+		if (factor==NULL) continue;
+		list<Symbol> keys = factor->keys();
+		inserted = false;
 		BOOST_FOREACH(const Symbol& key, keys) {
 			if (!hasInterested || interested->find(key) != interested->end()) {
-				columns[key].push_back(i);
+				columns[key].push_back(n_row);
 				nrNonZeros++;
+				inserted = true;
 			}
 		}
+		if (inserted) n_row++;
 	}
 	int n_col = (int)(columns.size()); /* colamd arg 2: number of columns in A */
 	if(n_col != 0)
diff --git a/cpp/Ordering.cpp b/cpp/Ordering.cpp
index 9ba79e609..bd1adfbba 100644
--- a/cpp/Ordering.cpp
+++ b/cpp/Ordering.cpp
@@ -37,3 +37,16 @@ Ordering Ordering::subtract(const Ordering& keys) const {
 }
 
 /* ************************************************************************* */
+void Unordered::print(const string& s) const {
+  cout << s << " (" << size() << "):";
+  BOOST_FOREACH(const Symbol& key, *this)
+    cout << " " << (string)key;
+  cout << endl;
+}
+
+/* ************************************************************************* */
+bool Unordered::equals(const Unordered &other, double tol) const {
+	return *this == other;
+}
+
+/* ************************************************************************* */
diff --git a/cpp/Ordering.h b/cpp/Ordering.h
index 0fad87708..e453de0bd 100644
--- a/cpp/Ordering.h
+++ b/cpp/Ordering.h
@@ -7,6 +7,7 @@
 #pragma once
 
 #include <list>
+#include <set>
 #include <string>
 #include "Testable.h"
 #include "Key.h"
@@ -43,4 +44,26 @@ namespace gtsam {
 		Ordering subtract(const Ordering& keys) const;
 	};
 
+	/**
+	 * @class Unordered
+	 * @brief a set of unordered indice
+	 */
+	class Unordered: public std::set<Symbol>, public Testable<Unordered> {
+	public:
+		/** Default constructor creates empty ordering */
+		Unordered() { }
+
+		/** Create from a single symbol */
+		Unordered(Symbol key) { insert(key); }
+
+		/** Copy constructor */
+		Unordered(const std::set<Symbol>& keys_in) : std::set<Symbol>(keys_in) {}
+
+		/** whether a key exists */
+		bool exists(const Symbol& key) { return find(key) != end(); }
+
+		// Testable
+		void print(const std::string& s = "Unordered") const;
+		bool equals(const Unordered &t, double tol=0) const;
+	};
 }