Disabled extra debug-mode consistency checks that may affect runtime complexity, these are now only enabled when GTSAM_EXTRA_CONSISTENCY_CHECKS is defined

gtsam/inference/BayesNet-inl.h

@@ -134,7 +134,7 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class CONDITIONAL>
   void BayesNet<CONDITIONAL>::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())

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

gtsam/inference/BayesTree-inl.h

@@ -224,7 +224,7 @@ namespace gtsam {
   template<class CONDITIONAL, class CLIQUE>
   inline void BayesTree<CONDITIONAL,CLIQUE>::addToCliqueFront(BayesTree<CONDITIONAL,CLIQUE>& 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<Index>::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;

gtsam/inference/BayesTreeCliqueBase-inl.h

@@ -24,14 +24,6 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class DERIVED, class CONDITIONAL>
   void BayesTreeCliqueBase<DERIVED, CONDITIONAL>::assertInvariants() const {
-#ifndef NDEBUG
-    // We rely on the keys being sorted
-//    FastVector<Index> 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); }

gtsam/inference/Factor-inl.h

@@ -43,7 +43,7 @@ namespace gtsam {
   /* ************************************************************************* */
   template<typename KEY>
   void Factor<KEY>::assertInvariants() const {
-#ifndef NDEBUG
+#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
     // Check that keys are all unique
     std::multiset<KeyType> nonunique(keys_.begin(), keys_.end());
     std::set<KeyType> unique(keys_.begin(), keys_.end());

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;

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

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()");

gtsam/linear/GaussianFactorGraph.cpp

@@ -195,7 +195,7 @@ namespace gtsam {
   /* ************************************************************************* */
   // Helper functions for Combine
   static boost::tuple<vector<size_t>, size_t, size_t> countDims(const FactorGraph<JacobianFactor>& factors, const VariableSlots& variableSlots) {
-#ifndef NDEBUG
+#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
     vector<size_t> varDims(variableSlots.size(), numeric_limits<size_t>::max());
 #else
     vector<size_t> varDims(variableSlots.size());
@@ -213,7 +213,7 @@ namespace gtsam {
           if(sourceVarpos < numeric_limits<Index>::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<size_t>::max()) {
   varDims[jointVarpos] = vardim;
   n += vardim;

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<ISAM2Clique>& 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<delta.size(); ++j)
       assert(delta[j].unaryExpr(ptr_fun(isfinite<double>)).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<ISAM2>(isam.root(), expected);
-  for(size_t j = 0; j<expected.size(); ++j)
-    assert(equal_with_abs_tol(expected[j], deltaNewton[j], 1e-2));
-
-  FactorGraph<JacobianFactor> 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;

gtsam/nonlinear/ISAM2-inl.h

@@ -44,7 +44,7 @@ void optimizeWildfire(const boost::shared_ptr<CLIQUE>& 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>& 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]);
   }

gtsam/nonlinear/ISAM2.cpp

@@ -201,14 +201,18 @@ ISAM2::relinearizeAffectedFactors(const FastList<Index>& 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<CONDITIONAL>::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(""); }