diff --git a/examples/Data/calib.txt b/examples/Data/calib.txt
index 8eeb7e656..cb8b77384 100644
--- a/examples/Data/calib.txt
+++ b/examples/Data/calib.txt
@@ -1 +1 @@
-800   600   -1119.61507797   1119.61507797  399.5   299.5
+800   600   1119.61507797   1119.61507797  399.5   299.5
diff --git a/examples/Data/measurementsISAM.txt b/examples/Data/measurementsISAM.txt
deleted file mode 100644
index 7325c7dd9..000000000
--- a/examples/Data/measurementsISAM.txt
+++ /dev/null
@@ -1,12 +0,0 @@
-10
-ttpy10.feat
-ttpy20.feat
-ttpy30.feat
-ttpy40.feat
-ttpy50.feat
-ttpy60.feat
-ttpy70.feat
-ttpy80.feat
-ttpy90.feat
-ttpy100.feat
-
diff --git a/examples/Data/posesISAM.txt b/examples/Data/posesISAM.txt
deleted file mode 100644
index ebe39c6f2..000000000
--- a/examples/Data/posesISAM.txt
+++ /dev/null
@@ -1,12 +0,0 @@
-10
-ttpy10.pose
-ttpy20.pose
-ttpy30.pose
-ttpy40.pose
-ttpy50.pose
-ttpy60.pose
-ttpy70.pose
-ttpy80.pose
-ttpy90.pose
-ttpy100.pose
-
diff --git a/examples/Data/ttpy10.pose b/examples/Data/ttpy10.pose
index ee0f6231a..b9226c723 100644
--- a/examples/Data/ttpy10.pose
+++ b/examples/Data/ttpy10.pose
@@ -1,4 +1,4 @@
-0.939693 0.34202 0 0
--0.241845 0.664463 -0.707107 0
--0.241845 0.664463 0.707107 0
-34.202 -93.9693 100 1
+0.93969 0.24185 -0.24185 34.202
+0.34202 -0.66446 0.66446 -93.969
+0 -0.70711 -0.70711 100
+0 0 0 1
diff --git a/examples/Data/ttpy100.pose b/examples/Data/ttpy100.pose
index 4225e3d4e..cbd21233f 100644
--- a/examples/Data/ttpy100.pose
+++ b/examples/Data/ttpy100.pose
@@ -1,4 +1,4 @@
--0.939693 -0.34202 0 0
-0.241845 -0.664463 -0.707107 0
-0.241845 -0.664463 0.707107 0
--34.202 93.9693 100 1
+-0.93969 -0.24185 0.24185 -34.202
+-0.34202 0.66446 -0.66446 93.969
+0 -0.70711 -0.70711 100
+0 0 0 1
diff --git a/examples/Data/ttpy20.pose b/examples/Data/ttpy20.pose
index f8b252855..bba2606d0 100644
--- a/examples/Data/ttpy20.pose
+++ b/examples/Data/ttpy20.pose
@@ -1,4 +1,4 @@
-0.766044 0.642788 0 0
--0.454519 0.541675 -0.707107 0
--0.454519 0.541675 0.707107 0
-64.2788 -76.6044 100 1
+0.76604 0.45452 -0.45452 64.279
+0.64279 -0.54168 0.54168 -76.604
+0 -0.70711 -0.70711 100
+0 0 0 1
diff --git a/examples/Data/ttpy30.pose b/examples/Data/ttpy30.pose
index f7ad9f674..737e391f2 100644
--- a/examples/Data/ttpy30.pose
+++ b/examples/Data/ttpy30.pose
@@ -1,4 +1,4 @@
-0.5 0.866025 0 0
--0.612372 0.353553 -0.707107 0
--0.612372 0.353553 0.707107 0
-86.6025 -50 100 1
+0.5 0.61237 -0.61237 86.603
+0.86603 -0.35355 0.35355 -50
+0 -0.70711 -0.70711 100
+0 0 0 1
diff --git a/examples/Data/ttpy40.pose b/examples/Data/ttpy40.pose
index e9c9cd251..4e241c130 100644
--- a/examples/Data/ttpy40.pose
+++ b/examples/Data/ttpy40.pose
@@ -1,4 +1,4 @@
-0.173648 0.984808 0 0
--0.696364 0.122788 -0.707107 0
--0.696364 0.122788 0.707107 0
-98.4808 -17.3648 100 1
+0.17365 0.69636 -0.69636 98.481
+0.98481 -0.12279 0.12279 -17.365
+0 -0.70711 -0.70711 100
+0 0 0 1
diff --git a/examples/Data/ttpy50.pose b/examples/Data/ttpy50.pose
index 9c84b54f3..5a57b10ef 100644
--- a/examples/Data/ttpy50.pose
+++ b/examples/Data/ttpy50.pose
@@ -1,4 +1,4 @@
--0.173648 0.984808 0 0
--0.696364 -0.122788 -0.707107 0
--0.696364 -0.122788 0.707107 0
-98.4808 17.3648 100 1
+-0.17365 0.69636 -0.69636 98.481
+0.98481 0.12279 -0.12279 17.365
+0 -0.70711 -0.70711 100
+0 0 0 1
diff --git a/examples/Data/ttpy60.pose b/examples/Data/ttpy60.pose
index 9c0a8c942..4e5455a0d 100644
--- a/examples/Data/ttpy60.pose
+++ b/examples/Data/ttpy60.pose
@@ -1,4 +1,4 @@
--0.5 0.866025 0 0
--0.612372 -0.353553 -0.707107 0
--0.612372 -0.353553 0.707107 0
-86.6025 50 100 1
+-0.5 0.61237 -0.61237 86.603
+0.86603 0.35355 -0.35355 50
+0 -0.70711 -0.70711 100
+0 0 0 1
diff --git a/examples/Data/ttpy70.pose b/examples/Data/ttpy70.pose
index 0c2254de6..608e9ff7c 100644
--- a/examples/Data/ttpy70.pose
+++ b/examples/Data/ttpy70.pose
@@ -1,4 +1,4 @@
--0.766044 0.642788 0 0
--0.454519 -0.541675 -0.707107 0
--0.454519 -0.541675 0.707107 0
-64.2788 76.6044 100 1
+-0.76604 0.45452 -0.45452 64.279
+0.64279 0.54168 -0.54168 76.604
+0 -0.70711 -0.70711 100
+0 0 0 1
diff --git a/examples/Data/ttpy80.pose b/examples/Data/ttpy80.pose
index 9d6491304..8a6ae4cd1 100644
--- a/examples/Data/ttpy80.pose
+++ b/examples/Data/ttpy80.pose
@@ -1,4 +1,4 @@
--0.939693 0.34202 0 0
--0.241845 -0.664463 -0.707107 0
--0.241845 -0.664463 0.707107 0
-34.202 93.9693 100 1
+-0.93969 0.24185 -0.24185 34.202
+0.34202 0.66446 -0.66446 93.969
+0 -0.70711 -0.70711 100
+0 0 0 1
diff --git a/examples/Data/ttpy90.pose b/examples/Data/ttpy90.pose
index b51c1a58f..863f71fea 100644
--- a/examples/Data/ttpy90.pose
+++ b/examples/Data/ttpy90.pose
@@ -1,4 +1,4 @@
--1 0 0 0
-0 -0.707107 -0.707107 0
-0 -0.707107 0.707107 0
-0 100 100 1
+-1 -0 0 0
+0 0.70711 -0.70711 100
+0 -0.70711 -0.70711 100
+0 0 0 1
diff --git a/examples/vSLAMexample/vISAMexample.cpp b/examples/vSLAMexample/vISAMexample.cpp
index ed3a68ba8..bd35e3636 100644
--- a/examples/vSLAMexample/vISAMexample.cpp
+++ b/examples/vSLAMexample/vISAMexample.cpp
@@ -17,6 +17,7 @@
  */
 
 #include <boost/shared_ptr.hpp>
+#include <boost/foreach.hpp>
 using namespace boost;
 
 // Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
@@ -40,8 +41,8 @@ using namespace boost;
 /* ************************************************************************* */
 #define CALIB_FILE          "calib.txt"
 #define LANDMARKS_FILE      "landmarks.txt"
-#define POSES_FILE          "posesISAM.txt"
-#define MEASUREMENTS_FILE    "measurementsISAM.txt"
+#define POSES_FILE          "poses.txt"
+#define MEASUREMENTS_FILE    "measurements.txt"
 
 // Base data folder
 string g_dataFolder;
@@ -49,8 +50,8 @@ 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;          // prior camera poses at each frame
-std::vector<std::vector<Feature2D> > g_measurements;    // feature sets detected at each frame
+map<int, Pose3> g_poses;            // map: <camera_id, pose>
+std::map<int, std::vector<Feature2D> > g_measurements;    // feature sets detected at each frame
 
 // Noise models
 SharedNoiseModel measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f));
@@ -69,7 +70,7 @@ void readAllDataISAM() {
   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);
+  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 = readAllMeasurementsISAM(g_dataFolder, MEASUREMENTS_FILE);
@@ -80,7 +81,7 @@ void readAllDataISAM() {
  * Setup newFactors and initialValues for each new pose and set of measurements at each frame.
  */
 void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<visualSLAM::Values>& initialValues,
-    int pose_id, Pose3& pose, std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) {
+    int pose_id, const Pose3& pose, const std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) {
 
   // Create a graph of newFactors with new measurements
   newFactors = shared_ptr<Graph> (new Graph());
@@ -121,16 +122,19 @@ int main(int argc, char* argv[]) {
   g_dataFolder = string(argv[1]) + "/";
   readAllDataISAM();
 
-  // Create an NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates
+  // Create a NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates
   int relinearizeInterval = 3;
   NonlinearISAM<visualSLAM::Values> isam(relinearizeInterval);
 
-  // At each frame i with new camera pose and new set of measurements associated with it,
+  // At each frame (poseId) with new camera pose and set of associated measurements,
   // create a graph of new factors and update ISAM
-  for (size_t i = 0; i < g_measurements.size(); i++) {
+  typedef std::map<int, std::vector<Feature2D> > FeatureMap;
+  BOOST_FOREACH(const FeatureMap::value_type& features, g_measurements) {
+    const int poseId = features.first;
     shared_ptr<Graph> newFactors;
     shared_ptr<visualSLAM::Values> initialValues;
-    createNewFactors(newFactors, initialValues, i, g_poses[i], g_measurements[i], measurementSigma, g_calib);
+    createNewFactors(newFactors, initialValues, poseId, g_poses[poseId],
+            features.second, measurementSigma, g_calib);
 
     isam.update(*newFactors, *initialValues);
     visualSLAM::Values currentEstimate = isam.estimate();
diff --git a/examples/vSLAMexample/vSLAMutils.cpp b/examples/vSLAMexample/vSLAMutils.cpp
index ae9042f2a..1c3912f2d 100644
--- a/examples/vSLAMexample/vSLAMutils.cpp
+++ b/examples/vSLAMexample/vSLAMutils.cpp
@@ -46,10 +46,6 @@ std::map<int, Point3> readLandMarks(const std::string& landmarkFile) {
 }
 
 /* ************************************************************************* */
-/**
- * Read pose from file, output by Panda3D.
- * Warning: row major!!!
- */
 gtsam::Pose3 readPose(const char* Fn) {
   ifstream poseFile(Fn);
   if (!poseFile) {
@@ -62,18 +58,7 @@ gtsam::Pose3 readPose(const char* Fn) {
     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 !!!
+  Matrix T = Matrix_(4,4, v);   // row order !!!
 
   Pose3 pose(T);
   return pose;
@@ -166,28 +151,7 @@ std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const
 }
 
 /* ************************************************************************* */
-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) {
+std::map<int, 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;
@@ -196,13 +160,16 @@ std::vector<std::vector<Feature2D> > readAllMeasurementsISAM(const std::string&
   int numPoses;
   measurementsFile >> numPoses;
 
-  std::vector<std::vector<Feature2D> > allFeatures;
+  std::map<int, std::vector<Feature2D> > allFeatures;
 
   for (int i = 0; i<numPoses; i++) {
+    int poseId;
+    measurementsFile >> poseId;
+
     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);
+    vector<Feature2D> features = readFeatures(poseId, (baseFolder+featureFileName).c_str());
+    allFeatures[poseId] = features;
   }
 
   return allFeatures;
diff --git a/examples/vSLAMexample/vSLAMutils.h b/examples/vSLAMexample/vSLAMutils.h
index f1483cd70..52d492224 100644
--- a/examples/vSLAMexample/vSLAMutils.h
+++ b/examples/vSLAMexample/vSLAMutils.h
@@ -29,11 +29,9 @@ 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);
+std::map<int, std::vector<Feature2D> > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn);