From 431c9b19872165324bdf1d3983b3d743c8091f33 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Thu, 7 Nov 2013 21:41:23 +0000
Subject: [PATCH] Added stats mode in SolverComparer

---
 examples/SolverComparer.cpp | 50 +++++++++++++++++++++++++++++++++----
 1 file changed, 45 insertions(+), 5 deletions(-)

diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp
index e58c90e9f..7fbbb9ac1 100644
--- a/examples/SolverComparer.cpp
+++ b/examples/SolverComparer.cpp
@@ -9,18 +9,22 @@
 * -------------------------------------------------------------------------- */
 
 /**
-* @file    timeIncremental.cpp
+* @file    SolverComparer.cpp
 * @brief   Incremental and batch solving, timing, and accuracy comparisons
 * @author  Richard Roberts
+* @date    August, 2013
 */
 
 #include <gtsam/base/timing.h>
+#include <gtsam/base/treeTraversal-inst.h>
 #include <gtsam/slam/dataset.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/BearingRangeFactor.h>
 #include <gtsam/inference/Symbol.h>
+#include <gtsam/linear/GaussianJunctionTree.h>
+#include <gtsam/linear/GaussianEliminationTree.h>
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 #include <gtsam/nonlinear/Marginals.h>
@@ -94,6 +98,7 @@ bool incremental;
 bool batch;
 bool compare;
 bool perturb;
+bool stats;
 double perturbationNoise;
 string compareFile1, compareFile2;
 
@@ -105,6 +110,7 @@ void runIncremental();
 void runBatch();
 void runCompare();
 void runPerturb();
+void runStats();
 
 /* ************************************************************************* */
 int main(int argc, char *argv[]) {
@@ -123,6 +129,7 @@ int main(int argc, char *argv[]) {
     ("batch", "Run in batch mode, requires an initialization from --read-solution")
     ("compare", po::value<vector<string> >()->multitoken(), "Compare two solution files")
     ("perturb", po::value<double>(&perturbationNoise), "Perturb a solution file with the specified noise")
+    ("stats", "Gather factorization statistics about the dataset, writes text-file histograms")
     ;
   po::variables_map vm;
   po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);
@@ -131,7 +138,9 @@ int main(int argc, char *argv[]) {
   batch = (vm.count("batch") > 0);
   compare = (vm.count("compare") > 0);
   perturb = (vm.count("perturb") > 0);
-  incremental = (vm.count("incremental") > 0 || (!batch && !compare && !perturb));
+  stats = (vm.count("stats") > 0);
+  const int modesSpecified = int(batch) + int(compare) + int(perturb) + int(stats);
+  incremental = (vm.count("incremental") > 0 || modesSpecified == 0);
   if(compare) {
     const vector<string>& compareFiles = vm["compare"].as<vector<string> >();
     if(compareFiles.size() != 2) {
@@ -142,15 +151,15 @@ int main(int argc, char *argv[]) {
     compareFile2 = compareFiles[1];
   }
 
-  if((batch && incremental) || (batch && compare) || (incremental && compare)) {
-    cout << "Only one of --incremental, --batch, and --compare may be specified\n" << desc << endl;
+  if(modesSpecified > 1) {
+    cout << "Only one of --incremental, --batch, --compare, --perturb, and --stats may be specified\n" << desc << endl;
     exit(1);
   }
   if((incremental || batch) && datasetName.empty()) {
     cout << "In incremental and batch modes, a dataset must be specified\n" << desc << endl;
     exit(1);
   }
-  if(!(incremental || batch) && !datasetName.empty()) {
+  if(!(incremental || batch || stats) && !datasetName.empty()) {
     cout << "A dataset may only be specified in incremental or batch modes\n" << desc << endl;
     exit(1);
   }
@@ -162,6 +171,10 @@ int main(int argc, char *argv[]) {
     cout << "In perturb mode, specify input and output files\n" << desc << endl;
     exit(1);
   }
+  if(stats && (datasetName.empty() || inputFile.empty())) {
+    cout << "In stats mode, specify dataset and input file\n" << desc << endl;
+    exit(1);
+  }
 
   // Read input file
   if(!inputFile.empty())
@@ -210,6 +223,8 @@ int main(int argc, char *argv[]) {
     runCompare();
   else if(perturb)
     runPerturb();
+  else if(stats)
+    runStats();
 
   return 0;
 }
@@ -548,3 +563,28 @@ void runPerturb()
 
 }
 
+/* ************************************************************************* */
+void runStats()
+{
+  cout << "Gathering statistics..." << endl;
+  GaussianFactorGraph linear = *datasetMeasurements.linearize(initial);
+  GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::COLAMD(linear)));
+  treeTraversal::ForestStatistics statistics = treeTraversal::GatherStatistics(jt);
+
+  ofstream file;
+
+  cout << "Writing SolverComparer_Stats_problemSizeHistogram.txt..." << endl;
+  file.open("SolverComparer_Stats_problemSizeHistogram.txt");
+  treeTraversal::ForestStatistics::Write(file, statistics.problemSizeHistogram);
+  file.close();
+
+  cout << "Writing SolverComparer_Stats_numberOfChildrenHistogram.txt..." << endl;
+  file.open("SolverComparer_Stats_numberOfChildrenHistogram.txt");
+  treeTraversal::ForestStatistics::Write(file, statistics.numberOfChildrenHistogram);
+  file.close();
+
+  cout << "Writing SolverComparer_Stats_problemSizeOfSecondLargestChildHistogram.txt..." << endl;
+  file.open("SolverComparer_Stats_problemSizeOfSecondLargestChildHistogram.txt");
+  treeTraversal::ForestStatistics::Write(file, statistics.problemSizeOfSecondLargestChildHistogram);
+  file.close();
+}