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

Timing marginals in timeIncremental

release/4.3a0
Richard Roberts 2012-10-01 16:12:43 +00:00
parent fb409a2cc7
commit 96ce28625b
1 changed files with 16 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
tests/timeIncremental.cpp
View File

@ -21,6 +21,7 @@
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/Marginals.h>
using namespace std;
using namespace gtsam;
@ -120,7 +121,7 @@ int main(int argc, char *argv[]) {
isam2.update(newFactors, newVariables);
toc_(3, "Update ISAM2");
if(step % 20 == 0) {
if(step % 100 == 0) {
tic_(4, "chi2 (script only)");
Values estimate(isam2.calculateEstimate());
double chi2 = chi2_red(isam2.getFactorsUnsafe(), estimate);
@ -128,12 +129,25 @@ int main(int argc, char *argv[]) {
toc_(4, "chi2 (script only)");
}
if(step % 100 == 0) {
tictoc_finishedIteration_();
if(step % 1000 == 0) {
cout << "Step " << step << endl;
tictoc_print_();
}
}
// Compute marginals
Marginals marginals(isam2.getFactorsUnsafe(), isam2.calculateEstimate());
int i=0;
BOOST_FOREACH(Key key, initial.keys()) {
tic_(5, "marginalInformation");
Matrix info = marginals.marginalInformation(key);
toc_(5, "marginalInformation");
tictoc_finishedIteration_();
if(i % 1000 == 0)
tictoc_print_();
++i;
}
return 0;