diff --git a/cpp/Matrix.cpp b/cpp/Matrix.cpp
index 3cb7f72c0..0ece773c4 100644
--- a/cpp/Matrix.cpp
+++ b/cpp/Matrix.cpp
@@ -8,6 +8,7 @@
 #include <string.h>
 #include <iomanip>
 #include <list>
+#include <fstream>
 
 #ifdef GSL
 #include <gsl/gsl_blas.h> // needed for gsl blas
@@ -262,19 +263,26 @@ Vector row_(const Matrix& A, size_t i) {
 }
 
 /* ************************************************************************* */
-void print(const Matrix& A, const string &s) {
+void print(const Matrix& A, const string &s, ostream& stream) {
   size_t m = A.size1(), n = A.size2();
 
   // print out all elements
-  cout << s << "[\n";
+  stream << s << "[\n";
   for( size_t i = 0 ; i < m ; i++) {
     for( size_t j = 0 ; j < n ; j++) {
       double aij = A(i,j);
-      cout << setw(9) << (fabs(aij)<1e-12 ? 0 : aij) << "\t";
+      stream << setw(9) << (fabs(aij)<1e-12 ? 0 : aij) << "\t";
     }
-    cout << endl;
+    stream << endl;
   }
-  cout << "]" << endl;
+  stream << "]" << endl;
+}
+
+/* ************************************************************************* */
+void save(const Matrix& A, const string &s, const string& filename) {
+	fstream stream(filename.c_str(), fstream::out);
+	print(A, s + "=", stream);
+	stream.close();
 }
 
 /* ************************************************************************* */
diff --git a/cpp/Matrix.h b/cpp/Matrix.h
index ce796cf77..a21559dbe 100644
--- a/cpp/Matrix.h
+++ b/cpp/Matrix.h
@@ -135,7 +135,12 @@ Vector Vector_(const Matrix& A);
 /**
  * print a matrix
  */
-void print(const Matrix& A, const std::string& s = "");
+void print(const Matrix& A, const std::string& s = "", std::ostream& stream = std::cout);
+
+/**
+ * save a matrix to file, which can be loaded by matlab
+ */
+void save(const Matrix& A, const std::string &s, const std::string& filename);
 
 /**
  * extract submatrix, slice semantics, i.e. range = [i1,i2[ excluding i2
diff --git a/cpp/Vector.cpp b/cpp/Vector.cpp
index 89b1ed767..98364ef18 100644
--- a/cpp/Vector.cpp
+++ b/cpp/Vector.cpp
@@ -8,6 +8,7 @@
 #include <stdarg.h>
 #include <limits>
 #include <iostream>
+#include <fstream>
 #include <sstream>
 #include <iomanip>
 #include <cmath>
@@ -35,13 +36,13 @@ namespace ublas = boost::numeric::ublas;
 
 namespace gtsam {
   
-  void odprintf(const char *format, ...)
-  {
+  /* ************************************************************************* */
+  void odprintf_(const char *format, ostream& stream, ...) {
     char    buf[4096], *p = buf;
     int     n;
     
     va_list args;
-    va_start(args, format);
+    va_start(args, stream);
     #ifdef WIN32
     n = _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
     #else
@@ -52,11 +53,33 @@ namespace gtsam {
     #ifdef WIN32
     OutputDebugString(buf);
     #else    
+    stream << buf;
+    #endif
+  }
+
+  /* ************************************************************************* */
+  // copy and paste from above, as two functions can not be easily merged
+  void odprintf(const char *format, ...)
+  {
+    char    buf[4096], *p = buf;
+    int     n;
+
+    va_list args;
+    va_start(args, format);
+    #ifdef WIN32
+    n = _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
+    #else
+    n = vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
+    #endif
+    va_end(args);
+
+    #ifdef WIN32
+    OutputDebugString(buf);
+    #else
     cout << buf;
     #endif
-    
   }
-  
+
   /* ************************************************************************* */
   Vector Vector_( size_t m, const double* const data) {
     Vector v(m);
@@ -100,14 +123,21 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  void print(const Vector& v, const string& s) {
+  void print(const Vector& v, const string& s, ostream& stream) {
     size_t n = v.size();
-    odprintf("%s [", s.c_str());
+    odprintf_("%s [", stream, s.c_str());
     for(size_t i=0; i<n; i++)
-      odprintf("%g%s", v[i], (i<n-1 ? "; " : ""));
-    odprintf("]\n");
+      odprintf_("%g%s", stream, v[i], (i<n-1 ? "; " : ""));
+    odprintf_("]\n", stream);
   }
   
+  /* ************************************************************************* */
+  void save(const Vector& v, const string &s, const string& filename) {
+  	fstream stream(filename.c_str(), fstream::out);
+  	print(v, s + "=", stream);
+  	stream.close();
+  }
+
   /* ************************************************************************* */
   bool operator==(const Vector& vec1,const Vector& vec2) {
     Vector::const_iterator it1 = vec1.begin();
diff --git a/cpp/Vector.h b/cpp/Vector.h
index a06d573c1..8b6282a6b 100644
--- a/cpp/Vector.h
+++ b/cpp/Vector.h
@@ -91,7 +91,12 @@ inline size_t dim(const Vector& v) { return v.size(); }
 /**
  * print with optional string
  */
-void print(const Vector& v, const std::string& s = "");
+void print(const Vector& v, const std::string& s = "", std::ostream& stream = std::cout);
+
+/**
+ * save a vector to file, which can be loaded by matlab
+ */
+void save(const Vector& A, const std::string &s, const std::string& filename);
 
 /**
  * operator==()