vISAMexample is now working.

examples/vSLAMexample/ISAMLoop-inl.h

@@ -45,16 +45,14 @@ void ISAMLoop<Values>::update(const Factors& newFactors, const Values& initialVa
         }
     }
 
-    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: ");
     }
+
+    cout << "Update ISAM: " << endl;
     isam.update(*linearizedNewFactors);
   }
 }

examples/vSLAMexample/vISAMexample.cpp

@@ -30,10 +30,10 @@ 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"
+#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;
@@ -78,8 +78,8 @@ void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<Values>&
                       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,
@@ -89,10 +89,21 @@ void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<Values>&
                          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] );
+    }
 }
 
 
@@ -109,6 +120,7 @@ int main(int argc, char* argv[])
     }
 
     g_dataFolder = string(argv[1]);
+    g_dataFolder += "/";
     readAllData();
 
     ISAMLoop<Values> isam(3);
@@ -121,12 +133,13 @@ int main(int argc, char* argv[])
                          i, g_poses[i],
                          g_measurements[i], measurementSigma, g_calib);
 
-        cout << "Add prior pose and measurements of camera " << i << endl;
-        newFactors->print();
-        initialValues->print();
+//        cout << "Add prior pose and measurements of camera " << i << endl;
+//        newFactors->print();
+//        initialValues->print();
 
         isam.update(*newFactors, *initialValues);
         Values currentEstimate = isam.estimate();
+        cout << "****************************************************" << endl;
         currentEstimate.print("Current estimate: ");
     }
 

examples/vSLAMexample/vSFMexample.cpp

@@ -118,7 +118,7 @@ int main(int argc, char* argv[])
         exit(0);
     }
 
-    g_dataFolder = string(argv[1]);
+    g_dataFolder = string(argv[1]) + "/";
     readAllData();
 
     // Create a graph using the 2D measurements (features) and the calibration data

examples/vSLAMexample/vSLAMutils.cpp

@@ -69,7 +69,7 @@ gtsam::Pose3 readPose(const char* Fn)
 /* ************************************************************************* */
 std::map<int, gtsam::Pose3> readPoses(const std::string& baseFolder, const std::string& posesFn)
 {
-    ifstream posesFile((baseFolder+"/"+posesFn).c_str());
+    ifstream posesFile((baseFolder+posesFn).c_str());
     if (!posesFile)
     {
         cout << "Cannot read all pose file: " << posesFn << endl;
@@ -86,7 +86,7 @@ std::map<int, gtsam::Pose3> readPoses(const std::string& baseFolder, const std::
         string poseFileName;
         posesFile >> poseFileName;
 
-        Pose3 pose = readPose((baseFolder+"/"+poseFileName).c_str());
+        Pose3 pose = readPose((baseFolder+poseFileName).c_str());
         poses[poseId] = pose;
     }
 
@@ -136,10 +136,10 @@ std::vector<Feature2D> readFeatures(int pose_id, const char* filename)
 /* ************************************************************************* */
 std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn)
 {
-    ifstream measurementsFile((baseFolder+"/"+measurementsFn).c_str());
+    ifstream measurementsFile((baseFolder+measurementsFn).c_str());
     if (!measurementsFile)
     {
-        cout << "Cannot read all pose file: " << measurementsFn << endl;
+        cout << "Cannot read all pose file: " << baseFolder+measurementsFn << endl;
         exit(0);
     }
     int numPoses;
@@ -155,7 +155,7 @@ std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const
 
         string featureFileName;
         measurementsFile >> featureFileName;
-        vector<Feature2D> features = readFeatures(poseId, (baseFolder+"/"+featureFileName).c_str());
+        vector<Feature2D> features = readFeatures(poseId, (baseFolder+featureFileName).c_str());
         allFeatures.insert( allFeatures.end(), features.begin(), features.end() );
     }
 
@@ -191,7 +191,7 @@ std::vector<std::vector<Feature2D> > readAllMeasurementsISAM(const std::string&
     ifstream measurementsFile((baseFolder+measurementsFn).c_str());
     if (!measurementsFile)
     {
-        cout << "Cannot read all pose file: " << measurementsFn << endl;
+        cout << "Cannot read all pose file: " << baseFolder+measurementsFn << endl;
         exit(0);
     }
     int numPoses;

slam/visualSLAM.h

@@ -43,6 +43,8 @@ namespace gtsam { namespace visualSLAM {
   typedef NonlinearEquality<Values, PoseKey> PoseConstraint;
   typedef NonlinearEquality<Values, PointKey> PointConstraint;
   typedef PriorFactor<Values, PoseKey> PosePrior;
+  typedef PriorFactor<Values, PointKey> PointPrior;
+
 
 
 
@@ -190,6 +192,15 @@ namespace gtsam { namespace visualSLAM {
       push_back(factor);
     }
 
+    /**
+     *  Add a prior on a landmark
+     *  @param j index of landmark
+     *  @param model uncertainty model of this prior
+     */
+    void addPointPrior(int j, const Point3& p = Point3(), const SharedGaussian& model = noiseModel::Unit::Create(1)) {
+      boost::shared_ptr<PointPrior> factor(new PointPrior(j, p, model));
+      push_back(factor);
+    }
 
   }; // Graph