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

Merged in fix/consistency-checks (pull request #316) 

Fix/consistency checks

Approved-by: Alexander <aes368@cornell.edu>
release/4.3a0
Frank Dellaert 2018-09-29 22:58:52 +00:00
parent e89d312ff0 a87ce9bac4
commit 98f42bd1e6
6 changed files with 17 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/3rdparty/metis/libmetis/balance.c vendored
View File

@ -20,7 +20,7 @@ void Balance2Way(ctrl_t *ctrl, graph_t *graph, real_t *ntpwgts)
if (graph->ncon == 1) {
/* return right away if the balance is OK */
if (iabs(ntpwgts[0]*graph->tvwgt[0]-graph->pwgts[0]) < 3*graph->tvwgt[0]/graph->nvtxs)
if (fabs(ntpwgts[0]*graph->tvwgt[0]-graph->pwgts[0]) < 3*graph->tvwgt[0]/graph->nvtxs)
return;
if (graph->nbnd > 0)

2
gtsam/linear/JacobianFactor.cpp
View File

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

4
gtsam/navigation/ImuFactor.h
View File

@ -106,6 +106,10 @@ public:
preintMeasCov_(preintMeasCov) {
}
/// Virtual destructor
virtual ~PreintegratedImuMeasurements() {
}
/// print
void print(const std::string& s = "Preintegrated Measurements:") const override;

6
gtsam/navigation/TangentPreintegration.h
View File

@ -42,7 +42,7 @@ class TangentPreintegration : public PreintegrationBase {
}
public:
/// @name Constructors
/// @name Constructors/destructors
/// @{
/**
@ -53,6 +53,10 @@ public:
TangentPreintegration(const boost::shared_ptr<Params>& p,
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias());
/// Virtual destructor
virtual ~TangentPreintegration() {
}
/// @}
/// @name Basic utilities

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

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