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