diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 2c677dd17..566a98fc2 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -580,10 +580,6 @@ void JacobianFactor::gradientAtZero(double* d) const { /* ************************************************************************* */ Vector JacobianFactor::gradient(Key key, const VectorValues& x) const { - if (isConstrained()) { // Untested. But see the explanation in gradientAtZero() - Matrix A = getA(find(key)); - return A.transpose()*ones(rows()); - } // TODO: optimize it for JacobianFactor without converting to a HessianFactor HessianFactor hessian(*this); return hessian.gradient(key, x); diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 71f3febbf..d33c5e07c 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -230,7 +230,9 @@ namespace gtsam { virtual bool empty() const { return size() == 0 /*|| rows() == 0*/; } /** is noise model constrained ? */ - bool isConstrained() const { return model_->isConstrained(); } + bool isConstrained() const { + return model_ && model_->isConstrained(); + } /** Return the dimension of the variable pointed to by the given key iterator * todo: Remove this in favor of keeping track of dimensions with variables?