diff --git a/examples/vSLAMexample/ISAMLoop-inl.h b/examples/vSLAMexample/ISAMLoop-inl.h
new file mode 100644
index 000000000..d72ad0322
--- /dev/null
+++ b/examples/vSLAMexample/ISAMLoop-inl.h
@@ -0,0 +1,118 @@
+/*
+ * ISAMLoop.cpp
+ *
+ *  Created on: Jan 19, 2010
+ *      Author: Viorela Ila and Richard Roberts
+ */
+
+#pragma once
+
+#include <gtsam/linear/GaussianFactorGraph.h>
+//#include <BayesTree-inl.h>
+#include <gtsam/inference/ISAM-inl.h>
+#include <gtsam/nonlinear/Ordering.h>
+//#include <gtsam/inference/IndexTable.h>
+#include <boost/foreach.hpp>
+
+#include "ISAMLoop.h"
+
+using namespace gtsam;
+
+/* ************************************************************************* */
+template<class Values>
+void ISAMLoop<Values>::update(const Factors& newFactors, const Values& initialValues) {
+  // Reorder and relinearize every reorderInterval updates
+  if(newFactors.size() > 0) {
+    if(reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) {
+      reorder_relinearize();
+      reorderCounter_ = 0;
+    }
+
+    factors_.push_back(newFactors);
+
+//    BOOST_FOREACH(typename Factors::sharedFactor f, newFactors) {
+//      f->print("Adding factor: ");
+//    }
+
+    // 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());
+        }
+    }
+
+    ordering_.print();
+    newFactors.linearize(linPoint_, ordering_);
+    cout << "Don linearize!" << endl;
+
+    boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors(newFactors.linearize(linPoint_, ordering_));
+
+    cout << "After linearize: " << endl;
+    BOOST_FOREACH(GaussianFactorGraph::sharedFactor f, *linearizedNewFactors) {
+      f->print("Linearized factor: ");
+    }
+    isam.update(*linearizedNewFactors);
+  }
+}
+
+/* ************************************************************************* */
+template<class Values>
+void ISAMLoop<Values>::reorder_relinearize() {
+
+  //cout << "Reordering " << reorderCounter_;
+
+  cout << "Reordering, relinearizing..." << endl;
+
+  // Obtain the new linearization point
+  const Values newLinPoint = estimate();
+
+  isam.clear();
+
+  // Compute an ordering
+  ordering_ = *factors_.orderingCOLAMD(newLinPoint);
+
+//  cout << "Got estimate" << endl;
+//  newLinPoint.print("newLinPoint");
+//  factors_.print("factors");
+
+  // Create a linear factor graph at the new linearization point
+  boost::shared_ptr<GaussianFactorGraph> gfg(factors_.linearize(newLinPoint, ordering_));
+
+  // Just recreate the whole BayesTree
+  isam.update(*gfg);
+
+  //cout << "Reeliminating..." << endl;
+
+//  // Eliminate linear factor graph to a BayesNet with colamd ordering
+//  Ordering ordering = gfg->getOrdering();
+//  const BayesNet<GaussianConditional> bn(
+//      eliminate<GaussianFactor, GaussianConditional>(*gfg, ordering));
+//
+////  cout << "Rebuilding BayesTree..." << endl;
+//
+//  // Replace the BayesTree with a new one
+//  isam.clear();
+//  BOOST_REVERSE_FOREACH(const GaussianISAM::sharedConditional c, bn) {
+//    isam.insert(c, ordering);
+//  }
+
+  linPoint_ = newLinPoint;
+
+//  cout << "Done!" << endl;
+}
+
+/* ************************************************************************* */
+template<class Values>
+Values ISAMLoop<Values>::estimate() {
+//  cout << "ISAMLoop::estimate(): " << endl;
+//  linPoint_.print("linPoint_");
+//  isam.print("isam");
+  if(isam.size() > 0)
+    return linPoint_.expmap(optimize(isam), ordering_);
+  else
+    return linPoint_;
+}
diff --git a/examples/vSLAMexample/ISAMLoop.h b/examples/vSLAMexample/ISAMLoop.h
new file mode 100644
index 000000000..424dfdeea
--- /dev/null
+++ b/examples/vSLAMexample/ISAMLoop.h
@@ -0,0 +1,68 @@
+/*
+ * ISAMLoop.h
+ *
+ *  Created on: Jan 19, 2010
+ *      Author: Viorela Ila and Richard Roberts
+ */
+
+#pragma once
+
+#include <gtsam/linear/GaussianConditional.h>
+#include <gtsam/nonlinear/Ordering.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/linear/GaussianISAM.h>
+
+
+template<class Values>
+class ISAMLoop {
+public:
+
+  typedef gtsam::NonlinearFactorGraph<Values> Factors;
+
+public:
+//protected:
+
+  /** The internal iSAM object */
+  gtsam::GaussianISAM isam;
+
+  /** The current linearization point */
+  Values linPoint_;
+
+  /** The ordering */
+  gtsam::Ordering ordering_;
+
+  /** The original factors, used when relinearizing */
+  Factors factors_;
+
+  /** The reordering interval and counter */
+  int reorderInterval_;
+  int reorderCounter_;
+
+
+public:
+  ISAMLoop() : reorderInterval_(0), reorderCounter_(0) {}
+
+  /** Periodically reorder and relinearize */
+  ISAMLoop(int reorderInterval) : reorderInterval_(reorderInterval), reorderCounter_(0) {}
+
+  /** Add new factors along with their initial linearization points */
+  void update(const Factors& newFactors, const Values& initialValues);
+
+  /** Return the current solution estimate */
+  Values estimate();
+  Values calculateEstimate() { return estimate(); }
+
+  /** Return the current linearization point */
+  const Values& getLinearizationPoint() { return linPoint_; }
+
+  /** Get the ordering */
+  const gtsam::Ordering& getOrdering() const { return ordering_; }
+
+  const Factors& getFactorsUnsafe() { return factors_; }
+
+  /**
+   * Relinearization and reordering of variables
+   */
+  void reorder_relinearize();
+
+};
diff --git a/examples/vSLAMexample/vISAMexample.cpp b/examples/vSLAMexample/vISAMexample.cpp
new file mode 100644
index 000000000..8e466c5b9
--- /dev/null
+++ b/examples/vSLAMexample/vISAMexample.cpp
@@ -0,0 +1,136 @@
+/**
+ * @file    vSLAMexample.cpp
+ * @brief   An vSLAM example for synthesis sequence
+ * single camera
+ * @author  Duy-Nguyen
+ */
+
+#include <boost/shared_ptr.hpp>
+using namespace boost;
+
+#define GTSAM_MAGIC_KEY
+
+#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
+#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
+#include <gtsam/inference/graph-inl.h>
+#include <gtsam/slam/visualSLAM.h>
+#include <gtsam/slam/PriorFactor.h>
+
+#include <gtsam/inference/ISAM-inl.h>
+#include <gtsam/linear/GaussianISAM.h>
+#include "ISAMLoop.h"
+#include "ISAMLoop-inl.h"
+
+#include "vSLAMutils.h"
+#include "Feature2D.h"
+
+using namespace std;
+using namespace gtsam;
+using namespace gtsam::visualSLAM;
+using namespace boost;
+
+/* ************************************************************************* */
+#define CALIB_FILE          "/calib.txt"
+#define LANDMARKS_FILE      "/landmarks.txt"
+#define POSES_FILE          "/posesISAM.txt"
+#define MEASUREMENTS_FILE    "/measurementsISAM.txt"
+
+// Base data folder
+string g_dataFolder;
+
+// Store groundtruth values, read from files
+shared_ptrK g_calib;
+map<int, Point3> g_landmarks;        // map: <landmark_id, landmark_position>
+std::vector<Pose3> g_poses;            // map: <camera_id, pose>
+std::vector<std::vector<Feature2D> > g_measurements;    // map: <camera_id, detected_features> -- where: Feature2D: {camera_id, landmark_id, 2d feature_position}
+
+// Noise models
+SharedGaussian measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f));
+SharedGaussian poseSigma(noiseModel::Unit::Create(1));
+
+
+/* ************************************************************************* */
+/**
+  * 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 groundtruth landmarks' positions. These will be used later as intial estimates 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 all 2d measurements. Those will become factors linking their associating pose and the corresponding landmark.
+    g_measurements = readAllMeasurementsISAM(g_dataFolder, MEASUREMENTS_FILE);
+}
+
+/* ************************************************************************* */
+/**
+  * Setup vSLAM graph
+  * by adding and linking 2D features (measurements) detected in each captured image
+  * with their corresponding landmarks.
+  */
+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)
+{
+    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);
+    }
+
+    newFactors->addPosePrior(pose_id, pose, poseSigma);
+
+    initialValues = shared_ptr<Values>(new Values());
+    initialValues->insert(pose_id, pose);
+}
+
+
+/* ************************************************************************* */
+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);
+    }
+
+    g_dataFolder = string(argv[1]);
+    readAllData();
+
+    ISAMLoop<Values> isam(3);
+
+    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);
+
+        cout << "Add prior pose and measurements of camera " << i << endl;
+        newFactors->print();
+        initialValues->print();
+
+        isam.update(*newFactors, *initialValues);
+        Values currentEstimate = isam.estimate();
+        currentEstimate.print("Current estimate: ");
+    }
+
+}
+/* ************************************************************************* */
+
+
diff --git a/examples/vSLAMexample/vSFMexample.cpp b/examples/vSLAMexample/vSFMexample.cpp
new file mode 100644
index 000000000..b1d9c0144
--- /dev/null
+++ b/examples/vSLAMexample/vSFMexample.cpp
@@ -0,0 +1,148 @@
+/**
+ * @file    vSLAMexample.cpp
+ * @brief   An vSLAM example for synthesis sequence
+ * single camera
+ * @author  Duy-Nguyen
+ */
+
+#include <boost/shared_ptr.hpp>
+using namespace boost;
+
+#define GTSAM_MAGIC_KEY
+
+#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
+#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
+#include <gtsam/inference/graph-inl.h>
+#include <gtsam/slam/visualSLAM.h>
+#include <gtsam/slam/PriorFactor.h>
+
+#include "vSLAMutils.h"
+#include "Feature2D.h"
+
+using namespace std;
+using namespace gtsam;
+using namespace gtsam::visualSLAM;
+using namespace boost;
+
+/* ************************************************************************* */
+#define CALIB_FILE          "calib.txt"
+#define LANDMARKS_FILE      "landmarks.txt"
+#define POSES_FILE          "poses.txt"
+#define MEASUREMENTS_FILE    "measurements.txt"
+
+// Base data folder
+string g_dataFolder;
+
+// Store groundtruth values, read from files
+shared_ptrK g_calib;
+map<int, Point3> g_landmarks;        // map: <landmark_id, landmark_position>
+map<int, Pose3> g_poses;            // map: <camera_id, pose>
+std::vector<Feature2D> g_measurements;    // Feature2D: {camera_id, landmark_id, 2d feature_position}
+
+// Noise models
+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 groundtruth landmarks' positions. These will be used later as intial estimates 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 = 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;    
+
+    cout << "Built graph: " << endl;
+    for (size_t i= 0; i<measurements.size(); i++)
+    {
+        measurements[i].print();
+
+        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;
+
+    // Initialize landmarks 3D positions.
+    for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
+        initValues.insert( lmit->first, lmit->second );
+
+    // 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);
+    }
+
+    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 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));
+
+    // 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: ");
+
+}
+/* ************************************************************************* */
+
diff --git a/examples/vSLAMexample/vSLAMutils.cpp b/examples/vSLAMexample/vSLAMutils.cpp
new file mode 100644
index 000000000..a62bb613d
--- /dev/null
+++ b/examples/vSLAMexample/vSLAMutils.cpp
@@ -0,0 +1,211 @@
+#include "vSLAMutils.h"
+#include <fstream>
+#include <cstdio>
+
+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);
+    }
+
+    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;
+}
+
+/* ************************************************************************* */
+/**
+  * 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();
+
+    // 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);
+
+    Matrix T = Matrix_(4,4, vec);   // column order !!!
+
+    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;
+
+        string poseFileName;
+        posesFile >> poseFileName;
+
+        Pose3 pose = readPose((baseFolder+"/"+poseFileName).c_str());
+        poses[poseId] = pose;
+    }
+
+    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();
+
+    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 (size_t 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: " << 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: " << 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
new file mode 100644
index 000000000..a6543cdd3
--- /dev/null
+++ b/examples/vSLAMexample/vSLAMutils.h
@@ -0,0 +1,27 @@
+#ifndef LANDMARKUTILS_H
+#define LANDMARKUTILS_H
+
+#include <map>
+#include <vector>
+#include "Feature2D.h"
+#include "gtsam/geometry/Pose3.h"
+#include "gtsam/geometry/Point3.h"
+#include "gtsam/geometry/Cal3_S2.h"
+
+
+std::map<int, gtsam::Point3> readLandMarks(const std::string& landmarkFile);
+
+gtsam::Pose3 readPose(const char* poseFn);
+std::map<int, gtsam::Pose3> readPoses(const std::string& baseFolder, const std::string& posesFN);
+std::vector<gtsam::Pose3> readPosesISAM(const std::string& baseFolder, const std::string& posesFN);
+
+
+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