Disabled extra debug-mode consistency checks that may affect runtime complexity, these are now only enabled when GTSAM_EXTRA_CONSISTENCY_CHECKS is defined
							parent
							
								
									4d71fa10af
								
							
						
					
					
						commit
						e6993668ef
					
				|  | @ -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()) | ||||
|  |  | |||
|  | @ -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 | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
|  | @ -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); } | ||||
|  |  | |||
|  | @ -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()); | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
|  | @ -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
 | ||||
|  |  | |||
|  | @ -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()"); | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
|  | @ -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]); | ||||
|   } | ||||
|  |  | |||
|  | @ -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(""); } | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue