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