Fixed sparse bug

cpp/GaussianFactorGraph.cpp

@@ -249,12 +249,13 @@ Dimensions GaussianFactorGraph::columnIndices(const Ordering& ordering) const {
 		result.insert(make_pair(key,j));
 		// find dimension for this key
 		Dimensions::const_iterator it = variableSet.find(key);
+		if (it==variableSet.end()) // key not found, now what ?
+				throw invalid_argument("ColumnIndices: this ordering contains keys not in the graph");
 		// advance column index to next block by adding dim(key)
 		j += it->second;
 	}
 
-	return result;
+	return result;}
-}
 
 /* ************************************************************************* */
 Matrix GaussianFactorGraph::sparse(const Ordering& ordering) const {
@@ -266,6 +267,16 @@ Matrix GaussianFactorGraph::sparse(const Ordering& ordering) const {
 	// get the starting column indices for all variables
 	Dimensions indices = columnIndices(ordering);
 
+	return sparse(indices);
+}
+
+/* ************************************************************************* */
+Matrix GaussianFactorGraph::sparse(const Dimensions& indices) const {
+
+	// return values
+	list<int> I,J;
+	list<double> S;
+
 	// Collect the I,J,S lists for all factors
 	int row_index = 0;
 	BOOST_FOREACH(sharedFactor factor,factors_) {

cpp/GaussianFactorGraph.h

@@ -197,6 +197,12 @@ namespace gtsam {
   	 */
   	Matrix sparse(const Ordering& ordering) const;
 
+  	/**
+  	 * Version that takes column indices rather than ordering
+  	 */
+  	Matrix sparse(const Dimensions& indices) const;
+
+
   	/**
   	 * Take an optimal step in direction d by calculating optimal step-size
   	 * @param x: starting point for search

cpp/Pose2SLAMOptimizer.h

@@ -92,6 +92,8 @@ namespace gtsam {
 		Matrix a2() const { return system_->A2(*solver_.ordering()); }
 		Vector b1() const { return system_->b1(); }
 		Vector b2() const { return system_->b2(); }
+		std::pair<Matrix,Vector> Ab1() const { return system_->Ab1(*solver_.ordering()); }
+		std::pair<Matrix,Vector> Ab2() const { return system_->Ab2(*solver_.ordering()); }
 
 		/**
 		 * update estimate with pure delta config x

cpp/SubgraphPreconditioner.h

@@ -44,6 +44,8 @@ namespace gtsam {
 		 */
 		SubgraphPreconditioner(sharedFG& Ab1, sharedFG& Ab2, sharedBayesNet& Rc1,	sharedConfig& xbar);
 
+		std::pair<Matrix,Vector> Ab1(const Ordering& ordering) const { return Ab1_->matrix(ordering); }
+		std::pair<Matrix,Vector> Ab2(const Ordering& ordering) const { return Ab2_->matrix(ordering); }
 		Matrix A1(const Ordering& ordering) const { return Ab1_->sparse(ordering); }
 		Matrix A2(const Ordering& ordering) const { return Ab2_->sparse(ordering); }
 		Vector b1() const { return Ab1_->rhsVector(); }

cpp/testPose2SLAM.cpp

@@ -162,9 +162,19 @@ TEST(Pose2Graph, optimizeCircle) {
   // Check loop closure
   CHECK(assert_equal(delta,between(actual[5],actual[0])));
 
-//  Pose2SLAMOptimizer myOptimizer("3");
+  Pose2SLAMOptimizer myOptimizer("3");
-//  myOptimizer.linearize();
+  myOptimizer.linearize();
+
+//  Matrix A1;Vector b1;
+//  boost::tie(A1,b1)=myOptimizer.Ab1();
+//  print(A1,"A1");
+//  print(b1,"b1");
 //
+//  Matrix A2;Vector b2;
+//  boost::tie(A2,b2)=myOptimizer.Ab2();
+//  print(A2,"A2");
+//  print(b2,"b2");
+
 //  Matrix A1 = myOptimizer.a1();
 //  LONGS_EQUAL(3,  A1.size1());
 //  LONGS_EQUAL(17, A1.size2()); // 7 + 7 + 3
@@ -178,37 +188,42 @@ TEST(Pose2Graph, optimizeCircle) {
 //
 //  Vector b2 = myOptimizer.b2();
 //  LONGS_EQUAL(3, b2.size()); // 3
-//
+
-//  // Here, call matlab to
+  // Here, call matlab to
-//  // A=[A1;A2], b=[b1;b2]
+  // A=[A1;A2], b=[b1;b2]
-//  // R=qr(A1)
+  // R=qr(A1)
-//  // call pcg on A,b, with preconditioner R -> get x
+  // call pcg on A,b, with preconditioner R -> get x
-//
+
-//  Vector x = myOptimizer.optimize();
+  Vector x = myOptimizer.optimize();
-//  LONGS_EQUAL(9, x.size()); // 3 + 3 + 3
+  LONGS_EQUAL(9, x.size()); // 3 + 3 + 3
-//
+
-//  myOptimizer.update(x);
+  myOptimizer.update(x);
-//
+
-//  Pose2Config expected;
+  Pose2Config expected;
-//  expected.insert(0, Pose2(0.,0.,0.));
+  expected.insert(0, Pose2(0.,0.,0.));
-//  expected.insert(1, Pose2(1.,0.,0.));
+  expected.insert(1, Pose2(1.,0.,0.));
-//  expected.insert(2, Pose2(2.,0.,0.));
+  expected.insert(2, Pose2(2.,0.,0.));
-//
+
-//  // Check with ground truth
+  // Check with ground truth
-//  CHECK(assert_equal(expected, *myOptimizer.theta()));
+  CHECK(assert_equal(expected, *myOptimizer.theta()));
 }
 
+/* ************************************************************************* */
 TEST(Pose2Graph, optimize2) {
-//  Pose2SLAMOptimizer myOptimizer("100");
+  Pose2SLAMOptimizer myOptimizer("100");
-//
+  Matrix A1 = myOptimizer.a1();
-//  //cout << "error: " << myOptimizer.error() << endl;
+  Matrix A2 = myOptimizer.a2();
-//  for(int i = 0; i<10; i++) {
+  cout << "A1: " << A1.size1() << " " << A1.size2() << endl;
-//  	myOptimizer.linearize();
+  cout << "A2: " << A2.size1() << " " << A2.size2() << endl;
-//  	Vector x = myOptimizer.optimize();
+
-//  	myOptimizer.update(x);
+  //cout << "error: " << myOptimizer.error() << endl;
-//  }
+  for(int i = 0; i<10; i++) {
-//  //cout << "error: " << myOptimizer.error() << endl;
+  	myOptimizer.linearize();
-//  CHECK(myOptimizer.error() < 1.);
+  	Vector x = myOptimizer.optimize();
+  	myOptimizer.update(x);
+  }
+  //cout << "error: " << myOptimizer.error() << endl;
+  CHECK(myOptimizer.error() < 1.);
 }
 
 /* ************************************************************************* */