12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed consistency checks

release/4.3a0
Frank Dellaert 2018-09-28 11:20:55 -04:00
parent ce65b5d043
commit a87ce9bac4
3 changed files with 7 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/linear/JacobianFactor.cpp
View File

@ -188,7 +188,7 @@ boost::tuple<FastVector<DenseIndex>, DenseIndex, DenseIndex> _countDims(
m += factor->rows(); m += factor->rows();
} }
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS #if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS)
for(DenseIndex d: varDims) { for(DenseIndex d: varDims) {
assert(d != numeric_limits<DenseIndex>::max()); assert(d != numeric_limits<DenseIndex>::max());
} }

7
gtsam/nonlinear/ISAM2-impl.cpp
View File

@ -291,9 +291,10 @@ size_t ISAM2::Impl::UpdateGaussNewtonDelta(const FastVector<ISAM2::sharedClique>
lastBacksubVariableCount += optimizeWildfireNonRecursive( lastBacksubVariableCount += optimizeWildfireNonRecursive(
root, wildfireThreshold, replacedKeys, delta); // modifies delta root, wildfireThreshold, replacedKeys, delta); // modifies delta
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS #if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS)
for(size_t j=0; j<delta.size(); ++j) for (VectorValues::const_iterator key_delta = delta.begin(); key_delta != delta.end(); ++key_delta) {
assert(delta[j].unaryExpr(ptr_fun(isfinite<double>)).all()); assert(delta[key_delta->first].allFinite());
}
#endif #endif
} }

4
gtsam/nonlinear/ISAM2-inl.h
View File

@ -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? // Are any clique variables part of the tree that has been redone?
bool cliqueReplaced = replaced.exists((*clique)->frontals().front()); bool cliqueReplaced = replaced.exists((*clique)->frontals().front());
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS #if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS)
for(Key frontal: clique->conditional()->frontals()) { for(Key frontal: clique->conditional()->frontals()) {
assert(cliqueReplaced == replaced.exists(frontal)); assert(cliqueReplaced == replaced.exists(frontal));
} }
@ -121,7 +121,7 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
// Are any clique variables part of the tree that has been redone? // Are any clique variables part of the tree that has been redone?
bool cliqueReplaced = replaced.exists(clique->conditional()->frontals().front()); bool cliqueReplaced = replaced.exists(clique->conditional()->frontals().front());
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS #if !defined(NDEBUG) && defined(GTSAM_EXTRA_CONSISTENCY_CHECKS)
for(Key frontal: clique->conditional()->frontals()) { for(Key frontal: clique->conditional()->frontals()) {
assert(cliqueReplaced == replaced.exists(frontal)); assert(cliqueReplaced == replaced.exists(frontal));
} }