From f6adeb8fff4e951810a937b1c8dc24ec8d0bcaa0 Mon Sep 17 00:00:00 2001
From: Luca <luca.carlone@gatech.edu>
Date: Mon, 25 Aug 2014 16:36:58 -0400
Subject: [PATCH] working example

---
 examples/Pose3SLAMExample_initializePose3.cpp | 40 ++++++++++++++++---
 gtsam/slam/dataset.cpp                        | 10 ++---
 2 files changed, 40 insertions(+), 10 deletions(-)

diff --git a/examples/Pose3SLAMExample_initializePose3.cpp b/examples/Pose3SLAMExample_initializePose3.cpp
index 1754239af..b4bcf970c 100644
--- a/examples/Pose3SLAMExample_initializePose3.cpp
+++ b/examples/Pose3SLAMExample_initializePose3.cpp
@@ -19,6 +19,7 @@
 
 #include <gtsam/slam/InitializePose3.h>
 #include <gtsam/slam/dataset.h>
+#include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <fstream>
 
@@ -39,12 +40,17 @@ int main(const int argc, const char *argv[]) {
   bool is3D = true;
   boost::tie(graph, initial) = readG2o(g2oFile, is3D);
 
-  // Add prior on the pose having index (key) = 0
+  // Add prior on the first key
   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();
+  Key firstKey = 0;
+  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
+    std::cout << "Adding prior to g2o file " << std::endl;
+    firstKey = key_value.key;
+    graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
+    break;
+  }
 
   std::cout << "Initializing Pose3" << std::endl;
   Values initialization = InitializePose3::initialize(graphWithPrior);
@@ -54,10 +60,34 @@ int main(const int argc, const char *argv[]) {
     initialization.print("initialization");
   } else {
     const string outputFile = argv[2];
-    std::cout << "Writing results to file: " << outputFile << std::endl;
+    std::cout << "Writing results to file: " << outputFile << argc << std::endl;
     writeG2o(*graph, initialization, outputFile);
     std::cout << "done! " << std::endl;
-  }
 
+    // This should be deleted: only for debug
+    if (argc == 4){
+      const string inputFileRewritten = argv[3];
+      std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
+      // Additional: rewrite input with simplified keys 0,1,...
+      Values simpleInitial;
+      BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
+        Key key = key_value.key - firstKey;
+        simpleInitial.insert(key, initial->at(key_value.key));
+      }
+      NonlinearFactorGraph simpleGraph;
+      BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, *graph) {
+        boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
+            boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
+        if (pose3Between){
+          Key key1 = pose3Between->key1() - firstKey;
+          Key key2 = pose3Between->key2() - firstKey;
+          NonlinearFactor::shared_ptr simpleFactor(
+              new BetweenFactor<Pose3>(key1, key2, pose3Between->measured(), pose3Between->get_noiseModel()));
+          simpleGraph.add(simpleFactor);
+        }
+      }
+      writeG2o(simpleGraph, simpleInitial, inputFileRewritten);
+    }
+  }
   return 0;
 }
diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp
index 7d06ad0cf..e8b266b19 100644
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
@@ -182,7 +182,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID,
       break;
 
     if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) {
-      int id;
+      Key id;
       double x, y, yaw;
       is >> id >> x >> y >> yaw;
 
@@ -468,7 +468,7 @@ GraphAndValues load3D(const string& filename) {
     ls >> tag;
 
     if (tag == "VERTEX3") {
-      int id;
+      Key id;
       double x, y, z, roll, pitch, yaw;
       ls >> id >> x >> y >> z >> roll >> pitch >> yaw;
       Rot3 R = Rot3::ypr(yaw,pitch,roll);
@@ -476,7 +476,7 @@ GraphAndValues load3D(const string& filename) {
       initial->insert(id, Pose3(R,t));
     }
     if (tag == "VERTEX_SE3:QUAT") {
-      int id;
+      Key id;
       double x, y, z, qx, qy, qz, qw;
       ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw;
       Rot3 R = Rot3::quaternion(qw, qx, qy, qz);
@@ -495,7 +495,7 @@ GraphAndValues load3D(const string& filename) {
     ls >> tag;
 
     if (tag == "EDGE3") {
-      int id1, id2;
+      Key id1, id2;
       double x, y, z, roll, pitch, yaw;
       ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw;
       Rot3 R = Rot3::ypr(yaw,pitch,roll);
@@ -511,7 +511,7 @@ GraphAndValues load3D(const string& filename) {
     }
     if (tag == "EDGE_SE3:QUAT") {
       Matrix m = eye(6);
-      int id1, id2;
+      Key id1, id2;
       double x, y, z, qx, qy, qz, qw;
       ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw;
       Rot3 R = Rot3::quaternion(qw, qx, qy, qz);