added sizeOfA

cpp/GaussianFactorGraph.cpp

@@ -345,6 +345,17 @@ Dimensions GaussianFactorGraph::columnIndices(const Ordering& ordering) const {
 	return result;
 }
 
+/* ************************************************************************* */
+pair<size_t, size_t> GaussianFactorGraph::sizeOfA() const {
+	size_t m = 0, n = 0;
+	Dimensions variableSet = dimensions();
+	BOOST_FOREACH(const Dimensions::value_type value, variableSet)
+		n += value.second;
+	BOOST_FOREACH(const sharedFactor& factor,factors_)
+		m += factor->numberOfRows();
+	return make_pair(m, n);
+}
+
 /* ************************************************************************* */
 Matrix GaussianFactorGraph::sparse(const Ordering& ordering) const {
 

cpp/GaussianFactorGraph.h

@@ -211,6 +211,11 @@ namespace gtsam {
      */
     Dimensions columnIndices(const Ordering& ordering) const;
 
+    /**
+     * return the size of corresponding A matrix
+     */
+    std::pair<std::size_t, std::size_t> sizeOfA() const;
+
   	/**
   	 * Return 3*nzmax matrix where the rows correspond to the vectors i, j, and s
   	 * to generate an m-by-n sparse matrix, which can be given to MATLAB's sparse function.

cpp/Matrix.cpp

@@ -317,12 +317,12 @@ void print(const Matrix& A, const string &s, ostream& stream) {
     }
     stream << endl;
   }
-  stream << "]" << endl;
+  stream << "];" << endl;
 }
 
 /* ************************************************************************* */
 void save(const Matrix& A, const string &s, const string& filename) {
-	fstream stream(filename.c_str(), fstream::out);
+	fstream stream(filename.c_str(), fstream::out | fstream::app);
 	print(A, s + "=", stream);
 	stream.close();
 }

cpp/Vector.cpp

@@ -128,12 +128,12 @@ namespace gtsam {
     odprintf_("%s [", stream, s.c_str());
     for(size_t i=0; i<n; i++)
       odprintf_("%g%s", stream, v[i], (i<n-1 ? "; " : ""));
-    odprintf_("]\n", stream);
+    odprintf_("];\n", stream);
   }
   
   /* ************************************************************************* */
   void save(const Vector& v, const string &s, const string& filename) {
-  	fstream stream(filename.c_str(), fstream::out);
+  	fstream stream(filename.c_str(), fstream::out | fstream::app);
   	print(v, s + "=", stream);
   	stream.close();
   }

cpp/testGaussianFactorGraph.cpp

@@ -383,6 +383,17 @@ TEST( GaussianFactorGraph, matrix )
   CHECK(b==b1);
 }
 
+/* ************************************************************************* */
+TEST( GaussianFactorGraph, sizeOfA )
+{
+	// create a small linear factor graph
+	GaussianFactorGraph fg = createGaussianFactorGraph();
+
+  pair<size_t, size_t> mn = fg.sizeOfA();
+  CHECK(8 == mn.first);
+  CHECK(6 == mn.second);
+}
+
 /* ************************************************************************* */
 TEST( GaussianFactorGraph, sparse )
 {