diff --git a/examples/Makefile.am b/examples/Makefile.am index 67c3443eb..41ad7322b 100644 --- a/examples/Makefile.am +++ b/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 #---------------------------------------------------------------------------------------------------- diff --git a/examples/vSLAMexample/Data/measurements.txt b/examples/vSLAMexample/Data/measurements.txt new file mode 100644 index 000000000..ffed3636e --- /dev/null +++ b/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 + diff --git a/examples/vSLAMexample/Data/poses.txt b/examples/vSLAMexample/Data/poses.txt new file mode 100644 index 000000000..72fec9114 --- /dev/null +++ b/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 + diff --git a/examples/vSLAMexample/Feature2D.h b/examples/vSLAMexample/Feature2D.h index cbbc8665a..db48a1cfb 100644 --- a/examples/vSLAMexample/Feature2D.h +++ b/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; - m_p.print(); + std::cout << "Pose id: " << m_idCamera << " -- Landmark id: " << m_idLandmark << std::endl; + m_p.print("\tMeasurement: "); } }; diff --git a/examples/vSLAMexample/Makefile.am b/examples/vSLAMexample/Makefile.am index d851746b1..79c4c114b 100644 --- a/examples/vSLAMexample/Makefile.am +++ b/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) ./$^ diff --git a/examples/vSLAMexample/landmarkUtils.cpp b/examples/vSLAMexample/landmarkUtils.cpp index cca8eba2e..8d5a28634 100644 --- a/examples/vSLAMexample/landmarkUtils.cpp +++ b/examples/vSLAMexample/landmarkUtils.cpp @@ -6,9 +6,9 @@ using namespace gtsam; using namespace std; /* ************************************************************************* */ -bool readLandMarks(const char* landmarkFile, std::map& landmarks) -{ - ifstream file(landmarkFile); +std::map readLandMarks(const std::string& 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& landmarks) int num; file >> num; + std::map landmarks; landmarks.clear(); for (int i = 0; i& 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 readPoses(const std::string& baseFolder, const std::string& posesFn) { - char poseFn[128]; - sprintf(poseFn, "%s%d%s", poseFn_pre, poseId, poseFn_suf); - return readPose(poseFn); + ifstream posesFile((baseFolder+posesFn).c_str()); + if (!posesFile) + { + cout << "Cannot read all pose file: " << posesFn << endl; + exit(0); + } + int numPoses; + posesFile >> numPoses; + map poses; + for (int i = 0; i> 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 K; + return shared_ptrK(new Cal3_S2(fx, fy, 0, ox, oy)); // skew factor = 0 } /* ************************************************************************* */ -std::vector readFeatures(const char* filename) +std::vector readFeatures(int pose_id, const char* filename) { ifstream file(filename); if (!file) @@ -106,18 +125,39 @@ std::vector readFeatures(const char* filename) std::vector vFeatures_; for (size_t i = 0; i < numFeatures; i++) { - int id; double x, y; - file >> id >> x >> y; - vFeatures_.push_back(Feature2D(id, Point2(x, y))); + 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 readFeatures(const char* featFn_pre, const char* featFn_suf, int imageId) -{ - char featFn[128]; - sprintf(featFn, "%s%d%s", featFn_pre, imageId, featFn_suf); - return readFeatures(featFn); +std::vector 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 allFeatures; + allFeatures.clear(); + + for (int i = 0; i> poseId; + + string featureFileName; + measurementsFile >> featureFileName; + vector features = readFeatures(poseId, (baseFolder+featureFileName).c_str()); + allFeatures.insert( allFeatures.end(), features.begin(), features.end() ); + } + + return allFeatures; } diff --git a/examples/vSLAMexample/landmarkUtils.h b/examples/vSLAMexample/landmarkUtils.h index 7b6a35fb6..8b44f2e0e 100644 --- a/examples/vSLAMexample/landmarkUtils.h +++ b/examples/vSLAMexample/landmarkUtils.h @@ -9,15 +9,15 @@ #include "gtsam/geometry/Cal3_S2.h" -bool readLandMarks(const char* landmarkFile, std::map& landmarks); +std::map 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 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 readFeatures(const char* filename); -std::vector readFeatures(const char* featFn_pre, const char* featFn_suf, int imageId); +std::vector readFeatureFile(const char* filename); +std::vector readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn); #endif // LANDMARKUTILS_H diff --git a/examples/vSLAMexample/vSLAMexample.cpp b/examples/vSLAMexample/vSLAMexample.cpp index ad0f78106..4a3e29b4a 100644 --- a/examples/vSLAMexample/vSLAMexample.cpp +++ b/examples/vSLAMexample/vSLAMexample.cpp @@ -5,7 +5,6 @@ * @author Duy-Nguyen */ -#include #include using namespace boost; @@ -26,39 +25,62 @@ using namespace gtsam::visualSLAM; using namespace boost; /* ************************************************************************* */ -#define CALIB_FILE "Data/calib.txt" -#define LANDMARKS_FILE "Data/landmarks.txt" +#define CALIB_FILE "calib.txt" +#define LANDMARKS_FILE "landmarks.txt" +#define POSES_FILE "poses.txt" +#define MEASUREMENTS_FILE "measurements.txt" -#define POSEFN_PREFIX "Data/ttpy" -#define POSEFN_SUFFIX ".pose" -#define FEATFN_PREFIX "Data/ttpy" -#define FEATFN_SUFFIX ".feat" +// Base data folder +string g_dataFolder; -#define NUM_IMAGES 10 -const int ImageIds[NUM_IMAGES] = {10,20,30,40,50,60,70,80,90,100}; +// Store groundtruth values, read from files +shared_ptrK g_calib; +map g_landmarks; // map: +map g_poses; // map: +std::vector g_measurements; // Feature2D: {camera_id, landmark_id, 2d feature_position} -// Store groundtruth values -map g_landmarks; -vector g_poses; +// 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 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& 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)); + Graph g; - for (size_t i= 0; i features = readFeatures(FEATFN_PREFIX, FEATFN_SUFFIX, ImageIds[i]); - for (size_t j = 0; j landmarks, std::map poses) { Values initValues; // Initialize landmarks 3D positions. - for (map::iterator lmit = g_landmarks.begin(); lmit != g_landmarks.end(); lmit++) + for (map::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++) initValues.insert( lmit->first, lmit->second ); // Initialize camera poses. - for (int i = 0; i::iterator poseit = poses.begin(); poseit != poses.end(); poseit++) + initValues.insert( poseit->first, poseit->second); return initValues; } /* ************************************************************************* */ -int main() +int main(int argc, char* argv[]) { - shared_ptr graph(new Graph(setupGraph())); + if (argc <2) + { + cout << "Usage: vSLAMexample " << endl << endl; + cout << "\tPlease specify , 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); + } - // 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 initialValues(new Values(initializeValues())); + // Create a graph using the 2D measurements (features) and calibration data + shared_ptr graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib))); + + // Create an initial Values structure using groundtruth values as the initial estimates + boost::shared_ptr 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 - shared_ptr ordering(new Ordering); - char name[4]; - for (size_t i = 0; i::iterator poseit = g_poses.begin(); poseit != g_poses.end(); poseit++) +// graph->addPosePrior(poseit->first, poseit->second, noiseModel::Unit::Create(10)); - for (size_t i = 0; iprint("Initial estimates: "); - - // Optimize the graph. - Optimizer::verbosityLevel verborsity = Optimizer::ERROR; - Optimizer optimResult = optimizer.levenbergMarquardt(1e-5, 1e-5, verborsity); - optimResult.config()->print("After optimization: "); + // Print final results + cout << "*******************************************************" << endl; + result->print("FINAL RESULTS: "); } /* ************************************************************************* */