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

Standardized function names - marginalFactor, marginalFactorGraph, marginalCovariance

release/4.3a0
Richard Roberts 2010-10-25 22:45:45 +00:00
parent 46cfa84068
commit 3743342534
15 changed files with 50 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
examples/Pose2SLAMExample_advanced.cpp
View File

@ -77,8 +77,8 @@ int main(int argc, char** argv) {
result.print("final result");
/* Get covariances */
Matrix covariance1 = optimizer_result.marginalStandard(x1);
Matrix covariance2 = optimizer_result.marginalStandard(x2);
Matrix covariance1 = optimizer_result.marginalCovariance(x1);
Matrix covariance2 = optimizer_result.marginalCovariance(x2);
print(covariance1, "Covariance1");
print(covariance2, "Covariance2");

8
gtsam/inference/BayesTree-inl.h
View File

@ -364,7 +364,7 @@ namespace gtsam {
// Find marginal on the keys we are interested in
FactorGraph<typename CONDITIONAL::Factor> p_FSRfg(p_FSR);
return *GenericSequentialSolver<typename CONDITIONAL::Factor>(p_FSR).joint(keys());
return *GenericSequentialSolver<typename CONDITIONAL::Factor>(p_FSR).jointFactorGraph(keys());
}
/* ************************************************************************* */
@ -392,7 +392,7 @@ namespace gtsam {
// Calculate the marginal
vector<Index> keys12vector; keys12vector.reserve(keys12.size());
keys12vector.insert(keys12vector.begin(), keys12.begin(), keys12.end());
return *GenericSequentialSolver<typename CONDITIONAL::Factor>(joint).joint(keys12vector);
return *GenericSequentialSolver<typename CONDITIONAL::Factor>(joint).jointFactorGraph(keys12vector);
}
/* ************************************************************************* */
@ -706,7 +706,7 @@ namespace gtsam {
// calculate or retrieve its marginal
FactorGraph<typename CONDITIONAL::Factor> cliqueMarginal = clique->marginal(root_);
return GenericSequentialSolver<typename CONDITIONAL::Factor>(cliqueMarginal).marginal(key);
return GenericSequentialSolver<typename CONDITIONAL::Factor>(cliqueMarginal).marginalFactor(key);
}
/* ************************************************************************* */
@ -736,7 +736,7 @@ namespace gtsam {
// eliminate remaining factor graph to get requested joint
vector<Index> key12(2); key12[0] = key1; key12[1] = key2;
return GenericSequentialSolver<typename CONDITIONAL::Factor>(p_C1C2).joint(key12);
return GenericSequentialSolver<typename CONDITIONAL::Factor>(p_C1C2).jointFactorGraph(key12);
}
/* ************************************************************************* */

4
gtsam/inference/GenericMultifrontalSolver-inl.h
View File

@ -47,7 +47,7 @@ GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::eliminate() const {
/* ************************************************************************* */
template<class FACTOR, class JUNCTIONTREE>
typename FactorGraph<FACTOR>::shared_ptr
GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::joint(const std::vector<Index>& js) const {
GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::jointFactorGraph(const std::vector<Index>& js) const {
// We currently have code written only for computing the
@ -63,7 +63,7 @@ GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::joint(const std::vector<Index>&
/* ************************************************************************* */
template<class FACTOR, class JUNCTIONTREE>
typename FACTOR::shared_ptr GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::marginal(Index j) const {
typename FACTOR::shared_ptr GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::marginalFactor(Index j) const {
return eliminate()->marginal(j);
}

6
gtsam/inference/GenericMultifrontalSolver.h
View File

@ -53,13 +53,13 @@ public:
* all of the other variables. This function returns the result as a factor
* graph.
*/
typename FactorGraph<FACTOR>::shared_ptr joint(const std::vector<Index>& js) const;
typename FactorGraph<FACTOR>::shared_ptr jointFactorGraph(const std::vector<Index>& js) const;
/**
* Compute the marginal Gaussian density over a variable, by integrating out
* Compute the marginal density over a variable, by integrating out
* all of the other variables. This function returns the result as a factor.
*/
typename FACTOR::shared_ptr marginal(Index j) const;
typename FACTOR::shared_ptr marginalFactor(Index j) const;
};

6
gtsam/inference/GenericSequentialSolver-inl.h
View File

@ -44,7 +44,7 @@ typename BayesNet<typename FACTOR::Conditional>::shared_ptr GenericSequentialSol
/* ************************************************************************* */
template<class FACTOR>
typename FactorGraph<FACTOR>::shared_ptr GenericSequentialSolver<FACTOR>::joint(const std::vector<Index>& js) const {
typename FactorGraph<FACTOR>::shared_ptr GenericSequentialSolver<FACTOR>::jointFactorGraph(const std::vector<Index>& js) const {
// Compute a COLAMD permutation with the marginal variable constrained to the end.
Permutation::shared_ptr permutation(Inference::PermutationCOLAMD(structure_, js));
@ -84,12 +84,12 @@ typename FactorGraph<FACTOR>::shared_ptr GenericSequentialSolver<FACTOR>::joint(
/* ************************************************************************* */
template<class FACTOR>
typename FACTOR::shared_ptr GenericSequentialSolver<FACTOR>::marginal(Index j) const {
typename FACTOR::shared_ptr GenericSequentialSolver<FACTOR>::marginalFactor(Index j) const {
// Create a container for the one variable index
vector<Index> js(1); js[0] = j;
// Call joint and return the only factor in the factor graph it returns
return (*this->joint(js))[0];
return (*this->jointFactorGraph(js))[0];
}
}

4
gtsam/inference/GenericSequentialSolver.h
View File

@ -59,13 +59,13 @@ public:
* all of the other variables. This function returns the result as a factor
* graph.
*/
typename FactorGraph<FACTOR>::shared_ptr joint(const std::vector<Index>& js) const;
typename FactorGraph<FACTOR>::shared_ptr jointFactorGraph(const std::vector<Index>& js) const;
/**
* Compute the marginal Gaussian density over a variable, by integrating out
* all of the other variables. This function returns the result as a factor.
*/
typename FACTOR::shared_ptr marginal(Index j) const;
typename FACTOR::shared_ptr marginalFactor(Index j) const;
};

14
gtsam/inference/SymbolicSequentialSolver.cpp
View File

@ -24,18 +24,4 @@ namespace gtsam {
// An explicit instantiation to be compiled into the library
template class GenericSequentialSolver<IndexFactor>;
///* ************************************************************************* */
//SymbolicSequentialSolver::SymbolicSequentialSolver(const FactorGraph<IndexFactor>& factorGraph) :
// Base(factorGraph) {}
//
///* ************************************************************************* */
//BayesNet<IndexConditional>::shared_ptr SymbolicSequentialSolver::eliminate() const {
// return Base::eliminate();
//}
//
///* ************************************************************************* */
//SymbolicFactorGraph::shared_ptr SymbolicSequentialSolver::joint(const std::vector<Index>& js) const {
// return Base::joint(js);
//}
}

32
gtsam/inference/SymbolicSequentialSolver.h
View File

@ -25,37 +25,5 @@ namespace gtsam {
// The base class provides all of the needed functionality
typedef GenericSequentialSolver<IndexFactor> SymbolicSequentialSolver;
//class SymbolicSequentialSolver : GenericSequentialSolver<IndexFactor> {
//
//protected:
//
// typedef GenericSequentialSolver<IndexFactor> Base;
//
//public:
//
// SymbolicSequentialSolver(const FactorGraph<IndexFactor>& factorGraph);
//
// /**
// * Eliminate the factor graph sequentially. Uses a column elimination tree
// * to recursively eliminate.
// */
// BayesNet<IndexConditional>::shared_ptr eliminate() const;
//
// /**
// * Compute the marginal Gaussian density over a variable, by integrating out
// * all of the other variables. This function returns the result as a factor.
// */
// IndexFactor::shared_ptr marginal(Index j) const;
//
// /**
// * Compute the marginal joint over a set of variables, by integrating out
// * all of the other variables. This function returns the result as an upper-
// * triangular R factor and right-hand-side, i.e. a GaussianBayesNet with
// * R*x = d. To get a mean and covariance matrix, use jointStandard(...)
// */
// SymbolicFactorGraph::shared_ptr joint(const std::vector<Index>& js) const;
//
//};
}

13
gtsam/linear/GaussianMultifrontalSolver.cpp
View File

@ -55,13 +55,18 @@ VectorValues::shared_ptr GaussianMultifrontalSolver::optimize() const {
}
/* ************************************************************************* */
GaussianFactor::shared_ptr GaussianMultifrontalSolver::marginal(Index j) const {
return Base::marginal(j);
GaussianFactorGraph::shared_ptr GaussianMultifrontalSolver::jointFactorGraph(const std::vector<Index>& js) const {
return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(*Base::jointFactorGraph(js)));
}
/* ************************************************************************* */
std::pair<Vector, Matrix> GaussianMultifrontalSolver::marginalStandard(Index j) const {
GaussianConditional::shared_ptr conditional = Base::marginal(j)->eliminateFirst();
GaussianFactor::shared_ptr GaussianMultifrontalSolver::marginalFactor(Index j) const {
return Base::marginalFactor(j);
}
/* ************************************************************************* */
std::pair<Vector, Matrix> GaussianMultifrontalSolver::marginalCovariance(Index j) const {
GaussianConditional::shared_ptr conditional = Base::marginalFactor(j)->eliminateFirst();
Matrix R = conditional->get_R();
return make_pair(conditional->get_d(), inverse(ublas::prod(ublas::trans(R), R)));
}

11
gtsam/linear/GaussianMultifrontalSolver.h
View File

@ -86,13 +86,20 @@ public:
*/
VectorValues::shared_ptr optimize() const;
/**
* Compute the marginal joint over a set of variables, by integrating out
* all of the other variables. This function returns the result as a factor
* graph.
*/
GaussianFactorGraph::shared_ptr jointFactorGraph(const std::vector<Index>& js) const;
/**
* Compute the marginal Gaussian density over a variable, by integrating out
* all of the other variables. This function returns the result as an upper-
* triangular R factor and right-hand-side, i.e. a GaussianConditional with
* R*x = d. To get a mean and covariance matrix, use marginalStandard(...)
*/
GaussianFactor::shared_ptr marginal(Index j) const;
GaussianFactor::shared_ptr marginalFactor(Index j) const;
/**
* Compute the marginal Gaussian density over a variable, by integrating out
@ -101,7 +108,7 @@ public:
* returns a GaussianConditional, this function back-substitutes the R factor
* to obtain the mean, then computes \Sigma = (R^T * R)^-1.
*/
std::pair<Vector, Matrix> marginalStandard(Index j) const;
std::pair<Vector, Matrix> marginalCovariance(Index j) const;
};

13
gtsam/linear/GaussianSequentialSolver.cpp
View File

@ -72,20 +72,19 @@ VectorValues::shared_ptr GaussianSequentialSolver::optimize() const {
}
/* ************************************************************************* */
GaussianFactor::shared_ptr GaussianSequentialSolver::marginal(Index j) const {
return Base::marginal(j);
GaussianFactor::shared_ptr GaussianSequentialSolver::marginalFactor(Index j) const {
return Base::marginalFactor(j);
}
std::pair<Vector, Matrix> GaussianSequentialSolver::marginalStandard(Index j) const {
GaussianConditional::shared_ptr conditional = Base::marginal(j)->eliminateFirst();
std::pair<Vector, Matrix> GaussianSequentialSolver::marginalCovariance(Index j) const {
GaussianConditional::shared_ptr conditional = Base::marginalFactor(j)->eliminateFirst();
Matrix R = conditional->get_R();
return make_pair(conditional->get_d(), inverse(ublas::prod(ublas::trans(R), R)));
}
/* ************************************************************************* */
GaussianFactorGraph::shared_ptr GaussianSequentialSolver::joint(const std::vector<Index>& js) const {
return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(*Base::joint(js)));
GaussianFactorGraph::shared_ptr GaussianSequentialSolver::jointFactorGraph(const std::vector<Index>& js) const {
return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(*Base::jointFactorGraph(js)));
}
}

16
gtsam/linear/GaussianSequentialSolver.h
View File

@ -99,7 +99,7 @@ public:
* triangular R factor and right-hand-side, i.e. a GaussianConditional with
* R*x = d. To get a mean and covariance matrix, use marginalStandard(...)
*/
GaussianFactor::shared_ptr marginal(Index j) const;
GaussianFactor::shared_ptr marginalFactor(Index j) const;
/**
* Compute the marginal Gaussian density over a variable, by integrating out
@ -108,7 +108,7 @@ public:
* returns a GaussianConditional, this function back-substitutes the R factor
* to obtain the mean, then computes \Sigma = (R^T * R)^-1.
*/
std::pair<Vector, Matrix> marginalStandard(Index j) const;
std::pair<Vector, Matrix> marginalCovariance(Index j) const;
/**
* Compute the marginal joint over a set of variables, by integrating out
@ -116,18 +116,8 @@ public:
* triangular R factor and right-hand-side, i.e. a GaussianBayesNet with
* R*x = d. To get a mean and covariance matrix, use jointStandard(...)
*/
GaussianFactorGraph::shared_ptr joint(const std::vector<Index>& js) const;
GaussianFactorGraph::shared_ptr jointFactorGraph(const std::vector<Index>& js) const;
/**
* Compute the marginal joint over a set of variables, by integrating out
* all of the other variables. This function returns the result as a mean
* vector and covariance matrix. The variables will be ordered in the
* return values as they are ordered in the 'js' argument, not as they are
* ordered in the original factor graph. Compared to jointCanonical, which
* returns a GaussianBayesNet, this function back-substitutes the BayesNet to
* obtain the mean, then computes \Sigma = (R^T * R)^-1.
*/
// std::pair<Vector, Matrix> jointStandard(const std::vector<Index>& js) const;
};
}

4
gtsam/nonlinear/NonlinearOptimizer.h
View File

@ -209,8 +209,8 @@ namespace gtsam {
/**
* Return mean and covariance on a single variable
*/
Matrix marginalStandard(Symbol j) const {
return solver_->marginalStandard((*ordering_)[j]).second;
Matrix marginalCovariance(Symbol j) const {
return solver_->marginalCovariance((*ordering_)[j]).second;
}
/**

4
tests/testGaussianISAM.cpp
View File

@ -352,8 +352,8 @@ TEST(BayesTree, simpleMarginal)
gfg.add(0, -eye(2), 1, eye(2), ones(2), sharedSigma(2, 1.0));
gfg.add(1, -eye(2), 2, A12, ones(2), sharedSigma(2, 1.0));
Matrix expected(GaussianSequentialSolver(gfg).marginalStandard(2).second);
Matrix actual(GaussianMultifrontalSolver(gfg).marginalStandard(2).second);
Matrix expected(GaussianSequentialSolver(gfg).marginalCovariance(2).second);
Matrix actual(GaussianMultifrontalSolver(gfg).marginalCovariance(2).second);
CHECK(assert_equal(expected, actual));
}

6
tests/testInference.cpp
View File

@ -42,8 +42,8 @@ TEST(GaussianFactorGraph, createSmoother)
// eliminate
vector<Index> x3var; x3var.push_back(ordering["x3"]);
vector<Index> x1var; x1var.push_back(ordering["x1"]);
GaussianBayesNet p_x3 = *GaussianSequentialSolver(*GaussianSequentialSolver(fg2).joint(x3var)).eliminate();
GaussianBayesNet p_x1 = *GaussianSequentialSolver(*GaussianSequentialSolver(fg2).joint(x1var)).eliminate();
GaussianBayesNet p_x3 = *GaussianSequentialSolver(*GaussianSequentialSolver(fg2).jointFactorGraph(x3var)).eliminate();
GaussianBayesNet p_x1 = *GaussianSequentialSolver(*GaussianSequentialSolver(fg2).jointFactorGraph(x1var)).eliminate();
CHECK(assert_equal(*p_x1.back(),*p_x3.front())); // should be the same because of symmetry
}
@ -53,7 +53,7 @@ TEST( Inference, marginals )
// create and marginalize a small Bayes net on "x"
GaussianBayesNet cbn = createSmallGaussianBayesNet();
vector<Index> xvar; xvar.push_back(0);
GaussianBayesNet actual = *GaussianSequentialSolver(*GaussianSequentialSolver(GaussianFactorGraph(cbn)).joint(xvar)).eliminate();
GaussianBayesNet actual = *GaussianSequentialSolver(*GaussianSequentialSolver(GaussianFactorGraph(cbn)).jointFactorGraph(xvar)).eliminate();
// expected is just scalar Gaussian on x
GaussianBayesNet expected = scalarGaussian(0, 4, sqrt(2));