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

Added a marginalCovariance function directly to NonlinearISAM

release/4.3a0
Alex Cunningham 2011-02-11 22:23:42 +00:00
parent d8f05f78ff
commit 61d3d30f3b
2 changed files with 11 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/nonlinear/NonlinearISAM-inl.h
View File

@ -91,3 +91,11 @@ Values NonlinearISAM<Values>::estimate() {
else else
return linPoint_; return linPoint_;
} }
/* ************************************************************************* */
template<class Values>
Matrix NonlinearISAM<Values>::marginalCovariance(const Symbol& key) {
Matrix covariance; Vector mean;
boost::tie(mean, covariance) = isam_.marginal(ordering_[key]);
return covariance;
}

3
gtsam/nonlinear/NonlinearISAM.h
View File

@ -77,6 +77,9 @@ public:
/** replace the current ordering */ /** replace the current ordering */
void setOrdering(const Ordering& new_ordering) { ordering_ = new_ordering; } void setOrdering(const Ordering& new_ordering) { ordering_ = new_ordering; }
/** find the marginal covariance for a single variable */
Matrix marginalCovariance(const Symbol& key);
// access // access
/** access the underlying bayes tree */ /** access the underlying bayes tree */