Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
35 changes: 35 additions & 0 deletions gtsam/linear/GaussianBayesNet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,8 @@
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/linearExceptions.h>

#include <iterator>

Expand All @@ -42,6 +44,39 @@ namespace gtsam {
return optimize(solution);
}

void GaussianBayesNet::checkPivots(const GaussianFactorGraph& eliminated,
double tolerance) const {
// Hard constraints have no information scale, so they do not count.
VectorValues originalDiagonal;
for (const auto& factor : eliminated) {
const auto jacobian = std::dynamic_pointer_cast<JacobianFactor>(factor);
if (factor && !(jacobian && jacobian->isConstrained()))
factor->hessianDiagonalAdd(originalDiagonal);
}
for (const auto& conditional : *this) {
const SharedDiagonal& model = conditional->get_model();
if (model && model->isConstrained()) continue;
const auto R = conditional->R();
DenseIndex row = 0;
for (auto frontal = conditional->beginFrontals();
frontal != conditional->endFrontals(); ++frontal) {
const auto entry = originalDiagonal.find(*frontal);
if (entry == originalDiagonal.end()) {
row += conditional->getDim(frontal);
continue;
}
const Vector& diagonal = entry->second;
for (DenseIndex i = 0; i < diagonal.size(); ++i, ++row) {
const double pivot = model ? R(row, row) / model->sigma(row)
: R(row, row);
// Written so that a NaN pivot is rejected too.
if (!(pivot * pivot >= tolerance * diagonal(i)))
throw IndeterminateSystemException(*frontal);
}
}
}
}

VectorValues GaussianBayesNet::optimize(const VectorValues& given) const {
VectorValues solution = given;
// (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas)
Expand Down
26 changes: 26 additions & 0 deletions gtsam/linear/GaussianBayesNet.h
Original file line number Diff line number Diff line change
Expand Up @@ -25,9 +25,12 @@
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/global_includes.h>

#include <limits>
#include <utility>
namespace gtsam {

class GaussianFactorGraph;

/**
* GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals.
* @ingroup linear
Expand Down Expand Up @@ -125,6 +128,29 @@ namespace gtsam {
/// Version of optimize for incomplete BayesNet, given missing variables
VectorValues optimize(const VectorValues& given) const;

/// checkPivots tolerance for QR conditionals: the QR safeguard, squared.
static constexpr double qrPivotTolerance =
(64.0 * std::numeric_limits<double>::epsilon()) *
(64.0 * std::numeric_limits<double>::epsilon());

/**
* checkPivots tolerance for Cholesky conditionals, above the round-off
* level of exactly dependent columns (below 1e-13 in dense tests with up to
* 6000 unknowns).
*/
static constexpr double choleskyPivotTolerance = 0x1p-40;

/**
* Throw IndeterminateSystemException if a pivot is negligible relative to
* the factor graph that was eliminated, i.e., if R(i,i)^2 < tolerance *
* D(i) for a frontal scalar of an unconstrained conditional, where D is
* the Hessian diagonal of the graph's unconstrained factors. Elimination
* compares each pivot with the diagonal that reaches its own step, which
* misses a dependency revealed while eliminating an earlier variable.
*/
void checkPivots(const GaussianFactorGraph& eliminated,
double tolerance) const;

/**
* Sample using ancestral sampling
* Example:
Expand Down
66 changes: 66 additions & 0 deletions gtsam/linear/tests/testGaussianBayesNet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
#include <gtsam/linear/GaussianDensity.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/linearExceptions.h>

// STL/C++
#include <iostream>
Expand Down Expand Up @@ -385,6 +386,71 @@ TEST(GaussianBayesNet, Dot) {
"}");
}

/* ************************************************************************* */
namespace pivot_check_fixture {

// Unit priors on x0 and x1, so the reference diagonal is one.
GaussianFactorGraph unitPriors() {
GaussianFactorGraph graph;
graph.emplace_shared<JacobianFactor>(X(0), I_1x1, Vector1(0.0));
graph.emplace_shared<JacobianFactor>(X(1), I_1x1, Vector1(0.0));
return graph;
}

// p(x0 | x1) p(x1), with the given pivot for x1.
GaussianBayesNet withLastPivot(double pivot) {
GaussianBayesNet bayesNet;
bayesNet.emplace_shared<GaussianConditional>(X(0), Vector1(0.0), I_1x1, X(1),
I_1x1);
bayesNet.emplace_shared<GaussianConditional>(X(1), Vector1(0.0),
Matrix11{{pivot}});
return bayesNet;
}

// Pivots are compared with the diagonal of the graph that was eliminated,
// with a tolerance that depends on how the conditionals were computed.
TEST(GaussianBayesNet, CheckPivots) {
const GaussianFactorGraph eliminated = unitPriors();
withLastPivot(1e-5).checkPivots(eliminated,
GaussianBayesNet::choleskyPivotTolerance);

const GaussianBayesNet beyondCholesky = withLastPivot(1e-10);
beyondCholesky.checkPivots(eliminated, GaussianBayesNet::qrPivotTolerance);
try {
beyondCholesky.checkPivots(eliminated,
GaussianBayesNet::choleskyPivotTolerance);
FAIL("expected IndeterminateSystemException");
} catch (const IndeterminateSystemException& exception) {
EXPECT(exception.nearbyVariable() == X(1));
}

for (double pivot : {0.0, std::numeric_limits<double>::quiet_NaN()}) {
CHECK_EXCEPTION(withLastPivot(pivot).checkPivots(
eliminated, GaussianBayesNet::qrPivotTolerance),
IndeterminateSystemException);
}
}

// Hard constraints are not checked and do not inflate the reference diagonal.
TEST(GaussianBayesNet, CheckPivotsIgnoresConstraints) {
GaussianBayesNet constrained;
constrained.emplace_shared<GaussianConditional>(
X(0), Vector1(0.0), Matrix11{{0.0}}, noiseModel::Constrained::All(1));
constrained.checkPivots(unitPriors(), GaussianBayesNet::qrPivotTolerance);

// A weak prior is the only information scale on x1, so its pivot is fine.
GaussianFactorGraph eliminated;
eliminated.emplace_shared<JacobianFactor>(X(1), I_1x1, Vector1(0.0),
noiseModel::Constrained::All(1));
eliminated.emplace_shared<JacobianFactor>(
X(1), I_1x1, Vector1(0.0), noiseModel::Isotropic::Sigma(1, 1e8));
GaussianBayesNet weak;
weak.emplace_shared<GaussianConditional>(X(1), Vector1(0.0),
Matrix11{{1e-8}});
weak.checkPivots(eliminated, GaussianBayesNet::choleskyPivotTolerance);
}

} // namespace pivot_check_fixture
/* ************************************************************************* */
int main() {
TestResult tr;
Expand Down
5 changes: 5 additions & 0 deletions gtsam/nonlinear/DoglegOptimizer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -101,6 +101,11 @@ GaussianFactorGraph::shared_ptr DoglegOptimizer::iterate(void) {
}
else if ( params_.isSequential() ) {
GaussianBayesNet bn = *linear->eliminateSequential(*params_.ordering, params_.getEliminationFunction());
// Each sequential step only sees what earlier steps left behind.
bn.checkPivots(*linear,
params_.linearSolverType == DoglegParams::SEQUENTIAL_QR
? GaussianBayesNet::qrPivotTolerance
: GaussianBayesNet::choleskyPivotTolerance);
VectorValues dx_u = bn.optimizeGradientSearch();
VectorValues dx_n = bn.optimize();
result = DoglegOptimizerImpl::Iterate(getDelta(), DoglegOptimizerImpl::ONE_STEP_PER_ITERATION,
Expand Down
21 changes: 14 additions & 7 deletions gtsam/nonlinear/NonlinearOptimizer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/linear/internal/CholmodSolver.h>
Expand Down Expand Up @@ -189,14 +190,20 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph& gfg,
delta = gfg.optimize(params.getEliminationFunction());
} else if (params.isSequential()) {
// Sequential QR or Cholesky (decided by params.getEliminationFunction())
if (params.ordering)
delta = gfg.eliminateSequential(*params.ordering,
const GaussianBayesNet::shared_ptr bayesNet =
params.ordering
? gfg.eliminateSequential(*params.ordering,
params.getEliminationFunction())
->optimize();
else
delta = gfg.eliminateSequential(params.orderingType,
params.getEliminationFunction())
->optimize();
: gfg.eliminateSequential(params.orderingType,
params.getEliminationFunction());
// Each sequential step only sees what earlier steps left behind, so check
// the pivots against the original system as well.
bayesNet->checkPivots(
gfg,
params.linearSolverType == NonlinearOptimizerParams::SEQUENTIAL_QR
? GaussianBayesNet::qrPivotTolerance
: GaussianBayesNet::choleskyPivotTolerance);
delta = bayesNet->optimize();
} else if (params.isIterative()) {
// Conjugate Gradient -> needs params.iterativeParams
if (!params.iterativeParams)
Expand Down
30 changes: 30 additions & 0 deletions tests/testNonlinearOptimizer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1288,6 +1288,36 @@ TEST(NonlinearOptimizer, SingularGraphReportsIndeterminateSystem) {
}
}

// Sequential elimination reports a missing gauge that only an earlier pose's
// elimination reveals, as multifrontal elimination already does.
TEST(NonlinearOptimizer, SquareLoopWithoutPriorReportsIndeterminateSystem) {
const auto model = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.05));
NonlinearFactorGraph graph;
for (size_t i = 0; i < 4; ++i) {
const Pose2 measured(2.0, i < 3 ? 0.0 : 0.1, M_PI_2);
graph.emplace_shared<BetweenFactor<Pose2>>(i, (i + 1) % 4, measured,
model);
}
Values initial;
initial.insert(0, Pose2(0.1, -0.1, 0.1));
initial.insert(1, Pose2(2.1, 0.1, 1.5));
initial.insert(2, Pose2(1.9, 2.1, 3.1));
initial.insert(3, Pose2(-0.1, 1.9, -1.6));
for (const std::string solver : {"SEQUENTIAL_QR", "SEQUENTIAL_CHOLESKY",
"MULTIFRONTAL_QR", "MULTIFRONTAL_CHOLESKY"}) {
GaussNewtonParams gnParams;
gnParams.setOrdering(Ordering{0, 1, 2, 3});
gnParams.setLinearSolverType(solver);
CHECK_EXCEPTION(GaussNewtonOptimizer(graph, initial, gnParams).iterate(),
IndeterminateSystemException);
DoglegParams dlParams;
dlParams.setOrdering(Ordering{0, 1, 2, 3});
dlParams.setLinearSolverType(solver);
CHECK_EXCEPTION(DoglegOptimizer(graph, initial, dlParams).iterate(),
IndeterminateSystemException);
}
}

int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
Expand Down