diff --git a/gtsam_unstable/examples/KITTItoBALConverter.cpp b/gtsam_unstable/examples/KITTItoBALConverter.cpp deleted file mode 100644 index d63c0eac9..000000000 --- a/gtsam_unstable/examples/KITTItoBALConverter.cpp +++ /dev/null @@ -1,296 +0,0 @@ - -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file KITTItoBALConverter.cpp - * @brief Program for reading KITTI files and convert these ones to BAL format - * @date October, 2013 - * @author Pablo F. Alcantarilla - */ - -// Both relative poses and recovered trajectory poses will be stored as Pose3 objects -#include -#include - -// Each variable in the system (poses and landmarks) must be identified with a unique key. -// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). -// Here we will use Symbols -#include - -// We want to use iSAM2 to solve the range-SLAM problem incrementally -#include - -// iSAM2 requires as input a set set of new factors to be added stored in a factor graph, -// and initial guesses for any new variables used in the added factors -#include -#include - -// We will use a non-liear solver to batch-inituialize from the first 150 frames -#include - -// In GTSAM, measurement functions are represented as 'factors'. Several common factors -// have been provided with the library for solving robotics SLAM problems. -#include -#include - -// We need to use SFM_data to save it to BAL format -#include - -// Standard headers, added last, so we know headers above work on their own -#include -#include -#include -#include -#include - -using namespace std; -using namespace gtsam; -using namespace boost::assign; -namespace NM = gtsam::noiseModel; - -using symbol_shorthand::X; -using symbol_shorthand::L; - -typedef PriorFactor Pose3Prior; - -/* ************************************************************************* */ - -//// Helper functions taken from VO code -// Loaded all pose values into list -Values::shared_ptr loadPoseValues(const string& filename) { - Values::shared_ptr values(new Values()); - bool addNoise = false; - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); - - // read in camera poses - string full_filename = filename; - ifstream fin; - fin.open(full_filename.c_str()); - - int pose_id; - while (fin >> pose_id) { - double pose_matrix[16]; - for (int i = 0; i < 16; i++) { - fin >> pose_matrix[i]; - } - - Pose3 pose_(Matrix_(4, 4, pose_matrix)); - Rot3 r_ = pose_.rotation(); - Point3 t_ = pose_.translation(); - - /*if (addNoise) { - values->insert(Symbol('x',pose_id), Pose3(Matrix_(4, 4, pose_matrix)).compose(noise_pose)); - } else { - values->insert(Symbol('x',pose_id), Pose3(Matrix_(4, 4, pose_matrix))); - }*/ - - if (addNoise) { - values->insert(Symbol('x',pose_id), Pose3(r_,t_).compose(noise_pose)); - } else { - values->insert(Symbol('x',pose_id), Pose3(r_,t_)); - } - } - - fin.close(); - return values; -} - -/* ************************************************************************* */ - -// Loaded specific pose values that are in key list -Values::shared_ptr loadPoseValues(const string& filename, list keys) { - Values::shared_ptr values(new Values()); - std::list::iterator kit; - - // read in camera poses - string full_filename = filename; - ifstream fin; - fin.open(full_filename.c_str()); - - int pose_id; - while (fin >> pose_id) { - double pose_matrix[16]; - for (int i = 0; i < 16; i++) { - fin >> pose_matrix[i]; - } - kit = find (keys.begin(), keys.end(), X(pose_id)); - if (kit != keys.end()) { - cout << " Adding " << X(pose_id) << endl; - values->insert(Symbol('x',pose_id), Pose3(Matrix_(4, 4, pose_matrix))); - } - } - - fin.close(); - return values; -} - -/* ************************************************************************* */ - -// Load calibration info -Cal3_S2::shared_ptr loadCalibration(const string& filename) { - string full_filename = filename; - ifstream fin; - fin.open(full_filename.c_str()); - - // try loading from parent directory as backup - if(!fin) { - cerr << "Could not load " << full_filename; - exit(1); - } - - double fx, fy, s, u, v, b; - fin >> fx >> fy >> s >> u >> v >> b; - fin.close(); - - Cal3_S2::shared_ptr K(new Cal3_S2(fx, fy, s, u, v)); - - return K; -} - -/* ************************************************************************* */ - -void writeValues(string directory_, const Values& values){ - string filename = directory_ + "camera_poses.txt"; - ofstream fout; - fout.open(filename.c_str()); - fout.precision(20); - - // write out camera poses - BOOST_FOREACH(Values::ConstFiltered::value_type key_value, values.filter()) { - fout << Symbol(key_value.key).index(); - const gtsam::Matrix& matrix= key_value.value.matrix(); - for (size_t row=0; row < 4; ++row) { - for (size_t col=0; col < 4; ++col) { - fout << " " << matrix(row, col); - } - } - fout << endl; - } - fout.close(); - - if(values.filter().size() > 0) { - // write landmarks - filename = directory_ + "landmarks.txt"; - fout.open(filename.c_str()); - - BOOST_FOREACH(Values::ConstFiltered::value_type key_value, values.filter()) { - fout << Symbol(key_value.key).index(); - fout << " " << key_value.value.x(); - fout << " " << key_value.value.y(); - fout << " " << key_value.value.z(); - fout << endl; - } - fout.close(); - - } -} - -/* ************************************************************************* */ - -int main(int argc, char** argv) { - - - SfM_data kitti_sfm; - - int ncameras = 0, npoints = 0, nobservations = 0; - - bool debug = false; - - // Minimum number of views of a landmark before it is added to the graph (SmartProjectionFactor case only) - unsigned int minimumNumViews = 1; - - string HOME = getenv("HOME"); - string input_dir = HOME + "/Research/datasets/kitti/test/"; - string output_file = HOME + "/Research/datasets/kitti/test/test_bal.txt"; - - typedef GenericProjectionFactor ProjectionFactor; - NonlinearFactorGraph graph; - - // Load calibration - boost::shared_ptr K = loadCalibration(input_dir+"calibration.txt"); - Cal3Bundler Kd(K->fx(),0,0,K->px(),K->py()); - K->print("Calibration"); - - // Load values from VO camera poses output - gtsam::Values::shared_ptr loaded_values = loadPoseValues(input_dir+"camera_poses.txt"); - - // Load camera poses - BOOST_FOREACH(Values::ConstFiltered::value_type key_value, loaded_values->filter()) { - Pose3 pose = key_value.value; - kitti_sfm.cameras.push_back(SfM_Camera(pose,Kd)); - ncameras++; - } - - // Read in kitti dataset - ifstream fin; - fin.open((input_dir+"stereo_factors.txt").c_str()); - if(!fin) { - cerr << "Could not open stereo_factors.txt" << endl; - exit(1); - } - - // Read all measurements tracked by VO stereo - cout << "Loading stereo_factors.txt" << endl; - Key r, l, currentLandmark = 0; - std::list allViews; - std::vector views; - std::vector measurements; - Values values; - - float uL, uR, v, x, y, z; - while (fin >> r >> l >> uL >> uR >> v >> x >> y >> z){ - - if (debug) cout << "CurrentLandmark " << currentLandmark << " Landmark " << l << std::endl; - - if (loaded_values->exists(L(l)) == boost::none) { - Pose3 camera = loaded_values->at(X(r)); - - // Use transform to to get points in world coordinate frame - Point3 worldPoint = camera.transform_from(Point3(x, y, z)); - loaded_values->insert(L(l), worldPoint); // add point; - npoints++; - - //cout << "world p " << worldPoint << endl; - // Create a track without observations - SfM_Track track; - track.p = worldPoint; - track.r = .4; - track.g = .4; - track.b = .4; - kitti_sfm.tracks.push_back(track); - } - - nobservations++; - } - - fin.close(); - - // Open again the file to store the measurements - fin.open((input_dir+"stereo_factors.txt").c_str()); - if(!fin) { - cerr << "Could not stereo_factors.txt" << endl; - exit(1); - } - while (fin >> r >> l >> uL >> uR >> v >> x >> y >> z){ - SfM_Measurement observation = make_pair(r,Point2(uL,v)); - kitti_sfm.tracks[l].measurements.push_back(observation); - } - fin.close(); - - cout << "ncameras " << ncameras << " npoints " << npoints << endl; - cout << "nobservations " << nobservations << endl; - - writeBAL(output_file,kitti_sfm); - - exit(0); -} -