From e6e93475cfc75e6742e7db083649e515305f888e Mon Sep 17 00:00:00 2001
From: Kai Ni <nikai97@gmail.com>
Date: Wed, 26 May 2010 08:05:31 +0000
Subject: [PATCH] added sizeOfA

---
 cpp/GaussianFactorGraph.cpp     | 11 +++++++++++
 cpp/GaussianFactorGraph.h       |  5 +++++
 cpp/Matrix.cpp                  |  4 ++--
 cpp/Vector.cpp                  |  4 ++--
 cpp/testGaussianFactorGraph.cpp | 11 +++++++++++
 5 files changed, 31 insertions(+), 4 deletions(-)

diff --git a/cpp/GaussianFactorGraph.cpp b/cpp/GaussianFactorGraph.cpp
index c6e34ec59..31d376dbe 100644
--- a/cpp/GaussianFactorGraph.cpp
+++ b/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 {
 
diff --git a/cpp/GaussianFactorGraph.h b/cpp/GaussianFactorGraph.h
index 36f19d499..1f271f19e 100644
--- a/cpp/GaussianFactorGraph.h
+++ b/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.
diff --git a/cpp/Matrix.cpp b/cpp/Matrix.cpp
index 5ccb9f4eb..b295ae6a8 100644
--- a/cpp/Matrix.cpp
+++ b/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();
 }
diff --git a/cpp/Vector.cpp b/cpp/Vector.cpp
index 2bcb6262c..f6975aefc 100644
--- a/cpp/Vector.cpp
+++ b/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();
   }
diff --git a/cpp/testGaussianFactorGraph.cpp b/cpp/testGaussianFactorGraph.cpp
index d5e9e2e79..889bc291c 100644
--- a/cpp/testGaussianFactorGraph.cpp
+++ b/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 )
 {