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

rhs method

release/4.3a0
Frank Dellaert 2009-12-28 18:46:01 +00:00
parent 543d3fcd65
commit 8d4a029665
3 changed files with 25 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
cpp/GaussianFactorGraph.cpp
View File

@ -182,6 +182,14 @@ GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma) const {
return result;
}
/* ************************************************************************* */
Errors GaussianFactorGraph::rhs() const {
Errors e;
BOOST_FOREACH(sharedFactor factor,factors_)
e.push_back(ediv(factor->get_b(),factor->get_sigmas()));
return e;
}
/* ************************************************************************* */
pair<Matrix,Vector> GaussianFactorGraph::matrix(const Ordering& ordering) const {

5
cpp/GaussianFactorGraph.h
View File

@ -156,6 +156,11 @@ namespace gtsam {
*/
GaussianFactorGraph add_priors(double sigma) const;
/**
* Return RHS (b./sigmas) as Errors class
*/
Errors rhs() const;
/**
* Return (dense) matrix associated with factor graph
* @param ordering of variables needed for matrix column order

12
cpp/testGaussianFactorGraph.cpp
View File

@ -606,6 +606,18 @@ TEST( GaussianFactorGraph, transposeMultiplication )
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( GaussianFactorGraph, rhs )
{
GaussianFactorGraph Ab = createGaussianFactorGraph();
Errors expected, actual = Ab.rhs();
expected.push_back(Vector_(2,-1.0,-1.0));
expected.push_back(Vector_(2, 2.0,-1.0));
expected.push_back(Vector_(2, 0.0, 1.0));
expected.push_back(Vector_(2,-1.0, 1.5));
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
// Tests ported from ConstrainedGaussianFactorGraph
/* ************************************************************************* */