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

`optional<Ordering>` and `auto` modernization

release/4.3a0
Gerry Chen 2019-10-09 17:12:18 -04:00
parent e20494324f
commit bc511a26ec
2 changed files with 12 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/nonlinear/Marginals.cpp
View File

@ -27,7 +27,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization, Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization,
EliminateableFactorGraph<GaussianFactorGraph>::OptionalOrdering ordering) boost::optional<const Ordering &> ordering)
: values_(solution), factorization_(factorization) { : values_(solution), factorization_(factorization) {
gttic(MarginalsConstructor); gttic(MarginalsConstructor);
graph_ = *graph.linearize(solution); graph_ = *graph.linearize(solution);
@ -36,12 +36,12 @@ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution,
/* ************************************************************************* */ /* ************************************************************************* */
Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization, Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization,
EliminateableFactorGraph<GaussianFactorGraph>::OptionalOrdering ordering) boost::optional<const Ordering &> ordering)
: graph_(graph), factorization_(factorization) { : graph_(graph), factorization_(factorization) {
gttic(MarginalsConstructor); gttic(MarginalsConstructor);
Values vals; Values vals;
for (const VectorValues::KeyValuePair& v: solution) { for (const auto& keyValue: solution) {
vals.insert(v.first, v.second); vals.insert(keyValue.first, keyValue.second);
} }
values_ = vals; values_ = vals;
computeBayesTree(ordering); computeBayesTree(ordering);
@ -49,16 +49,15 @@ Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solut
/* ************************************************************************* */ /* ************************************************************************* */
Marginals::Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization, Marginals::Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization,
EliminateableFactorGraph<GaussianFactorGraph>::OptionalOrdering ordering) boost::optional<const Ordering &> ordering)
: graph_(graph), values_(solution), factorization_(factorization) { : graph_(graph), values_(solution), factorization_(factorization) {
gttic(MarginalsConstructor); gttic(MarginalsConstructor);
computeBayesTree(ordering); computeBayesTree(ordering);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Marginals::computeBayesTree(EliminateableFactorGraph<GaussianFactorGraph>::OptionalOrdering ordering) { void Marginals::computeBayesTree(boost::optional<const Ordering &> ordering) {
// Compute BayesTree // Compute BayesTree
factorization_ = factorization;
if(factorization_ == CHOLESKY) if(factorization_ == CHOLESKY)
bayesTree_ = *graph_.eliminateMultifrontal(ordering, EliminatePreferCholesky); bayesTree_ = *graph_.eliminateMultifrontal(ordering, EliminatePreferCholesky);
else if(factorization_ == QR) else if(factorization_ == QR)
@ -145,7 +144,7 @@ JointMarginal Marginals::jointMarginalInformation(const KeyVector& variables) co
// Get dimensions from factor graph // Get dimensions from factor graph
std::vector<size_t> dims; std::vector<size_t> dims;
dims.reserve(variablesSorted.size()); dims.reserve(variablesSorted.size());
for(Key key: variablesSorted) { for(const auto& key: variablesSorted) {
dims.push_back(values_.at(key).dim()); dims.push_back(values_.at(key).dim());
} }
@ -162,7 +161,7 @@ VectorValues Marginals::optimize() const {
void JointMarginal::print(const std::string& s, const KeyFormatter& formatter) const { void JointMarginal::print(const std::string& s, const KeyFormatter& formatter) const {
cout << s << "Joint marginal on keys "; cout << s << "Joint marginal on keys ";
bool first = true; bool first = true;
for(Key key: keys_) { for(const auto& key: keys_) {
if(!first) if(!first)
cout << ", "; cout << ", ";
else else

8
gtsam/nonlinear/Marginals.h
View File

@ -58,7 +58,7 @@ public:
* @param ordering An optional variable ordering for elimination. * @param ordering An optional variable ordering for elimination.
*/ */
Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY, Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY,
EliminateableFactorGraph<GaussianFactorGraph>::OptionalOrdering ordering = boost::none); boost::optional<const Ordering &> ordering = boost::none);
/** Construct a marginals class from a linear factor graph. /** Construct a marginals class from a linear factor graph.
* @param graph The factor graph defining the full joint density on all variables. * @param graph The factor graph defining the full joint density on all variables.
@ -67,7 +67,7 @@ public:
* @param ordering An optional variable ordering for elimination. * @param ordering An optional variable ordering for elimination.
*/ */
Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY, Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY,
EliminateableFactorGraph<GaussianFactorGraph>::OptionalOrdering ordering = boost::none); boost::optional<const Ordering &> ordering = boost::none);
/** Construct a marginals class from a linear factor graph. /** Construct a marginals class from a linear factor graph.
* @param graph The factor graph defining the full joint density on all variables. * @param graph The factor graph defining the full joint density on all variables.
@ -76,7 +76,7 @@ public:
* @param ordering An optional variable ordering for elimination. * @param ordering An optional variable ordering for elimination.
*/ */
Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization = CHOLESKY, Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization = CHOLESKY,
EliminateableFactorGraph<GaussianFactorGraph>::OptionalOrdering ordering = boost::none); boost::optional<const Ordering &> ordering = boost::none);
/** print */ /** print */
void print(const std::string& str = "Marginals: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; void print(const std::string& str = "Marginals: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
@ -103,7 +103,7 @@ public:
protected: protected:
/** Compute the Bayes Tree as a helper function to the constructor */ /** Compute the Bayes Tree as a helper function to the constructor */
void computeBayesTree(EliminateableFactorGraph<GaussianFactorGraph>::OptionalOrdering ordering); void computeBayesTree(boost::optional<const Ordering &> ordering);
}; };