Removed extra code that was needed for variant
parent
b9ccdf9b11
commit
a193867214
|
|
@ -26,7 +26,6 @@
|
||||||
#include <gtsam/base/timing.h>
|
#include <gtsam/base/timing.h>
|
||||||
|
|
||||||
#include <fstream>
|
#include <fstream>
|
||||||
#include <functional>
|
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
|
|
@ -278,9 +277,8 @@ namespace gtsam {
|
||||||
FactorGraphType cliqueMarginal = clique->marginal2(function);
|
FactorGraphType cliqueMarginal = clique->marginal2(function);
|
||||||
|
|
||||||
// Now, marginalize out everything that is not variable j
|
// Now, marginalize out everything that is not variable j
|
||||||
auto ordering = Ordering{j};
|
|
||||||
BayesNetType marginalBN =
|
BayesNetType marginalBN =
|
||||||
*cliqueMarginal.marginalMultifrontalBayesNet(std::cref(ordering), function);
|
*cliqueMarginal.marginalMultifrontalBayesNet(Ordering{j}, function);
|
||||||
|
|
||||||
// The Bayes net should contain only one conditional for variable j, so return it
|
// The Bayes net should contain only one conditional for variable j, so return it
|
||||||
return marginalBN.front();
|
return marginalBN.front();
|
||||||
|
|
@ -402,9 +400,8 @@ namespace gtsam {
|
||||||
gttoc(Disjoint_marginals);
|
gttoc(Disjoint_marginals);
|
||||||
}
|
}
|
||||||
|
|
||||||
auto ordering = Ordering{j1, j2};
|
|
||||||
// now, marginalize out everything that is not variable j1 or j2
|
// now, marginalize out everything that is not variable j1 or j2
|
||||||
return p_BC1C2.marginalMultifrontalBayesNet(std::cref(ordering), function);
|
return p_BC1C2.marginalMultifrontalBayesNet(Ordering{j1, j2}, function);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
||||||
|
|
@ -20,8 +20,6 @@
|
||||||
#include <gtsam/inference/FactorGraph-inst.h>
|
#include <gtsam/inference/FactorGraph-inst.h>
|
||||||
#include <gtsam/base/timing.h>
|
#include <gtsam/base/timing.h>
|
||||||
|
|
||||||
#include <functional>
|
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
@ -178,9 +176,8 @@ namespace gtsam {
|
||||||
// The variables we want to keepSet are exactly the ones in S
|
// The variables we want to keepSet are exactly the ones in S
|
||||||
KeyVector indicesS(this->conditional()->beginParents(),
|
KeyVector indicesS(this->conditional()->beginParents(),
|
||||||
this->conditional()->endParents());
|
this->conditional()->endParents());
|
||||||
auto ordering = Ordering(indicesS);
|
|
||||||
auto separatorMarginal =
|
auto separatorMarginal =
|
||||||
p_Cp.marginalMultifrontalBayesNet(std::cref(ordering), function);
|
p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), function);
|
||||||
cachedSeparatorMarginal_ = *separatorMarginal;
|
cachedSeparatorMarginal_ = *separatorMarginal;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -21,15 +21,6 @@
|
||||||
#include <gtsam/inference/EliminateableFactorGraph.h>
|
#include <gtsam/inference/EliminateableFactorGraph.h>
|
||||||
#include <gtsam/inference/inferenceExceptions.h>
|
#include <gtsam/inference/inferenceExceptions.h>
|
||||||
|
|
||||||
// some helper functions
|
|
||||||
namespace {
|
|
||||||
// A function to take a reference_wrapper object and return the underlying pointer
|
|
||||||
template<typename T>
|
|
||||||
T* get_pointer(std::reference_wrapper<T> ref) {
|
|
||||||
return &ref.get();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
|
||||||
|
|
@ -139,7 +139,7 @@ namespace gtsam {
|
||||||
OptionalVariableIndex variableIndex = {}) const;
|
OptionalVariableIndex variableIndex = {}) const;
|
||||||
|
|
||||||
/** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not
|
/** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not
|
||||||
* provided, the ordering will be computed using either COLAMD or METIS, dependeing on
|
* provided, the ordering will be computed using either COLAMD or METIS, depending on
|
||||||
* the parameter orderingType (Ordering::COLAMD or Ordering::METIS)
|
* the parameter orderingType (Ordering::COLAMD or Ordering::METIS)
|
||||||
*
|
*
|
||||||
* <b> Example - Full Cholesky elimination in COLAMD order: </b>
|
* <b> Example - Full Cholesky elimination in COLAMD order: </b>
|
||||||
|
|
@ -160,7 +160,7 @@ namespace gtsam {
|
||||||
OptionalVariableIndex variableIndex = {}) const;
|
OptionalVariableIndex variableIndex = {}) const;
|
||||||
|
|
||||||
/** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not
|
/** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not
|
||||||
* provided, the ordering will be computed using either COLAMD or METIS, dependeing on
|
* provided, the ordering will be computed using either COLAMD or METIS, depending on
|
||||||
* the parameter orderingType (Ordering::COLAMD or Ordering::METIS)
|
* the parameter orderingType (Ordering::COLAMD or Ordering::METIS)
|
||||||
*
|
*
|
||||||
* <b> Example - Full QR elimination in specified order:
|
* <b> Example - Full QR elimination in specified order:
|
||||||
|
|
@ -273,8 +273,7 @@ namespace gtsam {
|
||||||
* COLAMD marginalization order by default
|
* COLAMD marginalization order by default
|
||||||
* @param variables Determines the *ordered* variables whose marginal to compute,
|
* @param variables Determines the *ordered* variables whose marginal to compute,
|
||||||
* will be ordered in the returned BayesNet.
|
* will be ordered in the returned BayesNet.
|
||||||
* @param function Optional dense elimination function, if not provided the default will be
|
* @param function Optional dense elimination function..
|
||||||
* used.
|
|
||||||
* @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
|
* @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
|
||||||
* provided one will be computed. */
|
* provided one will be computed. */
|
||||||
std::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
|
std::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
|
||||||
|
|
@ -286,8 +285,7 @@ namespace gtsam {
|
||||||
* COLAMD marginalization order by default
|
* COLAMD marginalization order by default
|
||||||
* @param variables Determines the variables whose marginal to compute,
|
* @param variables Determines the variables whose marginal to compute,
|
||||||
* will be ordered using constrained COLAMD.
|
* will be ordered using constrained COLAMD.
|
||||||
* @param function Optional dense elimination function, if not provided the default will be
|
* @param function Optional dense elimination function..
|
||||||
* used.
|
|
||||||
* @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
|
* @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
|
||||||
* provided one will be computed. */
|
* provided one will be computed. */
|
||||||
std::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
|
std::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
|
||||||
|
|
@ -300,8 +298,7 @@ namespace gtsam {
|
||||||
* will be ordered in the returned BayesNet.
|
* will be ordered in the returned BayesNet.
|
||||||
* @param marginalizedVariableOrdering Ordering for the variables being marginalized out,
|
* @param marginalizedVariableOrdering Ordering for the variables being marginalized out,
|
||||||
* i.e. all variables not in \c variables.
|
* i.e. all variables not in \c variables.
|
||||||
* @param function Optional dense elimination function, if not provided the default will be
|
* @param function Optional dense elimination function..
|
||||||
* used.
|
|
||||||
* @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
|
* @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
|
||||||
* provided one will be computed. */
|
* provided one will be computed. */
|
||||||
std::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
|
std::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
|
||||||
|
|
@ -315,8 +312,7 @@ namespace gtsam {
|
||||||
* will be ordered using constrained COLAMD.
|
* will be ordered using constrained COLAMD.
|
||||||
* @param marginalizedVariableOrdering Ordering for the variables being marginalized out,
|
* @param marginalizedVariableOrdering Ordering for the variables being marginalized out,
|
||||||
* i.e. all variables not in \c variables.
|
* i.e. all variables not in \c variables.
|
||||||
* @param function Optional dense elimination function, if not provided the default will be
|
* @param function Optional dense elimination function..
|
||||||
* used.
|
|
||||||
* @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
|
* @param variableIndex Optional pre-computed VariableIndex for the factor graph, if not
|
||||||
* provided one will be computed. */
|
* provided one will be computed. */
|
||||||
std::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
|
std::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
|
||||||
|
|
|
||||||
|
|
@ -38,7 +38,7 @@ namespace gtsam {
|
||||||
Ordering lastKeyAsOrdering;
|
Ordering lastKeyAsOrdering;
|
||||||
lastKeyAsOrdering += lastKey;
|
lastKeyAsOrdering += lastKey;
|
||||||
const GaussianConditional::shared_ptr marginal =
|
const GaussianConditional::shared_ptr marginal =
|
||||||
linearFactorGraph.marginalMultifrontalBayesNet(std::cref(lastKeyAsOrdering))->front();
|
linearFactorGraph.marginalMultifrontalBayesNet(lastKeyAsOrdering)->front();
|
||||||
|
|
||||||
// Extract the current estimate of x1,P1
|
// Extract the current estimate of x1,P1
|
||||||
VectorValues result = marginal->solve(VectorValues());
|
VectorValues result = marginal->solve(VectorValues());
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue