From 12f19e8a7c739afe1126b05c12dde35e5d6dac59 Mon Sep 17 00:00:00 2001
From: cbeall3 <cbeall3@gatech.edu>
Date: Thu, 12 Jun 2014 16:09:39 -0400
Subject: [PATCH] Formatting/spacing

---
 examples/StereoVOExample.cpp | 25 +++++++++++++------------
 1 file changed, 13 insertions(+), 12 deletions(-)

diff --git a/examples/StereoVOExample.cpp b/examples/StereoVOExample.cpp
index 8a04d13a3..19725798c 100644
--- a/examples/StereoVOExample.cpp
+++ b/examples/StereoVOExample.cpp
@@ -24,47 +24,48 @@
  */
  
 #include <gtsam/geometry/Pose3.h>
-#include <gtsam/inference/Key.h>
+#include <gtsam/geometry/Cal3_S2Stereo.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/nonlinear/Marginals.h>
-#include <gtsam/nonlinear/Values.h>
-#include <gtsam/geometry/Cal3_S2Stereo.h>
 #include <gtsam/slam/StereoFactor.h>
-#include <gtsam/nonlinear/NonlinearEquality.h>
-#include <gtsam/inference/Symbol.h>
-
-
 
 using namespace std;
 using namespace gtsam;
 
 int main(int argc, char** argv){
+
   //create graph object, add first pose at origin with key '1'
   NonlinearFactorGraph graph;
-  Pose3 first_pose = Pose3();
-  graph.push_back(NonlinearEquality<Pose3>(1, first_pose));
+  Pose3 first_pose;
+  graph.push_back(NonlinearEquality<Pose3>(1, Pose3()));
   
   //create factor noise model with 3 sigmas of value 1
   const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
   //create stereo camera calibration object with .2m between cameras
   const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2));
+
   //create and add stereo factors between first pose (key value 1) and the three landmarks
   graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(520, 480, 440), model, 1, 3, K));
   graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(120, 80, 440), model, 1, 4, K));
   graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(320, 280, 140), model, 1, 5, K));
+
   //create and add stereo factors between second pose and the three landmarks
   graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(570, 520, 490), model, 2, 3, K));
   graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(70, 20, 490), model, 2, 4, K));
   graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(320, 270, 115), model, 2, 5, K));
+
   //create Values object to contain initial estimates of camera poses and landmark locations
-  Values initial_estimate = Values();
+  Values initial_estimate;
+
   //create and add iniital estimates
   initial_estimate.insert(1, first_pose);
   initial_estimate.insert(2, Pose3(Rot3(), Point3(0.1, -0.1, 1.1)));
   initial_estimate.insert(3, Point3(1, 1, 5));
   initial_estimate.insert(4, Point3(-1, 1, 5));
   initial_estimate.insert(5, Point3(0, -0.5, 5));
+
   //create Levenberg-Marquardt optimizer for resulting factor graph, optimize
   LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate);
   Values result = optimizer.optimize();
@@ -72,4 +73,4 @@ int main(int argc, char** argv){
   result.print("Final result:\n");
 
   return 0;
-}
\ No newline at end of file
+}