diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h
index 69e841e57..4ef963f5d 100644
--- a/gtsam/base/FastSet.h
+++ b/gtsam/base/FastSet.h
@@ -19,15 +19,14 @@
 #pragma once
 
 #include <gtsam/base/FastDefaultAllocator.h>
-
-#include <set>
-#include <iostream>
-#include <string>
-#include <cmath>
 #include <boost/mpl/has_xxx.hpp>
 #include <boost/utility/enable_if.hpp>
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/set.hpp>
+#include <set>
+#include <iostream>
+#include <string>
+#include <cmath>
 
 BOOST_MPL_HAS_XXX_TRAIT_DEF(print)
 
diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h
index c67f7dd61..dd0811d8b 100644
--- a/gtsam/base/GenericValue.h
+++ b/gtsam/base/GenericValue.h
@@ -26,7 +26,7 @@
 #include <boost/pool/pool_alloc.hpp>
 
 #include <cmath>
-#include <iostream>
+#include <iosfwd>
 #include <typeinfo> // operator typeid
 
 namespace gtsam {
diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp
index 7bcd32b9f..7fa0992ca 100644
--- a/gtsam/base/Matrix.cpp
+++ b/gtsam/base/Matrix.cpp
@@ -20,6 +20,8 @@
 #include <gtsam/base/timing.h>
 #include <gtsam/base/Vector.h>
 #include <gtsam/base/FastList.h>
+#include <gtsam/3rdparty/Eigen/Eigen/SVD>  
+#include <gtsam/3rdparty/Eigen/Eigen/LU>
 
 #include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
@@ -180,6 +182,7 @@ void transposeMultiplyAdd(double alpha, const Matrix& A, const Vector& e, SubVec
 }
 
 /* ************************************************************************* */
+//3 argument call
 void print(const Matrix& A, const string &s, ostream& stream) {
   size_t m = A.rows(), n = A.cols();
 
@@ -198,6 +201,12 @@ void print(const Matrix& A, const string &s, ostream& stream) {
   stream << "];" << endl;
 }
 
+/* ************************************************************************* */
+//1 or 2 argument call
+void print(const Matrix& A, const string &s){
+  print(A, s, cout);
+}
+
 /* ************************************************************************* */
 void save(const Matrix& A, const string &s, const string& filename) {
   fstream stream(filename.c_str(), fstream::out | fstream::app);
@@ -697,6 +706,19 @@ std::string formatMatrixIndented(const std::string& label, const Matrix& matrix,
   return ss.str();
 }
 
+void inplace_QR(Matrix& A){
+  size_t rows = A.rows();
+  size_t cols = A.cols();
+  size_t size = std::min(rows,cols);
 
+  typedef Eigen::internal::plain_diag_type<Matrix>::type HCoeffsType;
+  typedef Eigen::internal::plain_row_type<Matrix>::type RowVectorType;
+  HCoeffsType hCoeffs(size);
+  RowVectorType temp(cols);
+
+  Eigen::internal::householder_qr_inplace_blocked<Matrix, HCoeffsType>::run(A, hCoeffs, 48, temp.data());
+
+  zeroBelowDiagonal(A);
+}
 
 } // namespace gtsam
diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h
index c3cbfa341..27b7ec8f7 100644
--- a/gtsam/base/Matrix.h
+++ b/gtsam/base/Matrix.h
@@ -21,11 +21,14 @@
 // \callgraph
 
 #pragma once
-
 #include <gtsam/base/Vector.h>
+#include <boost/math/special_functions/fpclassify.hpp>
+#include <gtsam/3rdparty/Eigen/Eigen/Core>
+#include <gtsam/3rdparty/Eigen/Eigen/Cholesky>
+#include <gtsam/3rdparty/Eigen/Eigen/LU>
 #include <boost/format.hpp>
 #include <boost/tuple/tuple.hpp>
-#include <boost/math/special_functions/fpclassify.hpp>
+
 
 /**
  * Matrix is a typedef in the gtsam namespace
@@ -198,9 +201,14 @@ inline MATRIX prod(const MATRIX& A, const MATRIX&B) {
 }
 
 /**
- * print a matrix
+ * print without optional string, must specify cout yourself
  */
-GTSAM_EXPORT void print(const Matrix& A, const std::string& s = "", std::ostream& stream = std::cout);
+GTSAM_EXPORT void print(const Matrix& A, const std::string& s, std::ostream& stream);
+
+/**
+ * print with optional string to cout
+ */
+GTSAM_EXPORT void print(const Matrix& A, const std::string& s = "");
 
 /**
  * save a matrix to file, which can be loaded by matlab
@@ -367,21 +375,7 @@ GTSAM_EXPORT std::pair<Matrix,Matrix> qr(const Matrix& A);
  * @param A is the input matrix, and is the output
  * @param clear_below_diagonal enables zeroing out below diagonal
  */
-template <class MATRIX>
-void inplace_QR(MATRIX& A) {
-  size_t rows = A.rows();
-  size_t cols = A.cols();
-  size_t size = std::min(rows,cols);
-
-  typedef Eigen::internal::plain_diag_type<Matrix>::type HCoeffsType;
-  typedef Eigen::internal::plain_row_type<Matrix>::type RowVectorType;
-  HCoeffsType hCoeffs(size);
-  RowVectorType temp(cols);
-
-  Eigen::internal::householder_qr_inplace_blocked<MATRIX, HCoeffsType>::run(A, hCoeffs, 48, temp.data());
-
-  zeroBelowDiagonal(A);
-}
+void inplace_QR(Matrix& A);
 
 /**
  * Imperative algorithm for in-place full elimination with
diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h
index a12f453f4..70677cad1 100644
--- a/gtsam/base/Value.h
+++ b/gtsam/base/Value.h
@@ -18,9 +18,10 @@
 
 #pragma once
 
-#include <memory>
-#include <boost/serialization/assume_abstract.hpp>
 #include <gtsam/base/Vector.h>
+#include <boost/serialization/assume_abstract.hpp>
+#include <memory>
+
 
 namespace gtsam {
 
diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp
index a9ef8fe10..0a708427a 100644
--- a/gtsam/base/Vector.cpp
+++ b/gtsam/base/Vector.cpp
@@ -16,20 +16,18 @@
  * @author  Frank Dellaert
  */
 
+#include <gtsam/base/Vector.h>
+#include <boost/foreach.hpp>
+#include <boost/optional.hpp>
+#include <stdexcept>
 #include <cstdarg>
 #include <limits>
-#include <iostream>
 #include <fstream>
 #include <sstream>
 #include <iomanip>
 #include <cmath>
-#include <stdexcept>
-#include <boost/foreach.hpp>
-#include <boost/optional.hpp>
 #include <cstdio>
-
-#include <gtsam/base/Vector.h>
-
+#include <vector>
 
 using namespace std;
 
@@ -55,6 +53,7 @@ Vector delta(size_t n, size_t i, double value) {
 }
 
 /* ************************************************************************* */
+//3 argument call
 void print(const Vector& v, const string& s, ostream& stream) {
   size_t n = v.size();
 
@@ -65,6 +64,12 @@ void print(const Vector& v, const string& s, ostream& stream) {
   stream << "];" << endl;
 }
 
+/* ************************************************************************* */
+//1 or 2 argument call
+void print(const Vector& v, const string& s) {
+    print(v, s, cout);
+}
+
 /* ************************************************************************* */
 void save(const Vector& v, const string &s, const string& filename) {
   fstream stream(filename.c_str(), fstream::out | fstream::app);
diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h
index 74cd248a1..2d333848b 100644
--- a/gtsam/base/Vector.h
+++ b/gtsam/base/Vector.h
@@ -18,13 +18,12 @@
 
 // \callgraph
 
-#pragma once
 
-#include <list>
-#include <vector>
-#include <iostream>
+#pragma once
 #include <gtsam/global_includes.h>
-#include <gtsam/3rdparty/gtsam_eigen_includes.h>
+#include <gtsam/3rdparty/Eigen/Eigen/Core>
+#include <iosfwd>
+#include <list>
 
 namespace gtsam {
 
@@ -97,9 +96,14 @@ GTSAM_EXPORT bool zero(const Vector& v);
 inline size_t dim(const Vector& v) { return v.size(); }
 
 /**
- * print with optional string
+ * print without optional string, must specify cout yourself
  */
-GTSAM_EXPORT void print(const Vector& v, const std::string& s = "", std::ostream& stream = std::cout);
+GTSAM_EXPORT void print(const Vector& v, const std::string& s, std::ostream& stream);
+
+/**
+ * print with optional string to cout
+ */
+GTSAM_EXPORT void print(const Vector& v, const std::string& s = "");
 
 /**
  * save a vector to file, which can be loaded by matlab
diff --git a/gtsam/base/types.cpp b/gtsam/base/types.cpp
index 3f86dc0c1..03e7fd120 100644
--- a/gtsam/base/types.cpp
+++ b/gtsam/base/types.cpp
@@ -17,10 +17,9 @@
  * @addtogroup base
  */
 
-#include <boost/lexical_cast.hpp>
-
 #include <gtsam/base/types.h>
 #include <gtsam/inference/Symbol.h>
+#include <boost/lexical_cast.hpp>
 
 namespace gtsam {
 
diff --git a/gtsam/base/types.h b/gtsam/base/types.h
index aca04a14b..a66db13c8 100644
--- a/gtsam/base/types.h
+++ b/gtsam/base/types.h
@@ -21,18 +21,18 @@
 
 #include <gtsam/dllexport.h>
 #include <gtsam/config.h>
-
-#include <cstddef>
-#include <string>
-#include <iostream>
 #include <boost/function/function1.hpp>
 #include <boost/range/concepts.hpp>
 #include <boost/optional.hpp>
+#include <cstddef>
+#include <string>
+#include <ostream>
 
 #ifdef GTSAM_USE_TBB
 #include <tbb/task_scheduler_init.h>
 #include <tbb/tbb_exception.h>
 #include <tbb/scalable_allocator.h>
+#include <iostream>
 #endif
 
 #ifdef GTSAM_USE_EIGEN_MKL_OPENMP
diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h
index 606f62f87..9ebcbcf5c 100644
--- a/gtsam/geometry/EssentialMatrix.h
+++ b/gtsam/geometry/EssentialMatrix.h
@@ -10,7 +10,7 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Unit3.h>
 #include <gtsam/geometry/Point2.h>
-#include <iostream>
+#include <iosfwd>
 
 namespace gtsam {
 
diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h
index 73d0711be..ad727f45e 100644
--- a/gtsam/nonlinear/Values.h
+++ b/gtsam/nonlinear/Values.h
@@ -26,14 +26,9 @@
 
 #include <gtsam/base/GenericValue.h>
 #include <gtsam/base/VectorSpace.h>
-#include <gtsam/base/FastMap.h>
 #include <gtsam/inference/Key.h>
-
-#include <boost/optional.hpp>
-#include <boost/ptr_container/ptr_map.hpp>
 #include <boost/iterator/transform_iterator.hpp>
 #include <boost/iterator/filter_iterator.hpp>
-#include <boost/function.hpp>
 #ifdef __GNUC__
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wunused-variable"
@@ -43,7 +38,6 @@
 #pragma GCC diagnostic pop
 #endif
 #include <boost/ptr_container/serialize_ptr_map.hpp>
-#include <boost/iterator_adaptors.hpp>
 
 #include <string>
 #include <utility>