updated visual SLAM examples to follow correct coordinate system conventions. The ISAM example now also uses the same input files as the general SFM example.

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

examples/Data/measurementsISAM.txt

@@ -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
-

examples/Data/posesISAM.txt

@@ -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
-

examples/Data/ttpy10.pose

@@ -1,4 +1,4 @@
-0.939693 0.34202 0 0
+0.93969 0.24185 -0.24185 34.202
--0.241845 0.664463 -0.707107 0
+0.34202 -0.66446 0.66446 -93.969
--0.241845 0.664463 0.707107 0
+0 -0.70711 -0.70711 100
-34.202 -93.9693 100 1
+0 0 0 1

examples/Data/ttpy100.pose

@@ -1,4 +1,4 @@
--0.939693 -0.34202 0 0
+-0.93969 -0.24185 0.24185 -34.202
-0.241845 -0.664463 -0.707107 0
+-0.34202 0.66446 -0.66446 93.969
-0.241845 -0.664463 0.707107 0
+0 -0.70711 -0.70711 100
--34.202 93.9693 100 1
+0 0 0 1

examples/Data/ttpy20.pose

@@ -1,4 +1,4 @@
-0.766044 0.642788 0 0
+0.76604 0.45452 -0.45452 64.279
--0.454519 0.541675 -0.707107 0
+0.64279 -0.54168 0.54168 -76.604
--0.454519 0.541675 0.707107 0
+0 -0.70711 -0.70711 100
-64.2788 -76.6044 100 1
+0 0 0 1

examples/Data/ttpy30.pose

@@ -1,4 +1,4 @@
-0.5 0.866025 0 0
+0.5 0.61237 -0.61237 86.603
--0.612372 0.353553 -0.707107 0
+0.86603 -0.35355 0.35355 -50
--0.612372 0.353553 0.707107 0
+0 -0.70711 -0.70711 100
-86.6025 -50 100 1
+0 0 0 1

examples/Data/ttpy40.pose

@@ -1,4 +1,4 @@
-0.173648 0.984808 0 0
+0.17365 0.69636 -0.69636 98.481
--0.696364 0.122788 -0.707107 0
+0.98481 -0.12279 0.12279 -17.365
--0.696364 0.122788 0.707107 0
+0 -0.70711 -0.70711 100
-98.4808 -17.3648 100 1
+0 0 0 1

examples/Data/ttpy50.pose

@@ -1,4 +1,4 @@
--0.173648 0.984808 0 0
+-0.17365 0.69636 -0.69636 98.481
--0.696364 -0.122788 -0.707107 0
+0.98481 0.12279 -0.12279 17.365
--0.696364 -0.122788 0.707107 0
+0 -0.70711 -0.70711 100
-98.4808 17.3648 100 1
+0 0 0 1

examples/Data/ttpy60.pose

@@ -1,4 +1,4 @@
--0.5 0.866025 0 0
+-0.5 0.61237 -0.61237 86.603
--0.612372 -0.353553 -0.707107 0
+0.86603 0.35355 -0.35355 50
--0.612372 -0.353553 0.707107 0
+0 -0.70711 -0.70711 100
-86.6025 50 100 1
+0 0 0 1

examples/Data/ttpy70.pose

@@ -1,4 +1,4 @@
--0.766044 0.642788 0 0
+-0.76604 0.45452 -0.45452 64.279
--0.454519 -0.541675 -0.707107 0
+0.64279 0.54168 -0.54168 76.604
--0.454519 -0.541675 0.707107 0
+0 -0.70711 -0.70711 100
-64.2788 76.6044 100 1
+0 0 0 1

examples/Data/ttpy80.pose

@@ -1,4 +1,4 @@
--0.939693 0.34202 0 0
+-0.93969 0.24185 -0.24185 34.202
--0.241845 -0.664463 -0.707107 0
+0.34202 0.66446 -0.66446 93.969
--0.241845 -0.664463 0.707107 0
+0 -0.70711 -0.70711 100
-34.202 93.9693 100 1
+0 0 0 1

examples/Data/ttpy90.pose

@@ -1,4 +1,4 @@
--1 0 0 0
+-1 -0 0 0
-0 -0.707107 -0.707107 0
+0 0.70711 -0.70711 100
-0 -0.707107 0.707107 0
+0 -0.70711 -0.70711 100
-0 100 100 1
+0 0 0 1

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 POSES_FILE          "poses.txt"
-#define MEASUREMENTS_FILE    "measurementsISAM.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
+map<int, Pose3> g_poses;            // map: <camera_id, pose>
-std::vector<std::vector<Feature2D> > g_measurements;    // feature sets detected at each frame
+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();

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,
+  Matrix T = Matrix_(4,4, v);   // row order !!!
-  // 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;
@@ -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) {
+std::map<int, std::vector<Feature2D> > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn) {
-  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;
@@ -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
+    vector<Feature2D> features = readFeatures(poseId, (baseFolder+featureFileName).c_str());
-    allFeatures.push_back(features);
+    allFeatures[poseId] = features;
   }
 
   return allFeatures;

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);