diff --git a/gtsam/inference/Factor-inl.h b/gtsam/inference/Factor-inl.h index 822c6a0cd..6b911e0c1 100644 --- a/gtsam/inference/Factor-inl.h +++ b/gtsam/inference/Factor-inl.h @@ -44,9 +44,12 @@ namespace gtsam { void Factor::assertInvariants() const { #ifndef NDEBUG // Check that keys are all unique - std::multiset nonunique(keys_.begin(), keys_.end()); - std::set unique(keys_.begin(), keys_.end()); - assert(nonunique.size() == unique.size() && std::equal(nonunique.begin(), nonunique.end(), unique.begin())); + std::multiset < Key > nonunique(keys_.begin(), keys_.end()); + std::set < Key > unique(keys_.begin(), keys_.end()); + bool correct = (nonunique.size() != unique.size()) + && std::equal(nonunique.begin(), nonunique.end(), unique.begin()); + if (!correct) + throw std::logic_error("Factor::assertInvariants: detected inconsistency"); #endif }