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

Added version of calculateEstimate(const KEY&) to calculate only a single variable

release/4.3a0
Richard Roberts 2011-11-15 17:06:34 +00:00
parent 2c5135e5f9
commit 2f13d0b961
2 changed files with 20 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/nonlinear/ISAM2-inl.h
View File

@ -564,6 +564,15 @@ Values ISAM2<Conditional, Values>::calculateEstimate() const {
return ret;
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES>
template<class KEY>
typename KEY::Value ISAM2<CONDITIONAL, VALUES>::calculateEstimate(const KEY& key) const {
const Index index = getOrdering()[key];
const SubVector delta = getDelta()[index];
return getLinearizationPoint()[key].retract(delta);
}
/* ************************************************************************* */
template<class Conditional, class Values>
Values ISAM2<Conditional, Values>::calculateBestEstimate() const {

12
gtsam/nonlinear/ISAM2.h
View File

@ -205,10 +205,20 @@ public:
const VALUES& getLinearizationPoint() const {return theta_;}
/** Compute an estimate from the incomplete linear delta computed during the last update.
* This delta is incomplete because it was not updated below wildfire_threshold.
* This delta is incomplete because it was not updated below wildfire_threshold. If only
* a single variable is needed, it is faster to call calculateEstimate(const KEY&).
*/
VALUES calculateEstimate() const;
/** Compute an estimate for a single variable using its incomplete linear delta computed
* during the last update. This is faster than calling the no-argument version of
* calculateEstimate, which operates on all variables.
* @param key
* @return
*/
template<class KEY>
typename KEY::Value calculateEstimate(const KEY& key) const;
/// @name Public members for non-typical usage
//@{