diff --git a/cpp/Matrix.h b/cpp/Matrix.h index e79f7c93a..dcded7637 100644 --- a/cpp/Matrix.h +++ b/cpp/Matrix.h @@ -115,8 +115,9 @@ inline Vector operator*(const Vector & v, const Matrix& A) { * overload * for matrix multiplication (as BOOST does not) */ inline Matrix operator*(const Matrix& A, const Matrix& B) { - if (A.size2()!=B.size1()) throw std::invalid_argument( - boost::str(boost::format("Matrix operator* : A.n(%d)!=B.m(%d)") % A.size2() % B.size1())); + // richard: boost already does this check in debug mode I think +// if (A.size2()!=B.size1()) throw std::invalid_argument( +// boost::str(boost::format("Matrix operator* : A.n(%d)!=B.m(%d)") % A.size2() % B.size1())); return prod(A,B); } diff --git a/cpp/NoiseModel.cpp b/cpp/NoiseModel.cpp index 3733ee352..5fee1f3e6 100644 --- a/cpp/NoiseModel.cpp +++ b/cpp/NoiseModel.cpp @@ -47,24 +47,26 @@ namespace gtsam { // functional Matrix Gaussian::Whiten(const Matrix& H) const { - size_t m = H.size1(), n = H.size2(); - Matrix W(m, n); - for (int j = 0; j < n; j++) { - Vector wj = whiten(column(H, j)); - for (int i = 0; i < m; i++) - W(i, j) = wj(i); - } - return W; +// size_t m = H.size1(), n = H.size2(); +// Matrix W(m, n); +// for (int j = 0; j < n; j++) { +// Vector wj = whiten(column(H, j)); +// for (int i = 0; i < m; i++) +// W(i, j) = wj(i); +// } +// return W; + return sqrt_information_ * H; } // in place void Gaussian::WhitenInPlace(Matrix& H) const { - size_t m = H.size1(), n = H.size2(); - for (int j = 0; j < n; j++) { - Vector wj = whiten(column(H, j)); - for (int i = 0; i < m; i++) - H(i, j) = wj(i); - } +// size_t m = H.size1(), n = H.size2(); +// for (int j = 0; j < n; j++) { +// Vector wj = whiten(column(H, j)); +// for (int i = 0; i < m; i++) +// H(i, j) = wj(i); +// } + H = sqrt_information_ * H; } /* ************************************************************************* */