diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index 7d229a1ea..08b131152 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -72,24 +72,24 @@ bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const { && noiseModel_->equals(*e->noiseModel_, tol))); } -static void check(const SharedNoiseModel& noiseModel, const Vector& b) { +static void check(const SharedNoiseModel& noiseModel, size_t m) { if (!noiseModel) throw std::invalid_argument("NoiseModelFactor: no NoiseModel."); - if ((size_t) b.size() != noiseModel->dim()) + if (m != noiseModel->dim()) throw std::invalid_argument( "NoiseModelFactor was created with a NoiseModel of incorrect dimension."); } Vector NoiseModelFactor::whitenedError(const Values& c) const { const Vector b = unwhitenedError(c); - check(noiseModel_, b); + check(noiseModel_, b.size()); return noiseModel_->whiten(b); } double NoiseModelFactor::error(const Values& c) const { if (this->active(c)) { const Vector b = unwhitenedError(c); - check(noiseModel_, b); + check(noiseModel_, b.size()); return 0.5 * noiseModel_->distance(b); } else { return 0.0; @@ -106,7 +106,7 @@ boost::shared_ptr NoiseModelFactor::linearize( // Call evaluate error to get Jacobians and RHS vector b std::vector A(this->size()); Vector b = -unwhitenedError(x, A); - check(noiseModel_, b); + check(noiseModel_, b.size()); // Whiten the corresponding system now this->noiseModel_->WhitenSystem(A, b);