From e605c2dbc51dc44f4338239dd5e25a26762f9754 Mon Sep 17 00:00:00 2001
From: Luca <luca.carlone@gatech.edu>
Date: Mon, 25 Aug 2014 16:01:53 -0400
Subject: [PATCH] added initialization example

---
 examples/Pose3SLAMExample_initializePose3.cpp | 63 +++++++++++++++++++
 1 file changed, 63 insertions(+)
 create mode 100644 examples/Pose3SLAMExample_initializePose3.cpp

diff --git a/examples/Pose3SLAMExample_initializePose3.cpp b/examples/Pose3SLAMExample_initializePose3.cpp
new file mode 100644
index 000000000..1754239af
--- /dev/null
+++ b/examples/Pose3SLAMExample_initializePose3.cpp
@@ -0,0 +1,63 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Pose3SLAMExample_initializePose3.cpp
+ * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
+ * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
+ * @date Aug 25, 2014
+ * @author Luca Carlone
+ */
+
+#include <gtsam/slam/InitializePose3.h>
+#include <gtsam/slam/dataset.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <fstream>
+
+using namespace std;
+using namespace gtsam;
+
+int main(const int argc, const char *argv[]) {
+
+  // Read graph from file
+  string g2oFile;
+  if (argc < 2)
+    g2oFile = findExampleDataFile("pose3example.txt");
+  else
+    g2oFile = argv[1];
+
+  NonlinearFactorGraph::shared_ptr graph;
+  Values::shared_ptr initial;
+  bool is3D = true;
+  boost::tie(graph, initial) = readG2o(g2oFile, is3D);
+
+  // Add prior on the pose having index (key) = 0
+  NonlinearFactorGraph graphWithPrior = *graph;
+  noiseModel::Diagonal::shared_ptr priorModel = //
+      noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4));
+  graphWithPrior.add(PriorFactor<Pose3>(0, Pose3(), priorModel));
+  // graphWithPrior.print();
+
+  std::cout << "Initializing Pose3" << std::endl;
+  Values initialization = InitializePose3::initialize(graphWithPrior);
+  std::cout << "done!" << std::endl;
+
+  if (argc < 3) {
+    initialization.print("initialization");
+  } else {
+    const string outputFile = argv[2];
+    std::cout << "Writing results to file: " << outputFile << std::endl;
+    writeG2o(*graph, initialization, outputFile);
+    std::cout << "done! " << std::endl;
+  }
+
+  return 0;
+}