diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index bdb51701e..e2a582d9b 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -86,6 +86,7 @@ namespace gtsam { size_t dims[] = { 1 }; Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+1, b_in.size())); getb() = b_in; + model_ = noiseModel::Unit::Create(this->rows()); assertInvariants(); } @@ -93,6 +94,10 @@ namespace gtsam { JacobianFactor::JacobianFactor(Index i1, const Matrix& A1, const Vector& b, const SharedDiagonal& model) : GaussianFactor(i1), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) { + + if(model->dim() != b.size()) + throw InvalidNoiseModel(b.size(), model->dim()); + size_t dims[] = { A1.cols(), 1}; Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+2, b.size())); Ab_(0) = A1; @@ -104,6 +109,10 @@ namespace gtsam { JacobianFactor::JacobianFactor(Index i1, const Matrix& A1, Index i2, const Matrix& A2, const Vector& b, const SharedDiagonal& model) : GaussianFactor(i1,i2), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) { + + if(model->dim() != b.size()) + throw InvalidNoiseModel(b.size(), model->dim()); + size_t dims[] = { A1.cols(), A2.cols(), 1}; Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+3, b.size())); Ab_(0) = A1; @@ -116,6 +125,10 @@ namespace gtsam { JacobianFactor::JacobianFactor(Index i1, const Matrix& A1, Index i2, const Matrix& A2, Index i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model) : GaussianFactor(i1,i2,i3), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) { + + if(model->dim() != b.size()) + throw InvalidNoiseModel(b.size(), model->dim()); + size_t dims[] = { A1.cols(), A2.cols(), A3.cols(), 1}; Ab_.copyStructureFrom(BlockAb(matrix_, dims, dims+4, b.size())); Ab_(0) = A1; @@ -131,6 +144,10 @@ namespace gtsam { GaussianFactor(GetKeys(terms.size(), terms.begin(), terms.end())), model_(model), firstNonzeroBlocks_(b.size(), 0), Ab_(matrix_) { + + if(model->dim() != b.size()) + throw InvalidNoiseModel(b.size(), model->dim()); + size_t* dims = (size_t*)alloca(sizeof(size_t)*(terms.size()+1)); // FIXME: alloca is bad, just ask Google. for(size_t j=0; jdim() != b.size()) + throw InvalidNoiseModel(b.size(), model->dim()); + size_t* dims=(size_t*)alloca(sizeof(size_t)*(terms.size()+1)); // FIXME: alloca is bad, just ask Google. size_t j=0; std::list >::const_iterator term=terms.begin(); @@ -542,10 +563,23 @@ namespace gtsam { /* ************************************************************************* */ void JacobianFactor::setModel(bool anyConstrained, const Vector& sigmas) { + if(sigmas.size() != this->rows()) + throw InvalidNoiseModel(this->rows(), sigmas.size()); if (anyConstrained) model_ = noiseModel::Constrained::MixedSigmas(sigmas); else model_ = noiseModel::Diagonal::Sigmas(sigmas); } + /* ************************************************************************* */ + const char* JacobianFactor::InvalidNoiseModel::what() throw() { + if(description_.empty()) + description_ = (boost::format( + "A JacobianFactor was attempted to be constructed or modified to use a\n" + "noise model of incompatible dimension. The JacobianFactor has\n" + "dimensionality (i.e. length of error vector) %d but the provided noise\n" + "model has dimensionality %d.") % factorDims % noiseModelDims).str(); + return description_.c_str(); + } + } diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 276d11f27..f5c8bf29b 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -299,6 +299,23 @@ namespace gtsam { /** Assert invariants after elimination */ void assertInvariants() const; + /** An exception indicating that the noise model dimension passed into the + * JacobianFactor has a different dimensionality than the factor. */ + class InvalidNoiseModel : std::exception { + public: + const size_t factorDims; ///< The dimensionality of the factor + const size_t noiseModelDims; ///< The dimensionality of the noise model + + InvalidNoiseModel(size_t factorDims, size_t noiseModelDims) : + factorDims(factorDims), noiseModelDims(noiseModelDims) {} + virtual ~InvalidNoiseModel() throw() {} + + virtual const char* what() throw(); + + private: + mutable std::string description_; + }; + private: // Friend HessianFactor to facilitate conversion constructors @@ -321,5 +338,6 @@ namespace gtsam { } }; // JacobianFactor + } // gtsam