diff --git a/examples/vSLAMexample/Feature2D.cpp b/examples/vSLAMexample/Feature2D.cpp
index 30bd7fc48..adb1aa6b1 100644
--- a/examples/vSLAMexample/Feature2D.cpp
+++ b/examples/vSLAMexample/Feature2D.cpp
@@ -1,2 +1,19 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 Feature2D.cpp
+ * @brief
+ * @author Duy-Nguyen
+ */
+
 #include "Feature2D.h"
 
diff --git a/examples/vSLAMexample/Feature2D.h b/examples/vSLAMexample/Feature2D.h
index 7e3fceea0..848a9e615 100644
--- a/examples/vSLAMexample/Feature2D.h
+++ b/examples/vSLAMexample/Feature2D.h
@@ -1,28 +1,41 @@
-#ifndef FEATURE2D_H
-#define FEATURE2D_H
+/* ----------------------------------------------------------------------------
+
+ * 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 Feature2D.h
+ * @brief
+ * @author Duy-Nguyen
+ */
+
+#pragma once
 
 #include "gtsam/geometry/Point2.h"
 #include <iostream>
 
-class Feature2D
+struct Feature2D
 {
-public:
-    gtsam::Point2 m_p;
-    int m_idCamera;            // id of the camera pose that makes this measurement
-    int m_idLandmark;       // id of the 3D landmark that it is associated with
-public:
-    Feature2D(int idCamera, int idLandmark, gtsam::Point2 p)
-            :m_p(p),
-            m_idCamera(idCamera),
-            m_idLandmark(idLandmark)
-    {};
 
-    void print(const std::string& s = "") const
-    {
-        std::cout << s << std::endl;
-        std::cout << "Pose id: " << m_idCamera << " -- Landmark id: "  << m_idLandmark << std::endl;
-        m_p.print("\tMeasurement: ");
-    }
+  gtsam::Point2 m_p;
+  int m_idCamera;            // id of the camera pose that makes this measurement
+  int m_idLandmark;       // id of the 3D landmark that it is associated with
+
+  Feature2D(int idCamera, int idLandmark, gtsam::Point2 p) :
+    m_p(p),
+    m_idCamera(idCamera),
+    m_idLandmark(idLandmark) {}
+
+  void print(const std::string& s = "") const {
+    std::cout << s << std::endl;
+    std::cout << "Pose id: " << m_idCamera << " -- Landmark id: "  << m_idLandmark << std::endl;
+    m_p.print("\tMeasurement: ");
+  }
+
 };
-
-#endif // FEATURE2D_H
diff --git a/examples/vSLAMexample/ISAMLoop-inl.h b/examples/vSLAMexample/ISAMLoop-inl.h
index b78079d70..7c6ec5cb9 100644
--- a/examples/vSLAMexample/ISAMLoop-inl.h
+++ b/examples/vSLAMexample/ISAMLoop-inl.h
@@ -1,3 +1,14 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+ * -------------------------------------------------------------------------- */
+
 /*
  * ISAMLoop.cpp
  *
@@ -22,29 +33,29 @@ void ISAMLoop<Values>::update(const Factors& newFactors, const Values& initialVa
 
   if(newFactors.size() > 0) {
 
-      // Reorder and relinearize every reorderInterval updates
-      if(reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) {
-        reorder_relinearize();
-        reorderCounter_ = 0;
+    // Reorder and relinearize every reorderInterval updates
+    if(reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) {
+      reorder_relinearize();
+      reorderCounter_ = 0;
+    }
+
+    factors_.push_back(newFactors);
+
+    // Linearize new factors and insert them
+    // TODO: optimize for whole config?
+    linPoint_.insert(initialValues);
+
+    // Augment ordering
+    BOOST_FOREACH(const typename Factors::sharedFactor& factor, newFactors) {
+      BOOST_FOREACH(const Symbol& key, factor->keys()) {
+        ordering_.tryInsert(key, ordering_.nVars());
       }
+    }
 
-      factors_.push_back(newFactors);
+    boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors(newFactors.linearize(linPoint_, ordering_));
 
-      // Linearize new factors and insert them
-      // TODO: optimize for whole config?
-      linPoint_.insert(initialValues);
-
-      // Augment ordering
-      BOOST_FOREACH(const typename Factors::sharedFactor& factor, newFactors) {
-          BOOST_FOREACH(const Symbol& key, factor->keys()) {
-            ordering_.tryInsert(key, ordering_.nVars());
-          }
-      }
-
-      boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors(newFactors.linearize(linPoint_, ordering_));
-
-      // Update ISAM
-      isam.update(*linearizedNewFactors);
+    // Update ISAM
+    isam.update(*linearizedNewFactors);
   }
 }
 
diff --git a/examples/vSLAMexample/ISAMLoop.h b/examples/vSLAMexample/ISAMLoop.h
index 966bd68e5..e96b0b758 100644
--- a/examples/vSLAMexample/ISAMLoop.h
+++ b/examples/vSLAMexample/ISAMLoop.h
@@ -1,3 +1,14 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+ * -------------------------------------------------------------------------- */
+
 /*
  * ISAMLoop.h
  *
diff --git a/examples/vSLAMexample/vISAMexample.cpp b/examples/vSLAMexample/vISAMexample.cpp
index dfdf74e92..75c0c76ac 100644
--- a/examples/vSLAMexample/vISAMexample.cpp
+++ b/examples/vSLAMexample/vISAMexample.cpp
@@ -1,3 +1,14 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    vSLAMexample.cpp
  * @brief   An vSLAM example for synthesis sequence
@@ -51,96 +62,86 @@ SharedGaussian poseSigma(noiseModel::Unit::Create(1));
 
 /* ************************************************************************* */
 /**
-  * Read all data: calibration file, landmarks, poses, and all features measurements
-  * Data is stored in global variables, which are used later to simulate incremental updates.
-  */
-void readAllDataISAM()
-{
-    g_calib = readCalibData(g_dataFolder + CALIB_FILE);
+ * Read all data: calibration file, landmarks, poses, and all features measurements
+ * Data is stored in global variables, which are used later to simulate incremental updates.
+ */
+void readAllDataISAM() {
+  g_calib = readCalibData(g_dataFolder + CALIB_FILE);
 
-    // Read groundtruth landmarks' positions. These will be used later as intial estimates and priors for landmark nodes.
-    g_landmarks = readLandMarks(g_dataFolder + LANDMARKS_FILE);
+  // Read groundtruth landmarks' positions. These will be used later as intial estimates and priors for landmark nodes.
+  g_landmarks = readLandMarks(g_dataFolder + LANDMARKS_FILE);
 
-    // Read groundtruth camera poses. These will be used later as intial estimates for pose nodes.
-    g_poses = readPosesISAM(g_dataFolder, POSES_FILE);
+  // Read groundtruth camera poses. These will be used later as intial estimates for pose nodes.
+  g_poses = readPosesISAM(g_dataFolder, POSES_FILE);
 
-    // Read all 2d measurements. Those will become factors linking their associating pose and the corresponding landmark.
-    g_measurements = readAllMeasurementsISAM(g_dataFolder, MEASUREMENTS_FILE);
+  // Read all 2d measurements. Those will become factors linking their associating pose and the corresponding landmark.
+  g_measurements = readAllMeasurementsISAM(g_dataFolder, MEASUREMENTS_FILE);
 }
 
 /* ************************************************************************* */
 /**
-  * Setup newFactors and initialValues for each new pose and set of measurements at each frame.
-  */
+ * Setup newFactors and initialValues for each new pose and set of measurements at each frame.
+ */
 void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<Values>& initialValues,
-                      int pose_id, Pose3& pose,
-                      std::vector<Feature2D>& measurements, SharedGaussian measurementSigma, shared_ptrK calib)
-{
-    // Create a graph of newFactors with new measurements
-    newFactors = shared_ptr<Graph>(new Graph());
-    for (size_t i= 0; i<measurements.size(); i++)
-    {
-        newFactors->addMeasurement(measurements[i].m_p,
-                         measurementSigma,
-                         pose_id,
-                         measurements[i].m_idLandmark,
-                         calib);
-    }
+    int pose_id, Pose3& pose, std::vector<Feature2D>& measurements, SharedGaussian measurementSigma, shared_ptrK calib) {
 
-    // ... we need priors on the new pose and all new landmarks
-    newFactors->addPosePrior(pose_id, pose, poseSigma);
-    for (size_t i= 0; i<measurements.size(); i++)
-    {
-      newFactors->addPointPrior(measurements[i].m_idLandmark, g_landmarks[measurements[i].m_idLandmark]);
-    }
+  // Create a graph of newFactors with new measurements
+  newFactors = shared_ptr<Graph> (new Graph());
+  for (size_t i = 0; i < measurements.size(); i++) {
+    newFactors->addMeasurement(
+        measurements[i].m_p,
+        measurementSigma,
+        pose_id,
+        measurements[i].m_idLandmark,
+        calib);
+  }
 
+  // ... we need priors on the new pose and all new landmarks
+  newFactors->addPosePrior(pose_id, pose, poseSigma);
+  for (size_t i = 0; i < measurements.size(); i++) {
+    newFactors->addPointPrior(measurements[i].m_idLandmark, g_landmarks[measurements[i].m_idLandmark]);
+  }
 
-    // Create initial values for all nodes in the newFactors
-    initialValues = shared_ptr<Values>(new Values());
-    initialValues->insert(pose_id, pose);
-    for (size_t i= 0; i<measurements.size(); i++)
-    {
-      initialValues->insert( measurements[i].m_idLandmark, g_landmarks[measurements[i].m_idLandmark] );
-    }
+  // Create initial values for all nodes in the newFactors
+  initialValues = shared_ptr<Values> (new Values());
+  initialValues->insert(pose_id, pose);
+  for (size_t i = 0; i < measurements.size(); i++) {
+    initialValues->insert(measurements[i].m_idLandmark, g_landmarks[measurements[i].m_idLandmark]);
+  }
 }
 
-
 /* ************************************************************************* */
-int main(int argc, char* argv[])
-{
-    if (argc <2)
-    {
-        cout << "Usage: vISAMexample <DataFolder>" << endl << endl;
-        cout << "\tPlease specify <DataFolder>, which contains calibration file, initial landmarks, initial poses, and feature data." << endl;
-        cout << "\tSample folder is in $gtsam_source_folder$/examples/vSLAMexample/Data/" << endl << endl;
-        cout << "Example usage: vISAMexample '$gtsam_source_folder$/examples/vSLAMexample/Data/'" << endl;
-        exit(0);
-    }
+int main(int argc, char* argv[]) {
+  if (argc < 2) {
+    cout << "Usage: vISAMexample <DataFolder>" << endl << endl;
+    cout << "\tPlease specify <DataFolder>, which contains calibration file, initial\n"
+      "landmarks, initial poses, and feature data." << endl;
+    cout << "\tSample folder is in $gtsam_source_folder$/examples/vSLAMexample/Data/" << endl << endl;
+    cout << "Example usage: vISAMexample '$gtsam_source_folder$/examples/vSLAMexample/Data/'" << endl;
+    exit(0);
+  }
 
-    g_dataFolder = string(argv[1]) + "/";
-    readAllDataISAM();
+  g_dataFolder = string(argv[1]) + "/";
+  readAllDataISAM();
 
-    // Create an ISAMLoop which will be relinearized and reordered after every "relinearizeInterval" updates
-    int relinearizeInterval = 3;
-    ISAMLoop<Values> isam(relinearizeInterval);
+  // Create an ISAMLoop which will be relinearized and reordered after every "relinearizeInterval" updates
+  int relinearizeInterval = 3;
+  ISAMLoop<Values> isam(relinearizeInterval);
 
-    // At each frame i with new camera pose and new set of measurements associated with it,
-    // create a graph of new factors and update ISAM
-    for (size_t i = 0; i<g_measurements.size(); i++)
-    { 
-        shared_ptr<Graph> newFactors;
-        shared_ptr<Values> initialValues;
-        createNewFactors(newFactors, initialValues,
-                         i, g_poses[i],
-                         g_measurements[i], measurementSigma, g_calib);
+  // At each frame i with new camera pose and new set of measurements associated with it,
+  // create a graph of new factors and update ISAM
+  for (size_t i = 0; i < g_measurements.size(); i++) {
+    shared_ptr<Graph> newFactors;
+    shared_ptr<Values> initialValues;
+    createNewFactors(newFactors, initialValues, i, g_poses[i], g_measurements[i], measurementSigma, g_calib);
 
-        isam.update(*newFactors, *initialValues);
-        Values currentEstimate = isam.estimate();
-        cout << "****************************************************" << endl;
-        currentEstimate.print("Current estimate: ");
-    }
+    isam.update(*newFactors, *initialValues);
+    Values currentEstimate = isam.estimate();
+    cout << "****************************************************" << endl;
+    currentEstimate.print("Current estimate: ");
+  }
 
+  return 0;
 }
 /* ************************************************************************* */
 
-
diff --git a/examples/vSLAMexample/vSFMexample.cpp b/examples/vSLAMexample/vSFMexample.cpp
index 33cc38380..29f7096ae 100644
--- a/examples/vSLAMexample/vSFMexample.cpp
+++ b/examples/vSLAMexample/vSFMexample.cpp
@@ -1,3 +1,14 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    vSLAMexample.cpp
  * @brief   An vSLAM example for synthesis sequence
@@ -44,105 +55,105 @@ SharedGaussian measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f));
 
 /* ************************************************************************* */
 /**
-  * Read all data: calibration file, landmarks, poses, and all features measurements
-  * Data is stored in global variables.
-  */
-void readAllData()
-{
-    g_calib = readCalibData(g_dataFolder +"/" + CALIB_FILE);
+ * Read all data: calibration file, landmarks, poses, and all features measurements
+ * Data is stored in global variables.
+ */
+void readAllData() {
 
-    // Read groundtruth landmarks' positions. These will be used later as intial estimates for landmark nodes.
-    g_landmarks = readLandMarks(g_dataFolder +"/"+ LANDMARKS_FILE);
+  g_calib = readCalibData(g_dataFolder + "/" + CALIB_FILE);
 
-    // Read groundtruth camera poses. These will be used later as intial estimates for pose nodes.
-    g_poses = readPoses(g_dataFolder, POSES_FILE);
+  // Read groundtruth landmarks' positions. These will be used later as intial estimates for landmark nodes.
+  g_landmarks = readLandMarks(g_dataFolder + "/" + LANDMARKS_FILE);
 
-    // Read all 2d measurements. Those will become factors linking their associating pose and the corresponding landmark.
-    g_measurements = readAllMeasurements(g_dataFolder, MEASUREMENTS_FILE);
+  // Read groundtruth camera poses. These will be used later as intial estimates for pose nodes.
+  g_poses = readPoses(g_dataFolder, POSES_FILE);
+
+  // Read all 2d measurements. Those will become factors linking their associating pose and the corresponding landmark.
+  g_measurements = readAllMeasurements(g_dataFolder, MEASUREMENTS_FILE);
 }
 
 /* ************************************************************************* */
 /**
-  * Setup vSLAM graph
-  * by adding and linking 2D features (measurements) detected in each captured image
-  * with their corresponding landmarks.
-  */
-Graph setupGraph(std::vector<Feature2D>& measurements, SharedGaussian measurementSigma, shared_ptrK calib)
-{
-    Graph g;    
+ * Setup vSLAM graph
+ * by adding and linking 2D features (measurements) detected in each captured image
+ * with their corresponding landmarks.
+ */
+Graph setupGraph(std::vector<Feature2D>& measurements, SharedGaussian measurementSigma, shared_ptrK calib) {
 
-    cout << "Built graph: " << endl;
-    for (size_t i= 0; i<measurements.size(); i++)
-    {
-        measurements[i].print();
+  Graph g;
 
-        g.addMeasurement(measurements[i].m_p,
-                         measurementSigma,
-                         measurements[i].m_idCamera,
-                         measurements[i].m_idLandmark,
-                         calib);
-    }
+  cout << "Built graph: " << endl;
+  for (size_t i = 0; i < measurements.size(); i++) {
+    measurements[i].print();
 
-    return g;
+    g.addMeasurement(
+        measurements[i].m_p,
+        measurementSigma,
+        measurements[i].m_idCamera,
+        measurements[i].m_idLandmark,
+        calib);
+  }
+
+  return g;
 }
 
 /* ************************************************************************* */
 /**
-  * Create a structure of initial estimates for all nodes (landmarks and poses) in the graph.
-  * The returned Values structure contains all initial values for all nodes.
-  */
-Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses)
-{
-    Values initValues;
+ * Create a structure of initial estimates for all nodes (landmarks and poses) in the graph.
+ * The returned Values structure contains all initial values for all nodes.
+ */
+Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses) {
 
-    // Initialize landmarks 3D positions.
-    for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
-        initValues.insert( lmit->first, lmit->second );
+  Values initValues;
 
-    // Initialize camera poses.
-    for (map<int, Pose3>::iterator poseit = poses.begin(); poseit != poses.end(); poseit++)
-        initValues.insert( poseit->first, poseit->second);
+  // Initialize landmarks 3D positions.
+  for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
+    initValues.insert(lmit->first, lmit->second);
 
-    return initValues;
+  // Initialize camera poses.
+  for (map<int, Pose3>::iterator poseit = poses.begin(); poseit != poses.end(); poseit++)
+    initValues.insert(poseit->first, poseit->second);
+
+  return initValues;
 }
 
 /* ************************************************************************* */
-int main(int argc, char* argv[])
-{
-    if (argc <2)
-    {
-        cout << "Usage: vSFMexample <DataFolder>" << endl << endl;
-        cout << "\tPlease specify <DataFolder>, which contains calibration file, initial landmarks, initial poses, and feature data." << endl;
-        cout << "\tSample folder is in $gtsam_source_folder$/examples/vSLAMexample/Data" << endl << endl;
-        cout << "Example usage: vSFMexample '$gtsam_source_folder$/examples/vSLAMexample/Data'" << endl;
-        exit(0);
-    }
+int main(int argc, char* argv[]) {
+  if (argc < 2) {
+    cout << "Usage: vSFMexample <DataFolder>" << endl << endl;
+    cout << "\tPlease specify <DataFolder>, which contains calibration file, initial\n"
+          "landmarks, initial poses, and feature data." << endl;
+    cout << "\tSample folder is in $gtsam_source_folder$/examples/vSLAMexample/Data" << endl << endl;
+    cout << "Example usage: vSFMexample '$gtsam_source_folder$/examples/vSLAMexample/Data'" << endl;
+    exit(0);
+  }
 
-    g_dataFolder = string(argv[1]) + "/";
-    readAllData();
+  g_dataFolder = string(argv[1]) + "/";
+  readAllData();
 
-    // Create a graph using the 2D measurements (features) and the calibration data
-    boost::shared_ptr<Graph> graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib)));
+  // Create a graph using the 2D measurements (features) and the calibration data
+  boost::shared_ptr<Graph> graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib)));
 
-    // Create an initial Values structure using groundtruth values as the initial estimates
-    boost::shared_ptr<Values> initialEstimates(new Values(initialize(g_landmarks, g_poses)));
-    cout << "*******************************************************" << endl;
-    initialEstimates->print("INITIAL ESTIMATES: ");
+  // Create an initial Values structure using groundtruth values as the initial estimates
+  boost::shared_ptr<Values> initialEstimates(new Values(initialize(g_landmarks, g_poses)));
+  cout << "*******************************************************" << endl;
+  initialEstimates->print("INITIAL ESTIMATES: ");
 
-    // Add prior factor for all poses in the graph
-    map<int, Pose3>::iterator poseit = g_poses.begin();
-    for (; poseit != g_poses.end(); poseit++)
-        graph->addPosePrior(poseit->first, poseit->second, noiseModel::Unit::Create(1));
+  // Add prior factor for all poses in the graph
+  map<int, Pose3>::iterator poseit = g_poses.begin();
+  for (; poseit != g_poses.end(); poseit++)
+    graph->addPosePrior(poseit->first, poseit->second, noiseModel::Unit::Create(1));
 
-    // Optimize the graph
-    cout << "*******************************************************" << endl;
-    Optimizer::Parameters::verbosityLevel verborsity = Optimizer::Parameters::DAMPED;
-    Optimizer::shared_values result = Optimizer::optimizeGN( graph, initialEstimates, verborsity );
+  // Optimize the graph
+  cout << "*******************************************************" << endl;
+  Optimizer::Parameters::verbosityLevel verborsity = Optimizer::Parameters::DAMPED;
+  Optimizer::shared_values result = Optimizer::optimizeGN(graph, initialEstimates, verborsity);
 
-    // Print final results
-    cout << "*******************************************************" << endl;
-    result->print("FINAL RESULTS: ");
+  // Print final results
+  cout << "*******************************************************" << endl;
+  result->print("FINAL RESULTS: ");
 
+  return 0;
 }
 /* ************************************************************************* */
 
diff --git a/examples/vSLAMexample/vSLAMutils.cpp b/examples/vSLAMexample/vSLAMutils.cpp
index 1f7861620..fb05b5232 100644
--- a/examples/vSLAMexample/vSLAMutils.cpp
+++ b/examples/vSLAMexample/vSLAMutils.cpp
@@ -1,3 +1,20 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 VSLAMutils.cpp
+ * @brief
+ * @author Duy-Nguyen
+ */
+
 #include "vSLAMutils.h"
 #include <fstream>
 #include <cstdio>
@@ -6,206 +23,187 @@ using namespace gtsam;
 using namespace std;
 
 /* ************************************************************************* */
-std::map<int, Point3> readLandMarks(const std::string& landmarkFile)
-{
-    ifstream file(landmarkFile.c_str());
-    if (!file) {
-        cout << "Cannot read landmark file: " << landmarkFile << endl;
-        exit(0);
-    }
+std::map<int, Point3> readLandMarks(const std::string& landmarkFile) {
+  ifstream file(landmarkFile.c_str());
+  if (!file) {
+    cout << "Cannot read landmark file: " << landmarkFile << endl;
+    exit(0);
+  }
 
-    int num;
-    file >> num;
-    std::map<int, Point3> landmarks;
-    landmarks.clear();
-    for (int i = 0; i<num; i++)
-    {
-        int color_id;
-        float x, y, z;
-        file >> color_id >> x >> y >> z;
-        landmarks[color_id] = Point3(x, y, z);
-    }
+  int num;
+  file >> num;
+  std::map<int, Point3> landmarks;
+  landmarks.clear();
+  for (int i = 0; i<num; i++) {
+    int color_id;
+    float x, y, z;
+    file >> color_id >> x >> y >> z;
+    landmarks[color_id] = Point3(x, y, z);
+  }
 
-    file.close();
-    return landmarks;
+  file.close();
+  return landmarks;
 }
 
 /* ************************************************************************* */
 /**
-  * Read pose from file, output by Panda3D.
-  * Warning: row major!!!
-  */
-gtsam::Pose3 readPose(const char* Fn)
-{
-    ifstream poseFile(Fn);
-    if (!poseFile)
-    {
-        cout << "Cannot read pose file: " << Fn << endl;
-        exit(0);
-    }
+ * Read pose from file, output by Panda3D.
+ * Warning: row major!!!
+ */
+gtsam::Pose3 readPose(const char* Fn) {
+  ifstream poseFile(Fn);
+  if (!poseFile) {
+    cout << "Cannot read pose file: " << Fn << endl;
+    exit(0);
+  }
 
-    double v[16];
-    for (int i = 0; i<16; i++)
-        poseFile >> v[i];
-    poseFile.close();
+  double v[16];
+  for (int i = 0; i<16; i++)
+    poseFile >> v[i];
+  poseFile.close();
 
-    // Because panda3d's camera is z-up, y-view,
-    // we swap z and y to have y-up, z-view, then negate z to stick with the right-hand rule
-    //... similar to OpenGL's camera
-    for (int i = 0; i<3; i++)
-    {
-        float t = v[4+i];
-        v[4+i] = v[8+i];
-        v[8+i] = -t;
-    }
+  // Because panda3d's camera is z-up, y-view,
+  // we swap z and y to have y-up, z-view, then negate z to stick with the right-hand rule
+  //... similar to OpenGL's camera
+  for (int i = 0; i<3; i++) {
+    float t = v[4+i];
+    v[4+i] = v[8+i];
+    v[8+i] = -t;
+  }
 
-    ::Vector vec = Vector_(16, v);
+  ::Vector vec = Vector_(16, v);
 
-    Matrix T = Matrix_(4,4, vec);   // column order !!!
+  Matrix T = Matrix_(4,4, vec);   // column order !!!
 
-    Pose3 pose(T);
-    return pose;
+  Pose3 pose(T);
+  return pose;
 }
 /* ************************************************************************* */
-std::map<int, gtsam::Pose3> readPoses(const std::string& baseFolder, const std::string& posesFn)
-{
-    ifstream posesFile((baseFolder+posesFn).c_str());
-    if (!posesFile)
-    {
-        cout << "Cannot read all pose file: " << posesFn << endl;
-        exit(0);
-    }
-    int numPoses;
-    posesFile >> numPoses;
-    map<int, Pose3> poses;
-    for (int i = 0; i<numPoses; i++)
-    {
-        int poseId;
-        posesFile >> poseId;
+std::map<int, gtsam::Pose3> readPoses(const std::string& baseFolder, const std::string& posesFn) {
+  ifstream posesFile((baseFolder+posesFn).c_str());
+  if (!posesFile) {
+    cout << "Cannot read all pose file: " << posesFn << endl;
+    exit(0);
+  }
+  int numPoses;
+  posesFile >> numPoses;
+  map<int, Pose3> poses;
+  for (int i = 0; i<numPoses; i++) {
+    int poseId;
+    posesFile >> poseId;
 
-        string poseFileName;
-        posesFile >> poseFileName;
+    string poseFileName;
+    posesFile >> poseFileName;
 
-        Pose3 pose = readPose((baseFolder+poseFileName).c_str());
-        poses[poseId] = pose;
-    }
+    Pose3 pose = readPose((baseFolder+poseFileName).c_str());
+    poses[poseId] = pose;
+  }
 
-    return poses;
+  return poses;
 }
 
 /* ************************************************************************* */
-gtsam::shared_ptrK readCalibData(const std::string& calibFn)
-{
-    ifstream calibFile(calibFn.c_str());
-    if (!calibFile)
-    {
-        cout << "Cannot read calib file: " << calibFn << endl;
-        exit(0);
-    }
-    int imX, imY;
-    float fx, fy, ox, oy;
-    calibFile >> imX >> imY >> fx >> fy >> ox >> oy;
-    calibFile.close();
+gtsam::shared_ptrK readCalibData(const std::string& calibFn) {
+  ifstream calibFile(calibFn.c_str());
+  if (!calibFile) {
+    cout << "Cannot read calib file: " << calibFn << endl;
+    exit(0);
+  }
+  int imX, imY;
+  float fx, fy, ox, oy;
+  calibFile >> imX >> imY >> fx >> fy >> ox >> oy;
+  calibFile.close();
 
-    return shared_ptrK(new Cal3_S2(fx, fy, 0, ox, oy));   // skew factor = 0
-}
-/* ************************************************************************* */
-std::vector<Feature2D> readFeatures(int pose_id, const char* filename)
-{
-    ifstream file(filename);
-    if (!file)
-    {
-        cout << "Cannot read feature file: " << filename<< endl;
-        exit(0);
-    }
-
-    int numFeatures;
-    file >> numFeatures ;
-
-    std::vector<Feature2D> vFeatures_;
-    for (int i = 0; i < numFeatures; i++)
-    {
-        int landmark_id; double x, y;
-        file >> landmark_id >> x >> y;
-        vFeatures_.push_back(Feature2D(pose_id, landmark_id, Point2(x, y)));
-    }
-
-    file.close();
-    return vFeatures_;
-}
-/* ************************************************************************* */
-std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn)
-{
-    ifstream measurementsFile((baseFolder+measurementsFn).c_str());
-    if (!measurementsFile)
-    {
-        cout << "Cannot read all pose file: " << baseFolder+measurementsFn << endl;
-        exit(0);
-    }
-    int numPoses;
-    measurementsFile >> numPoses;
-
-    vector<Feature2D> allFeatures;
-    allFeatures.clear();
-
-    for (int i = 0; i<numPoses; i++)
-    {
-        int poseId;
-        measurementsFile >> poseId;
-
-        string featureFileName;
-        measurementsFile >> featureFileName;
-        vector<Feature2D> features = readFeatures(poseId, (baseFolder+featureFileName).c_str());
-        allFeatures.insert( allFeatures.end(), features.begin(), features.end() );
-    }
-
-    return allFeatures;
-}
-/* ************************************************************************* */
-std::vector<gtsam::Pose3> readPosesISAM(const std::string& baseFolder, const std::string& posesFn)
-{
-    ifstream posesFile((baseFolder+posesFn).c_str());
-    if (!posesFile)
-    {
-        cout << "Cannot read all pose ISAM file: " << posesFn << endl;
-        exit(0);
-    }
-    int numPoses;
-    posesFile >> numPoses;
-    vector<Pose3> poses;
-    for (int i = 0; i<numPoses; i++)
-    {
-        string poseFileName;
-        posesFile >> poseFileName;
-
-        Pose3 pose = readPose((baseFolder+poseFileName).c_str());
-        poses.push_back(pose);
-    }
-
-    return poses;
+  return shared_ptrK(new Cal3_S2(fx, fy, 0, ox, oy));   // skew factor = 0
 }
 
 /* ************************************************************************* */
-std::vector<std::vector<Feature2D> > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn)
-{
-    ifstream measurementsFile((baseFolder+measurementsFn).c_str());
-    if (!measurementsFile)
-    {
-        cout << "Cannot read all pose file: " << baseFolder+measurementsFn << endl;
-        exit(0);
-    }
-    int numPoses;
-    measurementsFile >> numPoses;
+std::vector<Feature2D> readFeatures(int pose_id, const char* filename) {
+  ifstream file(filename);
+  if (!file) {
+    cout << "Cannot read feature file: " << filename<< endl;
+    exit(0);
+  }
 
-    std::vector<std::vector<Feature2D> > allFeatures;
+  int numFeatures;
+  file >> numFeatures ;
 
-    for (int i = 0; i<numPoses; i++)
-    {
-        string featureFileName;
-        measurementsFile >> featureFileName;
-        vector<Feature2D> features = readFeatures(-1, (baseFolder+featureFileName).c_str());    // we don't care about pose id in ISAM
-        allFeatures.push_back(features);
-    }
+  std::vector<Feature2D> vFeatures_;
+  for (int i = 0; i < numFeatures; i++) {
+    int landmark_id; double x, y;
+    file >> landmark_id >> x >> y;
+    vFeatures_.push_back(Feature2D(pose_id, landmark_id, Point2(x, y)));
+  }
 
-    return allFeatures;
+  file.close();
+  return vFeatures_;
+}
+
+/* ************************************************************************* */
+std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn) {
+  ifstream measurementsFile((baseFolder+measurementsFn).c_str());
+  if (!measurementsFile) {
+    cout << "Cannot read all pose file: " << baseFolder+measurementsFn << endl;
+    exit(0);
+  }
+  int numPoses;
+  measurementsFile >> numPoses;
+
+  vector<Feature2D> allFeatures;
+  allFeatures.clear();
+
+  for (int i = 0; i<numPoses; i++) {
+    int poseId;
+    measurementsFile >> poseId;
+
+    string featureFileName;
+    measurementsFile >> featureFileName;
+    vector<Feature2D> features = readFeatures(poseId, (baseFolder+featureFileName).c_str());
+    allFeatures.insert( allFeatures.end(), features.begin(), features.end() );
+  }
+
+  return allFeatures;
+}
+
+/* ************************************************************************* */
+std::vector<gtsam::Pose3> readPosesISAM(const std::string& baseFolder, const std::string& posesFn) {
+  ifstream posesFile((baseFolder+posesFn).c_str());
+  if (!posesFile) {
+    cout << "Cannot read all pose ISAM file: " << posesFn << endl;
+    exit(0);
+  }
+  int numPoses;
+  posesFile >> numPoses;
+  vector<Pose3> poses;
+  for (int i = 0; i<numPoses; i++) {
+    string poseFileName;
+    posesFile >> poseFileName;
+
+    Pose3 pose = readPose((baseFolder+poseFileName).c_str());
+    poses.push_back(pose);
+  }
+
+  return poses;
+}
+
+/* ************************************************************************* */
+std::vector<std::vector<Feature2D> > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn) {
+  ifstream measurementsFile((baseFolder+measurementsFn).c_str());
+  if (!measurementsFile) {
+    cout << "Cannot read all pose file: " << baseFolder+measurementsFn << endl;
+    exit(0);
+  }
+  int numPoses;
+  measurementsFile >> numPoses;
+
+  std::vector<std::vector<Feature2D> > allFeatures;
+
+  for (int i = 0; i<numPoses; i++) {
+    string featureFileName;
+    measurementsFile >> featureFileName;
+    vector<Feature2D> features = readFeatures(-1, (baseFolder+featureFileName).c_str());    // we don't care about pose id in ISAM
+    allFeatures.push_back(features);
+  }
+
+  return allFeatures;
 }
diff --git a/examples/vSLAMexample/vSLAMutils.h b/examples/vSLAMexample/vSLAMutils.h
index a6543cdd3..0bb1a1193 100644
--- a/examples/vSLAMexample/vSLAMutils.h
+++ b/examples/vSLAMexample/vSLAMutils.h
@@ -1,5 +1,21 @@
-#ifndef LANDMARKUTILS_H
-#define LANDMARKUTILS_H
+/* ----------------------------------------------------------------------------
+
+ * 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 Feature2D.cpp
+ * @brief
+ * @author Duy-Nguyen
+ */
+
+#pragma once
 
 #include <map>
 #include <vector>
@@ -21,7 +37,3 @@ gtsam::shared_ptrK readCalibData(const std::string& calibFn);
 std::vector<Feature2D> readFeatureFile(const char* filename);
 std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn);
 std::vector< std::vector<Feature2D> > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn);
-
-
-
-#endif // LANDMARKUTILS_H