diff --git a/gtsam/linear/JacobianFactor-inl.h b/gtsam/linear/JacobianFactor-inl.h index c1ef25520..aee0b7563 100644 --- a/gtsam/linear/JacobianFactor-inl.h +++ b/gtsam/linear/JacobianFactor-inl.h @@ -23,9 +23,9 @@ namespace gtsam { /* ************************************************************************* */ - template - JacobianFactor::JacobianFactor(const TERMS&terms, const Vector &b, const SharedDiagonal& model) - { + template + JacobianFactor::JacobianFactor(const TERMS& terms, const Vector& b, + const SharedDiagonal& model) { fillTerms(terms, b, model); } @@ -34,8 +34,8 @@ namespace gtsam { JacobianFactor::JacobianFactor(const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& model) - : Base(keys), Ab_(augmentedMatrix) { - checkAndAssignModel(model, augmentedMatrix); + : Base(keys), Ab_(augmentedMatrix), model_(model) { + checkAb(model, augmentedMatrix); } /* ************************************************************************* */ @@ -43,33 +43,8 @@ namespace gtsam { JacobianFactor::JacobianFactor(const KEYS& keys, VerticalBlockMatrix&& augmentedMatrix, const SharedDiagonal& model) - : Base(keys), Ab_(std::move(augmentedMatrix)) { - checkAndAssignModel(model, Ab_); - } - - /* ************************************************************************* */ - void JacobianFactor::checkAndAssignModel( - const SharedDiagonal& model, const VerticalBlockMatrix& augmentedMatrix) { - // Check noise model dimension - if (model && (DenseIndex)model->dim() != augmentedMatrix.rows()) - throw InvalidNoiseModel(augmentedMatrix.rows(), model->dim()); - - // Check number of variables - if ((DenseIndex)Base::keys_.size() != augmentedMatrix.nBlocks() - 1) - throw std::invalid_argument( - "Error in JacobianFactor constructor input. Number of provided keys " - "plus one for the RHS vector must equal the number of provided " - "matrix blocks."); - - // Check RHS dimension - if (augmentedMatrix(augmentedMatrix.nBlocks() - 1).cols() != 1) - throw std::invalid_argument( - "Error in JacobianFactor constructor input. The last provided " - "matrix block must be the RHS vector, but the last provided block " - "had more than one column."); - - // Take noise model - model_ = model; + : Base(keys), Ab_(std::move(augmentedMatrix)), model_(model) { + checkAb(model, Ab_); } /* ************************************************************************* */ diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 51d513e33..9052cc460 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -112,6 +112,28 @@ JacobianFactor::JacobianFactor(const HessianFactor& factor) } } + /* ************************************************************************* */ +void JacobianFactor::checkAb(const SharedDiagonal& model, + const VerticalBlockMatrix& augmentedMatrix) const { + // Check noise model dimension + if (model && (DenseIndex)model->dim() != augmentedMatrix.rows()) + throw InvalidNoiseModel(augmentedMatrix.rows(), model->dim()); + + // Check number of variables + if ((DenseIndex)Base::keys_.size() != augmentedMatrix.nBlocks() - 1) + throw std::invalid_argument( + "Error in JacobianFactor constructor input. Number of provided keys " + "plus one for the RHS vector must equal the number of provided " + "matrix blocks."); + + // Check RHS dimension + if (augmentedMatrix(augmentedMatrix.nBlocks() - 1).cols() != 1) + throw std::invalid_argument( + "Error in JacobianFactor constructor input. The last provided " + "matrix block must be the RHS vector, but the last provided block " + "had more than one column."); +} + /* ************************************************************************* */ // Helper functions for combine constructor namespace { diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 1e82eb051..33f7183a6 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -403,10 +403,10 @@ namespace gtsam { void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel); /// Common code between VerticalBlockMatrix constructors - void checkAndAssignModel(const SharedDiagonal& model, - const VerticalBlockMatrix& augmentedMatrix); + void checkAb(const SharedDiagonal& model, + const VerticalBlockMatrix& augmentedMatrix) const; - private: + private: /** * Helper function for public constructors: