Fix vSLAMexample compilation error. Update to new Optimizer interface.

Fix data reading interface. Now the program requires a Data folder to be specified.

examples/Makefile.am

@@ -16,7 +16,7 @@ noinst_PROGRAMS += PlanarSLAMExample   # Solves SLAM example from tutorial by us
 noinst_PROGRAMS += Pose2SLAMExample    # Solves SLAM example from tutorial by using planarSLAM
 noinst_PROGRAMS += PlanarSLAMSelfContained   # Solves SLAM example from tutorial with all typedefs in the script
 noinst_PROGRAMS += Pose2SLAMwSPCG      # Solves a simple SLAM example with SPCG solver
-#SUBDIRS = vSLAMexample # does not compile....
+SUBDIRS = vSLAMexample # does not compile....
 #----------------------------------------------------------------------------------------------------
 # rules to build local programs
 #----------------------------------------------------------------------------------------------------

examples/vSLAMexample/Data/measurements.txt

@@ -0,0 +1,12 @@
+10
+1 ttpy10.feat
+2 ttpy20.feat
+3 ttpy30.feat
+4 ttpy40.feat
+5 ttpy50.feat
+6 ttpy60.feat
+7 ttpy70.feat
+8 ttpy80.feat
+9 ttpy90.feat
+10 ttpy100.feat
+

examples/vSLAMexample/Data/poses.txt

@@ -0,0 +1,12 @@
+10
+1 ttpy10.pose
+2 ttpy20.pose
+3 ttpy30.pose
+4 ttpy40.pose
+5 ttpy50.pose
+6 ttpy60.pose
+7 ttpy70.pose
+8 ttpy80.pose
+9 ttpy90.pose
+10 ttpy100.pose
+

examples/vSLAMexample/Feature2D.h

@@ -8,15 +8,20 @@ class Feature2D
 {
 public:
     gtsam::Point2 m_p;
-    int m_id;       // id of the 3D landmark that it is associated with
+		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 id, gtsam::Point2 p):m_id(id), m_p(p) {};
+		Feature2D(int idCamera, int idLandmark, gtsam::Point2 p)
+						:m_idCamera(idCamera),
+						m_idLandmark(idLandmark),
+						m_p(p)
+		{};
 
     void print(const std::string& s = "") const
     {
         std::cout << s << std::endl;
-        std::cout << "Id: " << m_id << std::endl;
+				std::cout << "Pose id: " << m_idCamera << " -- Landmark id: "  << m_idLandmark << std::endl;
-        m_p.print();
+				m_p.print("\tMeasurement: ");
     }
 };
 

examples/vSLAMexample/Makefile.am

@@ -17,10 +17,12 @@ vSLAMexample_SOURCES = vSLAMexample.cpp Feature2D.cpp landmarkUtils.cpp
 #----------------------------------------------------------------------------------------------------
 # rules to build local programs
 #----------------------------------------------------------------------------------------------------
-AM_CPPFLAGS = -I$(SparseInc) -I$(top_srcdir)/..
+AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(CCOLAMDInc) -I$(top_srcdir)/..
+AM_LDFLAGS = $(BOOST_LDFLAGS)
 LDADD = ../../libgtsam.la
 AM_DEFAULT_SOURCE_EXT = .cpp
 
+
 # rule to run an executable
 %.run: % $(LDADD)
 	./$^

examples/vSLAMexample/landmarkUtils.cpp

@@ -6,9 +6,9 @@ using namespace gtsam;
 using namespace std;
 
 /* ************************************************************************* */
-bool readLandMarks(const char* landmarkFile, std::map<int, Point3>& landmarks)
+std::map<int, Point3> readLandMarks(const std::string& landmarkFile)
 {	
-    ifstream file(landmarkFile);
+		ifstream file(landmarkFile.c_str());
     if (!file) {
         cout << "Cannot read landmark file: " << landmarkFile << endl;
         exit(0);
@@ -16,6 +16,7 @@ bool readLandMarks(const char* landmarkFile, std::map<int, Point3>& landmarks)
 
     int num;
     file >> num;
+		std::map<int, Point3> landmarks;
     landmarks.clear();
     for (int i = 0; i<num; i++)
     {
@@ -26,7 +27,7 @@ bool readLandMarks(const char* landmarkFile, std::map<int, Point3>& landmarks)
     }
 
     file.close();
-    return true;
+		return landmarks;
 }
 
 /* ************************************************************************* */
@@ -66,17 +67,36 @@ gtsam::Pose3 readPose(const char* Fn)
     return pose;
 }
 /* ************************************************************************* */
-gtsam::Pose3 readPose(const char* poseFn_pre, const char* poseFn_suf, int poseId)
+std::map<int, gtsam::Pose3> readPoses(const std::string& baseFolder, const std::string& posesFn)
 {
-    char poseFn[128];
+		ifstream posesFile((baseFolder+posesFn).c_str());
-    sprintf(poseFn, "%s%d%s", poseFn_pre, poseId, poseFn_suf);
+		if (!posesFile)
-    return readPose(poseFn);
+		{
+				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::Cal3_S2 readCalibData(const char* calibFn)
+gtsam::shared_ptrK readCalibData(const std::string& calibFn)
 {
-    ifstream calibFile(calibFn);
+		ifstream calibFile(calibFn.c_str());
     if (!calibFile)
     {
         cout << "Cannot read calib file: " << calibFn << endl;
@@ -87,11 +107,10 @@ gtsam::Cal3_S2 readCalibData(const char* calibFn)
     calibFile >> imX >> imY >> fx >> fy >> ox >> oy;
     calibFile.close();
 
-    Cal3_S2 K(fx, fy, 0, ox, oy);   // skew factor = 0
+		return shared_ptrK(new Cal3_S2(fx, fy, 0, ox, oy));   // skew factor = 0
-    return K;
 }
 /* ************************************************************************* */
-std::vector<Feature2D> readFeatures(const char* filename)
+std::vector<Feature2D> readFeatures(int pose_id, const char* filename)
 {
     ifstream file(filename);
     if (!file)
@@ -106,18 +125,39 @@ std::vector<Feature2D> readFeatures(const char* filename)
     std::vector<Feature2D> vFeatures_;
     for (size_t i = 0; i < numFeatures; i++)
     {
-        int id; double x, y;
+				int landmark_id; double x, y;
-        file >> id >> x >> y;
+				file >> landmark_id >> x >> y;
-        vFeatures_.push_back(Feature2D(id, Point2(x, y)));
+				vFeatures_.push_back(Feature2D(pose_id, landmark_id, Point2(x, y)));
     }
 
     file.close();
     return vFeatures_;
 }
 /* ************************************************************************* */
-std::vector<Feature2D> readFeatures(const char* featFn_pre, const char* featFn_suf, int imageId)
+std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn)
+{;
+		ifstream measurementsFile((baseFolder+measurementsFn).c_str());
+		if (!measurementsFile)
 		{
-    char featFn[128];
+				cout << "Cannot read all pose file: " << measurementsFn << endl;
-    sprintf(featFn, "%s%d%s", featFn_pre, imageId, featFn_suf);
+				exit(0);
-    return readFeatures(featFn);
+		}
+		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;
 }

examples/vSLAMexample/landmarkUtils.h

@@ -9,15 +9,15 @@
 #include "gtsam/geometry/Cal3_S2.h"
 
 
-bool readLandMarks(const char* landmarkFile, std::map<int, gtsam::Point3>& landmarks);
+std::map<int, gtsam::Point3> readLandMarks(const std::string& landmarkFile);
 
 gtsam::Pose3 readPose(const char* poseFn);
-gtsam::Pose3 readPose(const char* poseFn_pre, const char* poseFn_suf, int poseId);
+std::map<int, gtsam::Pose3> readPoses(const std::string& baseFolder, const std::string& posesFN);
 
-gtsam::Cal3_S2 readCalibData(const char* calibFn);
+gtsam::shared_ptrK readCalibData(const std::string& calibFn);
 
-std::vector<Feature2D> readFeatures(const char* filename);
+std::vector<Feature2D> readFeatureFile(const char* filename);
-std::vector<Feature2D> readFeatures(const char* featFn_pre, const char* featFn_suf, int imageId);
+std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn);
 
 
 #endif // LANDMARKUTILS_H

examples/vSLAMexample/vSLAMexample.cpp

@@ -5,7 +5,6 @@
  * @author  Duy-Nguyen
  */
 
-#include <gtsam/CppUnitLite/TestHarness.h>
 #include <boost/shared_ptr.hpp>
 using namespace boost;
 
@@ -26,39 +25,62 @@ using namespace gtsam::visualSLAM;
 using namespace boost;
 
 /* ************************************************************************* */
-#define CALIB_FILE      "Data/calib.txt"
+#define CALIB_FILE					"calib.txt"
-#define LANDMARKS_FILE  "Data/landmarks.txt"
+#define LANDMARKS_FILE			"landmarks.txt"
+#define POSES_FILE					"poses.txt"
+#define MEASUREMENTS_FILE		"measurements.txt"
 
-#define POSEFN_PREFIX   "Data/ttpy"
+// Base data folder
-#define POSEFN_SUFFIX   ".pose"
+string g_dataFolder;
-#define FEATFN_PREFIX   "Data/ttpy"
-#define FEATFN_SUFFIX   ".feat"
 
-#define NUM_IMAGES 10
+// Store groundtruth values, read from files
-const int ImageIds[NUM_IMAGES] = {10,20,30,40,50,60,70,80,90,100};
+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}
 
-// Store groundtruth values
+// Noise models
-map<int, Point3> g_landmarks;
+SharedGaussian measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f));
-vector<Pose3> g_poses;
+
+/* ************************************************************************* */
+/**
+	* 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 associating 2D features (measurements) detected in each image
+	* by adding and linking 2D features (measurements) detected in each captured image
   * with their corresponding landmarks.
   */
-Graph setupGraph()
+Graph setupGraph(std::vector<Feature2D>& measurements, SharedGaussian measurementSigma, shared_ptrK calib)
 {
     Graph g;    
-    shared_ptrK sK(new Cal3_S2(readCalibData(CALIB_FILE)));
-    sK->print("Calibration: ");
-		SharedGaussian sigma(noiseModel::Isotropic::Sigma(2,5.0f));
 
-    for (size_t i= 0; i<NUM_IMAGES; i++)
+		cout << "Built graph: " << endl;
+		for (size_t i= 0; i<measurements.size(); i++)
     {
-        std::vector<Feature2D> features = readFeatures(FEATFN_PREFIX, FEATFN_SUFFIX, ImageIds[i]);
+				measurements[i].print();
-        for (size_t j = 0; j<features.size(); j++)
+
-            g.addMeasurement(features[j].m_p, sigma, i, features[j].m_id, sK);
+				g.addMeasurement(measurements[i].m_p,
+												 measurementSigma,
+												 measurements[i].m_idCamera,
+												 measurements[i].m_idLandmark,
+												 calib);
     }
 
     return g;
@@ -66,69 +88,62 @@ Graph setupGraph()
 
 /* ************************************************************************* */
 /**
-  * Read initial values.
+	* Create a structure of initial estimates for all nodes (landmarks and poses) in the graph.
-  * Note: These are ground-truth values, but we just use them as initial estimates.
+	* The returned Values structure contains all initial values for all nodes.
   */
-Values initializeValues()
+Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses)
 {
     Values initValues;
 
     // Initialize landmarks 3D positions.
-    for (map<int, Point3>::iterator lmit = g_landmarks.begin(); lmit != g_landmarks.end(); lmit++)
+		for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
         initValues.insert( lmit->first, lmit->second );
 
     // Initialize camera poses.
-    for (int i = 0; i<NUM_IMAGES; i++)
+		for (map<int, Pose3>::iterator poseit = poses.begin(); poseit != poses.end(); poseit++)
-        initValues.insert(i, g_poses[i]);
+				initValues.insert( poseit->first, poseit->second);
 
     return initValues;
 }
 
 /* ************************************************************************* */
-int main()
+int main(int argc, char* argv[])
 {
-    shared_ptr<Graph> graph(new Graph(setupGraph()));
+		if (argc <2)
-
-    // Read groundtruth landmarks' positions and poses. These will also be used later as intial estimates.
-    readLandMarks(LANDMARKS_FILE, g_landmarks);
-    for (int i = 0; i<NUM_IMAGES; i++)
 		{
-        Pose3 pose = readPose(POSEFN_PREFIX, POSEFN_SUFFIX, ImageIds[i]) ;
+				cout << "Usage: vSLAMexample <DataFolder>" << endl << endl;
-        g_poses.push_back( pose );
+				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: vSLAMexample '$gtsam_source_folder$/examples/vSLAMexample/Data'" << endl;
+				exit(0);
 		}
 
-    // Create an initial Values structure using groundtruth as the initial estimates
+		g_dataFolder = string(argv[1]);
-    boost::shared_ptr<Values> initialValues(new Values(initializeValues()));
+		readAllData();
+
+		// Create a graph using the 2D measurements (features) and calibration data
+		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 hard constraint on the first pose, used as fixed prior.
-		graph->addPoseConstraint(0, g_poses[0]);
+		graph->addPoseConstraint(g_poses.begin()->first, g_poses.begin()->second);
 
-    // Create an ordering of the variables
+		// Add prior factor for all poses in the graph
-    shared_ptr<Ordering> ordering(new Ordering);
+//		for (map<int, Pose3>::iterator poseit = g_poses.begin(); poseit != g_poses.end(); poseit++)
-    char name[4];
+//				graph->addPosePrior(poseit->first, poseit->second, noiseModel::Unit::Create(10));
-    for (size_t i = 0; i<g_landmarks.size(); i++)
-    {
-        sprintf(name, "l%d", i);    // "li"
-        *ordering += name;
-    }
 
-    for (size_t i = 0; i<NUM_IMAGES; i++)
+		// Optimize the graph
-    {
+		cout << "*******************************************************" << endl;
-        sprintf(name, "x%d", i);    // "xj"
+		Optimizer::Parameters::verbosityLevel verborsity = Optimizer::Parameters::DAMPED;
-        *ordering += name;
+		Optimizer::shared_values result = Optimizer::optimizeLM( graph, initialEstimates, verborsity );
-    }
 
-    // Create an optimizer and check its error
+		// Print final results
-    // We expect the initial to be zero because Values is the ground truth
+		cout << "*******************************************************" << endl;
-        Optimizer::shared_solver solver(new Optimizer::solver(ordering));
+		result->print("FINAL RESULTS: ");
-    Optimizer optimizer(graph, initialValues, solver);
-    cout << "Initial error: " << optimizer.error() << endl;
-    optimizer.config()->print("Initial estimates: ");
-
-    // Optimize the graph.
-		Optimizer::verbosityLevel verborsity = Optimizer::ERROR;
-		Optimizer optimResult = optimizer.levenbergMarquardt(1e-5, 1e-5, verborsity);
-    optimResult.config()->print("After optimization: ");
 
 }
 /* ************************************************************************* */