diff --git a/examples/vSLAMexample/landmarkUtils.cpp b/examples/vSLAMexample/landmarkUtils.cpp deleted file mode 100644 index 8d5a28634..000000000 --- a/examples/vSLAMexample/landmarkUtils.cpp +++ /dev/null @@ -1,163 +0,0 @@ -#include "landmarkUtils.h" -#include -#include - -using namespace gtsam; -using namespace std; - -/* ************************************************************************* */ -std::map readLandMarks(const std::string& landmarkFile) -{ - ifstream file(landmarkFile.c_str()); - if (!file) { - cout << "Cannot read landmark file: " << landmarkFile << endl; - exit(0); - } - - int num; - file >> num; - std::map landmarks; - landmarks.clear(); - for (int i = 0; i> color_id >> x >> y >> z; - landmarks[color_id] = Point3(x, y, z); - } - - file.close(); - return landmarks; -} - -/* ************************************************************************* */ -/** - * Read pose from file, output by Panda3D. - * Warning: row major!!! - */ -gtsam::Pose3 readPose(const char* Fn) -{ - ifstream poseFile(Fn); - if (!poseFile) - { - cout << "Cannot read pose file: " << Fn << endl; - exit(0); - } - - double v[16]; - for (int i = 0; i<16; i++) - 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 !!! - - Pose3 pose(T); - return pose; -} -/* ************************************************************************* */ -std::map readPoses(const std::string& baseFolder, const std::string& posesFn) -{ - 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::shared_ptrK readCalibData(const std::string& calibFn) -{ - ifstream calibFile(calibFn.c_str()); - if (!calibFile) - { - cout << "Cannot read calib file: " << calibFn << endl; - exit(0); - } - int imX, imY; - float fx, fy, ox, oy; - calibFile >> imX >> imY >> fx >> fy >> ox >> oy; - calibFile.close(); - - return shared_ptrK(new Cal3_S2(fx, fy, 0, ox, oy)); // skew factor = 0 -} -/* ************************************************************************* */ -std::vector readFeatures(int pose_id, const char* filename) -{ - ifstream file(filename); - if (!file) - { - cout << "Cannot read feature file: " << filename<< endl; - exit(0); - } - - int numFeatures; - file >> numFeatures ; - - std::vector vFeatures_; - for (size_t i = 0; i < numFeatures; i++) - { - 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 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 deleted file mode 100644 index 8b44f2e0e..000000000 --- a/examples/vSLAMexample/landmarkUtils.h +++ /dev/null @@ -1,23 +0,0 @@ -#ifndef LANDMARKUTILS_H -#define LANDMARKUTILS_H - -#include -#include -#include "Feature2D.h" -#include "gtsam/geometry/Pose3.h" -#include "gtsam/geometry/Point3.h" -#include "gtsam/geometry/Cal3_S2.h" - - -std::map readLandMarks(const std::string& landmarkFile); - -gtsam::Pose3 readPose(const char* poseFn); -std::map readPoses(const std::string& baseFolder, const std::string& posesFN); - -gtsam::shared_ptrK readCalibData(const std::string& calibFn); - -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 deleted file mode 100644 index c5842889e..000000000 --- a/examples/vSLAMexample/vSLAMexample.cpp +++ /dev/null @@ -1,148 +0,0 @@ -/** - * @file vSLAMexample.cpp - * @brief An vSLAM example for synthesis sequence - * single camera - * @author Duy-Nguyen - */ - -#include -using namespace boost; - -#define GTSAM_MAGIC_KEY - -#include -#include -#include -#include -#include - -#include "landmarkUtils.h" -#include "Feature2D.h" - -using namespace std; -using namespace gtsam; -using namespace gtsam::visualSLAM; -using namespace boost; - -/* ************************************************************************* */ -#define CALIB_FILE "calib.txt" -#define LANDMARKS_FILE "landmarks.txt" -#define POSES_FILE "poses.txt" -#define MEASUREMENTS_FILE "measurements.txt" - -// Base data folder -string g_dataFolder; - -// 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} - -// 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 linking 2D features (measurements) detected in each captured image - * with their corresponding landmarks. - */ -Graph setupGraph(std::vector& measurements, SharedGaussian measurementSigma, shared_ptrK calib) -{ - Graph g; - - cout << "Built graph: " << endl; - for (size_t i= 0; i landmarks, std::map poses) -{ - Values initValues; - - // Initialize landmarks 3D positions. - for (map::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++) - initValues.insert( lmit->first, lmit->second ); - - // Initialize camera poses. - for (map::iterator poseit = poses.begin(); poseit != poses.end(); poseit++) - initValues.insert( poseit->first, poseit->second); - - return initValues; -} - -/* ************************************************************************* */ -int main(int argc, char* argv[]) -{ - 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); - } - - g_dataFolder = string(argv[1]); - readAllData(); - - // Create a graph using the 2D measurements (features) and the 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 prior factor for all poses in the graph - map::iterator poseit = g_poses.begin(); - for (; poseit != g_poses.end(); poseit++) - graph->addPosePrior(poseit->first, poseit->second, noiseModel::Unit::Create(1)); - - // Optimize the graph - cout << "*******************************************************" << endl; - Optimizer::Parameters::verbosityLevel verborsity = Optimizer::Parameters::DAMPED; - Optimizer::shared_values result = Optimizer::optimizeGN( graph, initialEstimates, verborsity ); - - // Print final results - cout << "*******************************************************" << endl; - result->print("FINAL RESULTS: "); - -} -/* ************************************************************************* */ -