diff --git a/gtsam/slam/JacobianSchurFactor.h b/gtsam/slam/JacobianSchurFactor.h index d31eea05b..e7f99a736 100644 --- a/gtsam/slam/JacobianSchurFactor.h +++ b/gtsam/slam/JacobianSchurFactor.h @@ -23,7 +23,9 @@ namespace gtsam { /** - * JacobianFactor for Schur complement that uses Q noise model + * JacobianFactor for Schur complement + * Is base class for JacobianQFactor, JacobianFactorQR, and JacobianFactorSVD + * Provides raw memory access versions of linear operator. */ template class JacobianSchurFactor: public JacobianFactor { @@ -44,10 +46,11 @@ public: */ Vector operator*(const double* x) const { Vector Ax = zero(Ab_.rows()); - if (empty()) return Ax; + if (empty()) + return Ax; // Just iterate over all A matrices and multiply in correct config part - for(size_t pos=0; poswhiten(Ax) : Ax; @@ -57,17 +60,17 @@ public: * @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e * RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way */ - void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const - { + void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const { Vector E = alpha * (model_ ? model_->whiten(e) : e); // Just iterate over all A matrices and insert Ai^e into y - for(size_t pos=0; postransposeMultiplyAdd(alpha,Ax,y); - if (empty()) return; + if (empty()) + return; Vector Ax = zero(Ab_.rows()); // Just iterate over all A matrices and multiply in correct config part - for(size_t pos=0; poswhitenInPlace(Ax); model_->whitenInPlace(Ax); } + if (model_) { + model_->whitenInPlace(Ax); + model_->whitenInPlace(Ax); + } // multiply with alpha Ax *= alpha; // Again iterate over all A matrices and insert Ai^e into y - for(size_t pos=0; pos