From 0e46b023746d91c9fed7ae53cb2ab039c25e27e9 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 22 Jun 2012 22:51:32 +0000
Subject: [PATCH] Added >> stream operator to Matrix to easily read from input
 streams

---
 gtsam/base/Matrix.cpp           | 49 +++++++++++++++++++--------------
 gtsam/base/Matrix.h             |  2 +-
 gtsam/base/tests/testMatrix.cpp | 21 ++++++++++++++
 3 files changed, 50 insertions(+), 22 deletions(-)

diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp
index e96d5a329..e11263d3b 100644
--- a/gtsam/base/Matrix.cpp
+++ b/gtsam/base/Matrix.cpp
@@ -19,6 +19,7 @@
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/timing.h>
 #include <gtsam/base/Vector.h>
+#include <gtsam/base/FastList.h>
 
 #include <gtsam/3rdparty/Eigen/Eigen/Dense>
 #include <gtsam/3rdparty/Eigen/Eigen/SVD>
@@ -236,27 +237,33 @@ void save(const Matrix& A, const string &s, const string& filename) {
 }
 
 /* ************************************************************************* */
-//istream& operator>>(istream& inputStream, Matrix& destinationMatrix) {
-//  destinationMatrix.resize(0,0);
-//  string line;
-//  bool first = true;
-//  while(getline(inputStream, line)) {
-//    // Read coefficients from file
-//    vector<double> coeffs;
-//    std::copy(istream_iterator<double>(stringstream(line)), istream_iterator<double>(), coeffs.end());
-//    if(first) {
-//      destinationMatrix.resize(1,
-//    }
-//    if(coeffs.size() != dimLatent()) {
-//      cout << "Error reading motion file, latent variable dimension does not match file" << endl;
-//      exit(1);
-//    }
-//
-//    // Copy coefficients to alignment matrix
-//    alignment_.conservativeResize(alignment_.rows() + 1, dimLatent());
-//    alignment_.row(alignment_.rows() - 1) = Eigen::Map<Vector>(&coeffs[0], dimLatent()).transpose();
-//  }
-//}
+istream& operator>>(istream& inputStream, Matrix& destinationMatrix) {
+  string line;
+  FastList<vector<double> > coeffs;
+  bool first = true;
+  size_t width = 0;
+  size_t height = 0;
+  while(getline(inputStream, line)) {
+    // Read coefficients from file
+    coeffs.push_back(vector<double>());
+    if(!first)
+      coeffs.back().reserve(width);
+    std::copy(istream_iterator<double>(stringstream(line)), istream_iterator<double>(),
+      back_insert_iterator<vector<double> >(coeffs.back()));
+    if(first)
+      width = coeffs.back().size();
+    if(coeffs.size() != width)
+      throw runtime_error("Error reading matrix from input stream, inconsistent numbers of elements in rows");
+    ++ height;
+  }
+
+  // Copy coefficients to matrix
+  destinationMatrix.resize(height, width);
+  int row = 0;
+  BOOST_FOREACH(const vector<double>& rowVec, coeffs) {
+    destinationMatrix.row(row) = Eigen::Map<const Eigen::RowVectorXd>(&rowVec[0], width);
+  }
+}
 
 /* ************************************************************************* */
 void insertSub(Matrix& fullMatrix, const Matrix& subMatrix, size_t i, size_t j) {
diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h
index adb4ee36b..2c13ee7d4 100644
--- a/gtsam/base/Matrix.h
+++ b/gtsam/base/Matrix.h
@@ -188,7 +188,7 @@ void save(const Matrix& A, const std::string &s, const std::string& filename);
  * tab-, space-, or comma-separated, similar to the format read by the MATLAB
  * dlmread command.
  */
-//istream& operator>>(istream& inputStream, Matrix& destinationMatrix);
+istream& operator>>(istream& inputStream, Matrix& destinationMatrix);
 
 /**
  * extract submatrix, slice semantics, i.e. range = [i1,i2[ excluding i2
diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp
index abd71bacc..56be42c02 100644
--- a/gtsam/base/tests/testMatrix.cpp
+++ b/gtsam/base/tests/testMatrix.cpp
@@ -17,6 +17,7 @@
  **/
 
 #include <iostream>
+#include <sstream>
 #include <CppUnitLite/TestHarness.h>
 #include <boost/tuple/tuple.hpp>
 #include <boost/foreach.hpp>
@@ -261,6 +262,26 @@ TEST( matrix, insert_sub )
 	EXPECT(assert_equal(expected, big));
 }
 
+/* ************************************************************************* */
+TEST( matrix, stream_read ) {
+  Matrix expected = Matrix_(3,4,
+    1.1, 2.3, 4.2, 7.6,
+    -0.3, -8e-2, 5.1, 9.0,
+    1.2, 3.4, 4.5, 6.7);
+
+  string matrixAsString = 
+    "1.1 2.3 4.2 7.6\n"
+    "-0.3 -8e-2 5.1    9.0\n\r" // Test extra spaces and windows newlines
+    "1.2 \t 3.4 4.5 6.7"; // Test tab as separator
+
+  stringstream asStream(matrixAsString, ios::in);
+
+  Matrix actual;
+  asStream >> actual;
+
+  EXPECT(assert_equal(expected, actual));
+}
+
 /* ************************************************************************* */
 TEST( matrix, scale_columns )
 {