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

Merge pull request #925 from borglab/fix/889

release/4.3a0
Varun Agrawal 2021-11-12 10:15:38 -05:00 committed by GitHub
parent 496a206d08 cf0c8d2fee
commit 3e5d375716
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
8 changed files with 57 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
gtsam/inference/EliminateableFactorGraph-inst.h
View File

@ -79,28 +79,30 @@ namespace gtsam {
/* ************************************************************************* */
template <class FACTORGRAPH>
boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
boost::shared_ptr<
typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
EliminateableFactorGraph<FACTORGRAPH>::eliminateMultifrontal(
OptionalOrderingType orderingType, const Eliminate& function,
OptionalVariableIndex variableIndex) const
{
OptionalVariableIndex variableIndex) const {
if (!variableIndex) {
// If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
// for no variable index first so that it's always computed if we need to call COLAMD because
// no Ordering is provided. When removing optional from VariableIndex, create VariableIndex
// before creating ordering.
// If no VariableIndex provided, compute one and call this function again
// IMPORTANT: we check for no variable index first so that it's always
// computed if we need to call COLAMD because no Ordering is provided.
// When removing optional from VariableIndex, create VariableIndex before
// creating ordering.
VariableIndex computedVariableIndex(asDerived());
return eliminateMultifrontal(function, computedVariableIndex, orderingType);
}
else {
// Compute an ordering and call this function again. We are guaranteed to have a
// VariableIndex already here because we computed one if needed in the previous 'if' block.
return eliminateMultifrontal(orderingType, function,
computedVariableIndex);
} else {
// Compute an ordering and call this function again. We are guaranteed to
// have a VariableIndex already here because we computed one if needed in
// the previous 'if' block.
if (orderingType == Ordering::METIS) {
Ordering computedOrdering = Ordering::Metis(asDerived());
return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType);
return eliminateMultifrontal(computedOrdering, function, variableIndex);
} else {
Ordering computedOrdering = Ordering::Colamd(*variableIndex);
return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType);
return eliminateMultifrontal(computedOrdering, function, variableIndex);
}
}
}
@ -273,7 +275,7 @@ namespace gtsam {
else
{
// No ordering was provided for the unmarginalized variables, so order them with COLAMD.
return factorGraph->eliminateSequential(function);
return factorGraph->eliminateSequential(Ordering::COLAMD, function);
}
}
}
@ -340,7 +342,7 @@ namespace gtsam {
else
{
// No ordering was provided for the unmarginalized variables, so order them with COLAMD.
return factorGraph->eliminateMultifrontal(function);
return factorGraph->eliminateMultifrontal(Ordering::COLAMD, function);
}
}
}

2
gtsam/inference/EliminateableFactorGraph.h
View File

@ -288,6 +288,7 @@ namespace gtsam {
FactorGraphType& asDerived() { return static_cast<FactorGraphType&>(*this); }
public:
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/** \deprecated ordering and orderingType shouldn't both be specified */
boost::shared_ptr<BayesNetType> eliminateSequential(
const Ordering& ordering,
@ -339,6 +340,7 @@ namespace gtsam {
OptionalVariableIndex variableIndex = boost::none) const {
return marginalMultifrontalBayesTree(variables, function, variableIndex);
}
#endif
};
}

13
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -19,7 +19,6 @@
*/
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/linear/GaussianEliminationTree.h>
#include <gtsam/linear/GaussianJunctionTree.h>
@ -290,10 +289,11 @@ namespace gtsam {
return blocks;
}
/* ************************************************************************* */
/* ************************************************************************ */
VectorValues GaussianFactorGraph::optimize(const Eliminate& function) const {
gttic(GaussianFactorGraph_optimize);
return BaseEliminateable::eliminateMultifrontal(function)->optimize();
return BaseEliminateable::eliminateMultifrontal(Ordering::COLAMD, function)
->optimize();
}
/* ************************************************************************* */
@ -503,13 +503,6 @@ namespace gtsam {
return e;
}
/* ************************************************************************* */
/** \deprecated */
VectorValues GaussianFactorGraph::optimize(boost::none_t,
const Eliminate& function) const {
return optimize(function);
}
/* ************************************************************************* */
void GaussianFactorGraph::printErrors(
const VectorValues& values, const std::string& str,

16
gtsam/linear/GaussianFactorGraph.h
View File

@ -21,12 +21,13 @@
#pragma once
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/EliminateableFactorGraph.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/Errors.h> // Included here instead of fw-declared so we can use Errors::iterator
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
namespace gtsam {
@ -395,9 +396,14 @@ namespace gtsam {
public:
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/** \deprecated */
VectorValues optimize(boost::none_t,
const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
const Eliminate& function =
EliminationTraitsType::DefaultEliminate) const {
return optimize(function);
}
#endif
};

8
gtsam/nonlinear/Marginals.cpp
View File

@ -80,11 +80,15 @@ Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solut
/* ************************************************************************* */
void Marginals::computeBayesTree() {
// The default ordering to use.
const Ordering::OrderingType defaultOrderingType = Ordering::COLAMD;
// Compute BayesTree
if (factorization_ == CHOLESKY)
bayesTree_ = *graph_.eliminateMultifrontal(EliminatePreferCholesky);
bayesTree_ = *graph_.eliminateMultifrontal(defaultOrderingType,
EliminatePreferCholesky);
else if (factorization_ == QR)
bayesTree_ = *graph_.eliminateMultifrontal(EliminateQR);
bayesTree_ =
*graph_.eliminateMultifrontal(defaultOrderingType, EliminateQR);
}
/* ************************************************************************* */

2
gtsam/nonlinear/Marginals.h
View File

@ -131,6 +131,7 @@ protected:
void computeBayesTree(const Ordering& ordering);
public:
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/** \deprecated argument order changed due to removing boost::optional<Ordering> */
Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization,
const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {}
@ -142,6 +143,7 @@ public:
/** \deprecated argument order changed due to removing boost::optional<Ordering> */
Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization,
const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {}
#endif
};

2
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -265,6 +265,7 @@ namespace gtsam {
public:
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/** \deprecated */
boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
const Values& values, boost::none_t, const Dampen& dampen = nullptr) const
@ -274,6 +275,7 @@ namespace gtsam {
Values updateCholesky(const Values& values, boost::none_t,
const Dampen& dampen = nullptr) const
{return updateCholesky(values, dampen);}
#endif
};

10
gtsam/nonlinear/NonlinearOptimizer.cpp
View File

@ -147,11 +147,13 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph& gfg,
} else if (params.isSequential()) {
// Sequential QR or Cholesky (decided by params.getEliminationFunction())
if (params.ordering)
delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction(),
boost::none, params.orderingType)->optimize();
delta = gfg.eliminateSequential(*params.ordering,
params.getEliminationFunction())
->optimize();
else
delta = gfg.eliminateSequential(params.getEliminationFunction(), boost::none,
params.orderingType)->optimize();
delta = gfg.eliminateSequential(params.orderingType,
params.getEliminationFunction())
->optimize();
} else if (params.isIterative()) {
// Conjugate Gradient -> needs params.iterativeParams
if (!params.iterativeParams)