diff --git a/gtsam/inference/BayesNet-inl.h b/gtsam/inference/BayesNet-inl.h index 903826e3a..f0e26c98b 100644 --- a/gtsam/inference/BayesNet-inl.h +++ b/gtsam/inference/BayesNet-inl.h @@ -134,7 +134,7 @@ namespace gtsam { /* ************************************************************************* */ template void BayesNet::popLeaf(iterator conditional) { -#ifndef NDEBUG +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS BOOST_FOREACH(typename CONDITIONAL::shared_ptr checkConditional, conditionals_) { BOOST_FOREACH(Index key, (*conditional)->frontals()) { if(std::find(checkConditional->beginParents(), checkConditional->endParents(), key) != checkConditional->endParents()) diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h index 7aa54b216..ad39da301 100644 --- a/gtsam/inference/BayesNet.h +++ b/gtsam/inference/BayesNet.h @@ -153,9 +153,9 @@ public: * particular key, use find(), which has \f$ O(n) \f$ complexity. The * popLeaf function by itself has \f$ O(1) \f$ complexity. * - * If gtsam is compiled without NDEBUG defined, this function will check that - * the node is indeed a leaf, but otherwise will not check, because the check - * has \f$ O(n^2) \f$ complexity. + * If gtsam is compiled with GTSAM_EXTRA_CONSISTENCY_CHECKS defined, this + * function will check that the node is indeed a leaf, but otherwise will + * not check, because the check has \f$ O(n^2) \f$ complexity. * * Example 1: \code diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index 3ebb502f2..9f672e56f 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -224,7 +224,7 @@ namespace gtsam { template inline void BayesTree::addToCliqueFront(BayesTree& bayesTree, const sharedConditional& conditional, const sharedClique& clique) { static const bool debug = false; -#ifndef NDEBUG +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS // Debug check to make sure the conditional variable is ordered lower than // its parents and that all of its parents are present either in this // clique or its separator. @@ -431,18 +431,6 @@ namespace gtsam { // if the parents and parent clique have the same size, add to parent clique if ((*parent_clique)->size() == size_t(parents.size())) { if(debug) std::cout << "Adding to parent clique" << std::endl; -#ifndef NDEBUG - // Debug check that the parent indices of the new conditional match the indices - // currently in the clique. -// list::const_iterator parent = parents.begin(); -// typename Clique::const_iterator cond = parent_clique->begin(); -// while(parent != parents.end()) { -// assert(cond != parent_clique->end()); -// assert(*parent == (*cond)->key()); -// ++ parent; -// ++ cond; -// } -#endif addToCliqueFront(bayesTree, conditional, parent_clique); } else { if(debug) std::cout << "Starting new clique" << std::endl; diff --git a/gtsam/inference/BayesTreeCliqueBase-inl.h b/gtsam/inference/BayesTreeCliqueBase-inl.h index 5f01aa395..e2ac492b7 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inl.h +++ b/gtsam/inference/BayesTreeCliqueBase-inl.h @@ -24,14 +24,6 @@ namespace gtsam { /* ************************************************************************* */ template void BayesTreeCliqueBase::assertInvariants() const { -#ifndef NDEBUG - // We rely on the keys being sorted -// FastVector sortedUniqueKeys(conditional_->begin(), conditional_->end()); -// std::sort(sortedUniqueKeys.begin(), sortedUniqueKeys.end()); -// std::unique(sortedUniqueKeys.begin(), sortedUniqueKeys.end()); -// assert(sortedUniqueKeys.size() == conditional_->size() && -// std::equal(sortedUniqueKeys.begin(), sortedUniqueKeys.end(), conditional_->begin())); -#endif } /* ************************************************************************* */ @@ -138,7 +130,7 @@ namespace gtsam { const internal::Reduction& inverseReduction) { bool changed = conditional_->reduceSeparatorWithInverse(inverseReduction); -#ifndef NDEBUG +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS if(!changed) { BOOST_FOREACH(const derived_ptr& child, children_) { assert(child->reduceSeparatorWithInverse(inverseReduction) == false); } diff --git a/gtsam/inference/Factor-inl.h b/gtsam/inference/Factor-inl.h index 1283f44c6..7b22e083e 100644 --- a/gtsam/inference/Factor-inl.h +++ b/gtsam/inference/Factor-inl.h @@ -43,7 +43,7 @@ namespace gtsam { /* ************************************************************************* */ template void Factor::assertInvariants() const { -#ifndef NDEBUG +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS // Check that keys are all unique std::multiset nonunique(keys_.begin(), keys_.end()); std::set unique(keys_.begin(), keys_.end()); diff --git a/gtsam/inference/IndexConditional.cpp b/gtsam/inference/IndexConditional.cpp index f2ff4c645..20e978afe 100644 --- a/gtsam/inference/IndexConditional.cpp +++ b/gtsam/inference/IndexConditional.cpp @@ -39,7 +39,7 @@ namespace gtsam { /* ************************************************************************* */ bool IndexConditional::reduceSeparatorWithInverse(const internal::Reduction& inverseReduction) { -#ifndef NDEBUG +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS BOOST_FOREACH(KeyType key, frontals()) { assert(inverseReduction.find(key) == inverseReduction.end()); } #endif bool parentChanged = false; diff --git a/gtsam/inference/JunctionTree-inl.h b/gtsam/inference/JunctionTree-inl.h index 1fc58cbb6..1978bde38 100644 --- a/gtsam/inference/JunctionTree-inl.h +++ b/gtsam/inference/JunctionTree-inl.h @@ -169,7 +169,9 @@ namespace gtsam { current->frontal.size())); gttoc(CombineAndEliminate); +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS assert(std::equal(eliminated.second->begin(), eliminated.second->end(), current->separator.begin())); +#endif gttic(Update_tree); // create a new clique corresponding the combined factors diff --git a/gtsam/inference/VariableIndex.cpp b/gtsam/inference/VariableIndex.cpp index 11ce68ec6..91036e82a 100644 --- a/gtsam/inference/VariableIndex.cpp +++ b/gtsam/inference/VariableIndex.cpp @@ -92,7 +92,7 @@ void VariableIndex::permuteInPlace(const Permutation& selector, const Permutatio /* ************************************************************************* */ void VariableIndex::removeUnusedAtEnd(size_t nToRemove) { -#ifndef NDEBUG +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS for(size_t i = this->size() - nToRemove; i < this->size(); ++i) if(!(*this)[i].empty()) throw std::invalid_argument("Attempting to remove non-empty variables with VariableIndex::removeUnusedAtEnd()"); diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index d9212703b..b04287288 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -195,7 +195,7 @@ namespace gtsam { /* ************************************************************************* */ // Helper functions for Combine static boost::tuple, size_t, size_t> countDims(const FactorGraph& factors, const VariableSlots& variableSlots) { -#ifndef NDEBUG +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS vector varDims(variableSlots.size(), numeric_limits::max()); #else vector varDims(variableSlots.size()); @@ -213,7 +213,7 @@ namespace gtsam { if(sourceVarpos < numeric_limits::max()) { const JacobianFactor& sourceFactor = *factors[sourceFactorI]; size_t vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos); -#ifndef NDEBUG +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS if(varDims[jointVarpos] == numeric_limits::max()) { varDims[jointVarpos] = vardim; n += vardim; diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index b1034a594..9abd62016 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -303,7 +303,7 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, PartialSolveResult result; gttic(select_affected_variables); -#ifndef NDEBUG +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS // Debug check that all variables involved in the factors to be re-eliminated // are in affectedKeys, since we will use it to select a subset of variables. BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { @@ -368,7 +368,7 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, if(debug) factors.print("Colamd-ordered affected factors: "); -#ifndef NDEBUG +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS VariableIndex fromScratchIndex(factors); assert(assert_equal(fromScratchIndex, affectedFactorsIndex)); #endif @@ -422,7 +422,7 @@ size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr& root, std: // Optimize with wildfire lastBacksubVariableCount = optimizeWildfireNonRecursive(root, wildfireThreshold, replacedKeys, delta); // modifies delta_ -#ifndef NDEBUG +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS for(size_t j=0; j)).all()); #endif @@ -487,20 +487,6 @@ size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, double wildfireThresho internal::updateDoglegDeltas(isam.root(), replacedKeys, grad, deltaNewton, RgProd, varsUpdated); optimizeWildfireNonRecursive(isam.root(), wildfireThreshold, replacedKeys, deltaNewton); -#if 0 - VectorValues expected = *allocateVectorValues(isam); - internal::optimizeInPlace(isam.root(), expected); - for(size_t j = 0; j Rd_jfg(isam); - Errors Rg = Rd_jfg * grad; - double RgMagExpected = dot(Rg, Rg); - double RgMagActual = RgProd.container().vector().squaredNorm(); - cout << fabs(RgMagExpected - RgMagActual) << endl; - assert(fabs(RgMagExpected - RgMagActual) < (1e-8 * RgMagActual + 1e-4)); -#endif - replacedKeys.assign(replacedKeys.size(), false); return varsUpdated; diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 87975f59b..4a9cf105a 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -44,7 +44,7 @@ void optimizeWildfire(const boost::shared_ptr& clique, double threshold, // Are any clique variables part of the tree that has been redone? bool cliqueReplaced = replaced[(*clique)->frontals().front()]; -#ifndef NDEBUG +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS BOOST_FOREACH(Index frontal, (*clique)->frontals()) { assert(cliqueReplaced == replaced[frontal]); } @@ -120,7 +120,7 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh // Are any clique variables part of the tree that has been redone? bool cliqueReplaced = replaced[(*clique)->frontals().front()]; -#ifndef NDEBUG +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS BOOST_FOREACH(Index frontal, (*clique)->frontals()) { assert(cliqueReplaced == replaced[frontal]); } diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 3dc0b430d..9b063f0ce 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -201,14 +201,18 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const Fas } if(inside) { if(useCachedLinear) { +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS assert(linearFactors_[idx]); - linearized->push_back(linearFactors_[idx]); assert(linearFactors_[idx]->keys() == nonlinearFactors_[idx]->symbolic(ordering_)->keys()); +#endif + linearized->push_back(linearFactors_[idx]); } else { GaussianFactor::shared_ptr linearFactor = nonlinearFactors_[idx]->linearize(theta_, ordering_); linearized->push_back(linearFactor); if(params_.cacheLinearizedFactors) { +#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS assert(linearFactors_[idx]->keys() == linearFactor->keys()); +#endif linearFactors_[idx] = linearFactor; } } @@ -230,13 +234,6 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { BOOST_FOREACH(sharedClique orphan, orphans) { // find the last variable that was eliminated Index key = (*orphan)->frontals().back(); -#ifndef NDEBUG -// typename BayesNet::const_iterator it = orphan->end(); -// const CONDITIONAL& lastCONDITIONAL = **(--it); -// typename CONDITIONAL::const_iterator keyit = lastCONDITIONAL.endParents(); -// const Index lastKey = *(--keyit); -// assert(key == lastKey); -#endif // retrieve the cached factor and add to boundary cachedBoundary.push_back(orphan->cachedFactor()); if(debug) { cout << "Cached factor for variable " << key; orphan->cachedFactor()->print(""); }