diff --git a/cpp/NoiseModel.cpp b/cpp/NoiseModel.cpp index a6e28efe0..bcbe26866 100644 --- a/cpp/NoiseModel.cpp +++ b/cpp/NoiseModel.cpp @@ -6,12 +6,20 @@ * Author: Frank Dellaert */ -//#include - #include "NoiseModel.h" +namespace ublas = boost::numeric::ublas; +typedef ublas::matrix_column column; + namespace gtsam { + Matrix GaussianNoiseModel::whiten(const Matrix& H) { + size_t n = H.size2(), m = H.size1(); + Matrix G(m,n); + for(int j=0;j