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

added method to access isam2 results in incremental fixed lag smoother

release/4.3a0
Luca 2016-08-24 14:19:02 -04:00
parent b6c2ab6664
commit 1337ac8ab0
3 changed files with 7 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam_unstable/nonlinear/FixedLagSmoother.h
View File

@ -68,7 +68,6 @@ public:
} }
}; };
/** default constructor */ /** default constructor */
FixedLagSmoother(double smootherLag = 0.0) : smootherLag_(smootherLag) { } FixedLagSmoother(double smootherLag = 0.0) : smootherLag_(smootherLag) { }

2
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
View File

@ -125,7 +125,7 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end()); KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end());
// Update iSAM2 // Update iSAM2
ISAM2Result isamResult = isam_.update(newFactors, newTheta, isamResult_ = isam_.update(newFactors, newTheta,
factorToRemove, constrainedKeys, boost::none, additionalMarkedKeys); factorToRemove, constrainedKeys, boost::none, additionalMarkedKeys);
if (debug) { if (debug) {

6
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h
View File

@ -109,11 +109,17 @@ public:
return isam_.marginalCovariance(key); return isam_.marginalCovariance(key);
} }
/// Get results of latest isam2 update
const ISAM2Result getISAM2Result() const{ return isamResult_; }
protected: protected:
/** An iSAM2 object used to perform inference. The smoother lag is controlled /** An iSAM2 object used to perform inference. The smoother lag is controlled
* by what factors are removed each iteration */ * by what factors are removed each iteration */
ISAM2 isam_; ISAM2 isam_;
/** Store results of latest isam2 update */
ISAM2Result isamResult_;
/** Erase any keys associated with timestamps before the provided time */ /** Erase any keys associated with timestamps before the provided time */
void eraseKeysBefore(double timestamp); void eraseKeysBefore(double timestamp);