remove column singletons in the factor graph

cpp/BearingRangeFactor.h

@@ -21,20 +21,18 @@ namespace gtsam {
 	PointKey, Point2> {
 	private:
 
-		// the bearing factor
+		// the measurement
-		BearingFactor<Config, PoseKey, PointKey> bearing_;
+		Rot2 bearing_;
-
+		double range_;
-		// the range factor
-		RangeFactor<Config, PoseKey, PointKey> range_;
 
 		typedef NonlinearFactor2<Config, PoseKey, Pose2, PointKey, Point2> Base;
 
 	public:
 
 		BearingRangeFactor(); /* Default constructor */
-		BearingRangeFactor(const PoseKey& i, const PointKey& j, const std::pair<Rot2, double>& z,
+		BearingRangeFactor(const PoseKey& i, const PointKey& j, const Rot2& bearing, const double range,
 				const SharedGaussian& model) :
-					Base(model, i, j), bearing_(i, j, z.first, model), range_(i, j, z.second, model) {
+					Base(model, i, j), bearing_(bearing), range_(range) {
 		}
 
 		/** h(x)-z -> between(z,h(x)) for Rot2 manifold */
@@ -45,24 +43,22 @@ namespace gtsam {
 			boost::optional<Matrix&> H21_ = H1 ? boost::optional<Matrix&>(H21) : boost::optional<Matrix&>();
 			boost::optional<Matrix&> H12_ = H2 ? boost::optional<Matrix&>(H12) : boost::optional<Matrix&>();
 			boost::optional<Matrix&> H22_ = H2 ? boost::optional<Matrix&>(H22) : boost::optional<Matrix&>();
-			Vector e1 = bearing_.evaluateError(pose, point, H11_, H12_);
+
-			Vector e2 =   range_.evaluateError(pose, point, H21_, H22_);
+			Rot2 y1 = gtsam::bearing(pose, point, H11_, H12_);
-			if (H1) *H1 = stack_matrices(H11, H21);
+			Vector e1 = logmap(between(bearing_, y1));
-			if (H2) *H2 = stack_matrices(H12, H22);
+
+			double y2 = gtsam::range(pose, point, H21_, H22_);
+			Vector e2 = Vector_(1, y2 - range_);
+
+			if (H1) *H1 = gtsam::stack(2, &H11, &H21);
+			if (H2) *H2 = gtsam::stack(2, &H12, &H22);
 			return concatVectors(2, &e1, &e2);
 		}
 
 		/** return the measured */
 		inline const std::pair<Rot2, double> measured() const {
-			return concatVectors(2, bearing_.measured(), range_.measured());
+			return std::make_pair(bearing_, range_);
 		}
-
-		/** return the bearing factor */
-		const BearingFactor<Config, PoseKey, PointKey>& bearing() const { return bearing_; }
-
-		/** return the range factor */
-		const RangeFactor<Config, PoseKey, PointKey>& range() const { return range_; }
-
 	}; // BearingRangeFactor
 
 } // namespace gtsam

cpp/FactorGraph-inl.h

@@ -134,6 +134,41 @@ Ordering FactorGraph<Factor>::keys() const {
 	return keys;
 }
 
+/* ************************************************************************* */
+template<class Factor>
+std::pair<FactorGraph<Factor>, set<Symbol> > FactorGraph<Factor>::removeSingletons() {
+	FactorGraph<Factor> singletonGraph;
+	set<Symbol> singletons;
+
+	while(true) {
+		// find all the singleton variables
+		Ordering new_singletons;
+		Symbol key;
+		list<int> indices;
+		BOOST_FOREACH(boost::tie(key, indices), indices_) {
+			// find out the number of factors associated with the current key
+			int numValidFactors = 0;
+			BOOST_FOREACH(const int& i, indices)
+				if (factors_[i]!=NULL) numValidFactors++;
+
+			if (numValidFactors == 1) {
+				new_singletons.push_back(key);
+				BOOST_FOREACH(const int& i, indices)
+					if (factors_[i]!=NULL) singletonGraph.push_back(factors_[i]);
+			}
+		}
+		singletons.insert(new_singletons.begin(), new_singletons.end());
+
+		BOOST_FOREACH(const Symbol& singleton, new_singletons)
+			findAndRemoveFactors(singleton);
+
+		// exit when there are no more singletons
+		if (new_singletons.empty()) break;
+	}
+
+	return make_pair(singletonGraph, singletons);
+}
+
 /* ************************************************************************* */
 /**
  * Call colamd given a column-major symbolic matrix A
@@ -248,6 +283,9 @@ FactorGraph<Factor>::findAndRemoveFactors(const Symbol& key) {
 		found.push_back(fi);     // add to found
 		fi.reset();              // set factor to NULL == remove(i)
 	}
+
+	indices_.erase(key);
+
 	return found;
 }
 

cpp/FactorGraph.h

@@ -97,6 +97,11 @@ namespace gtsam {
 			return !(indices_.find(key)==indices_.end());
 		}
 
+		/** check whether a variable is a singleton, i.e. it only involve*/
+
+		/** remove singleton variables and the related factors */
+		std::pair<FactorGraph<Factor>, std::set<Symbol> > removeSingletons();
+
 		/**
 		 * Compute colamd ordering, including I/O and shared pointer version
 		 */

cpp/Matrix.cpp

@@ -742,11 +742,6 @@ Matrix stack(size_t nrMatrices, ...)
   return A;
 }
 
-/* ************************************************************************* */
-Matrix stack_matrices(const Matrix& A, const Matrix& B) {
-	return stack(2, &A, &B);
-}
-
 /* ************************************************************************* */
 Matrix collect(const std::vector<const Matrix *>& matrices, size_t m, size_t n)
 {

cpp/Matrix.h

@@ -287,9 +287,6 @@ Vector backSubstituteLower(const Matrix& L, const Vector& d, bool unit=false);
  */
 Matrix stack(size_t nrMatrices, ...);
 
-/** a shortcut to prevent the name confliction with STL stack */
-Matrix stack_matrices(const Matrix& A, const Matrix& B);
-
 /**
  * create a matrix by concatenating
  * Given a set of matrices: A1, A2, A3...

cpp/testFactorgraph.cpp

@@ -9,6 +9,9 @@
 #include <iostream>
 #include <boost/shared_ptr.hpp>
 #include <boost/tuple/tuple.hpp>
+#include <boost/assign/std/set.hpp> // for operator +=
+using namespace boost::assign;
+
 #include <CppUnitLite/TestHarness.h>
 
 #define GTSAM_MAGIC_KEY
@@ -44,6 +47,41 @@ TEST( FactorGraph, splitMinimumSpanningTree )
 	CHECK(assert_equal(expectedC,C));
 }
 
+/* ************************************************************************* */
+/**
+ *  x1 - x2 - x3 - x4 - x5
+ *       |    |  / |
+ *       l1   l2   l3
+ */
+TEST( FactorGraph, removeSingletons )
+{
+	SymbolicFactorGraph G;
+	G.push_factor("x1", "x2");
+	G.push_factor("x2", "x3");
+	G.push_factor("x3", "x4");
+	G.push_factor("x4", "x5");
+	G.push_factor("x2", "l1");
+	G.push_factor("x3", "l2");
+	G.push_factor("x4", "l2");
+	G.push_factor("x4", "l3");
+
+	SymbolicFactorGraph singletonGraph;
+	set<Symbol> singletons;
+	boost::tie(singletonGraph, singletons) = G.removeSingletons();
+
+	set<Symbol> singletons_excepted; singletons_excepted += "x1", "x2", "x5", "l1", "l3";
+	CHECK(singletons_excepted == singletons);
+
+	SymbolicFactorGraph singletonGraph_excepted;
+	singletonGraph_excepted.push_factor("x2", "l1");
+	singletonGraph_excepted.push_factor("x4", "l3");
+	singletonGraph_excepted.push_factor("x1", "x2");
+	singletonGraph_excepted.push_factor("x4", "x5");
+	singletonGraph_excepted.push_factor("x2", "x3");
+	CHECK(singletonGraph_excepted.equals(singletonGraph));
+}
+
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;

cpp/testPlanarSLAM.cpp

@@ -61,7 +61,7 @@ TEST( planarSLAM, BearingRangeFactor )
 	// Create factor
 	Rot2 r = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1
 	double b(sqrt(2) - 0.22); // h(x) - z = 0.22
-	planarSLAM::BearingRange factor(2, 3, make_pair(r,b), sigma2);
+	planarSLAM::BearingRange factor(2, 3, r, b, sigma2);
 
 	// create config
 	planarSLAM::Config c;