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

Take ordering into account

release/4.3a0
dellaert 2016-01-28 01:27:33 -08:00
parent e66fcf8612
commit a1b23b24bc
1 changed files with 14 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/linear/GaussianBayesNet.cpp
View File

@ -141,7 +141,20 @@ namespace gtsam {
/* ************************************************************************* */
pair<Matrix,Vector> GaussianBayesNet::matrix() const
{
return GaussianFactorGraph(*this).jacobian();
GaussianFactorGraph factorGraph(*this);
KeySet keys = factorGraph.keys();
// add frontal keys in order
Ordering ordering;
BOOST_FOREACH (const sharedConditional& cg, *this)
BOOST_FOREACH (Key key, cg->frontals()) {
ordering.push_back(key);
keys.erase(key);
}
// add remaining keys in case Bayes net is incomplete
BOOST_FOREACH (Key key, keys)
ordering.push_back(key);
// return matrix and RHS
return factorGraph.jacobian(ordering);
}
///* ************************************************************************* */