From 5af82c4042a8665c981c2c0caca2e212da055fbc Mon Sep 17 00:00:00 2001 From: Zsolt Kira Date: Thu, 8 Aug 2013 17:16:23 +0000 Subject: [PATCH] SmartProjectionFactor example: Cleaned up some code, added comments, changed some of the LM parameters. Switched to use smaller kitti dataset for now. --- .../SmartProjectionFactorExample_kitti.cpp | 71 +++++++++++++++---- 1 file changed, 56 insertions(+), 15 deletions(-) diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample_kitti.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample_kitti.cpp index 795f780a4..c8e0dc058 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample_kitti.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample_kitti.cpp @@ -54,10 +54,13 @@ using symbol_shorthand::L; typedef PriorFactor Pose3Prior; -static SharedNoiseModel prior_model(noiseModel::Diagonal::Sigmas(Vector_(6, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01))); - +//// 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; @@ -70,14 +73,19 @@ Values::shared_ptr loadPoseValues(const string& filename) { for (int i = 0; i < 16; i++) { fin >> pose_matrix[i]; } - values->insert(Symbol('x',pose_id), Pose3(Matrix_(4, 4, pose_matrix))); + + 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))); + } } 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; @@ -104,6 +112,26 @@ Values::shared_ptr loadPoseValues(const string& filename, list keys) { 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; +} + // main int main(int argc, char** argv) { @@ -113,12 +141,18 @@ int main(int argc, char** argv) { string HOME = getenv("HOME"); //string input_dir = HOME + "/data/kitti/loop_closures_merged/"; string input_dir = HOME + "/data/KITTI_00_200/"; - Cal3_S2::shared_ptr K(new Cal3_S2(718.856, 718.856, 0.0, 607.1928, 185.2157)); typedef SmartProjectionFactor SmartFactor; + static SharedNoiseModel pixel_sigma(noiseModel::Unit::Create(3)); + static SharedNoiseModel prior_model(noiseModel::Diagonal::Sigmas(Vector_(6, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01))); + NonlinearFactorGraph graph; + + // Load calibration + //Cal3_S2::shared_ptr K(new Cal3_S2(718.856, 718.856, 0.0, 607.1928, 185.2157)); + boost::shared_ptr K = loadCalibration(input_dir+"calibration.txt"); + K->print("Calibration"); // Read in kitti dataset - // load stereo factors and initialize landmarks when first seen ifstream fin; fin.open((input_dir+"stereo_factors.txt").c_str()); if(!fin) { @@ -126,11 +160,7 @@ int main(int argc, char** argv) { exit(1); } - static SharedNoiseModel pixel_sigma(noiseModel::Unit::Create(3)); - static SharedNoiseModel prior_model(noiseModel::Diagonal::Sigmas(Vector_(6, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01))); - NonlinearFactorGraph graph; - - // read all measurements tracked by VO + // read all measurements tracked by VO stereo cout << "Loading stereo_factors.txt" << endl; int count = 0; Key currentLandmark = 0; @@ -191,10 +221,12 @@ int main(int argc, char** argv) { count=0; } } - cout << "Graph size: " << graph.size() << endl; + + // Load values from VO camera poses output gtsam::Values::shared_ptr loaded_values = loadPoseValues(input_dir+"camera_poses.txt"); graph.add(Pose3Prior(X(0),loaded_values->at(X(0)), prior_model)); + //graph.print("thegraph"); // Get all view in the graph and populate poses from VO output // TODO: Handle loop closures properly @@ -202,7 +234,7 @@ int main(int argc, char** argv) { allViews.unique(); cout << "All Keys (" << allViews.size() << ")" << endl; - /* + /* If using only subset of graph, only read in values for keys that are used values = *loadPoseValues(input_dir+"camera_poses.txt", allViews); BOOST_FOREACH(const Key& k, allViews) { if (debug) cout << k << " "; @@ -217,9 +249,19 @@ int main(int argc, char** argv) { params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; params.verbosity = NonlinearOptimizerParams::ERROR; + params.lambdaInitial = 1; + params.lambdaFactor = 10; + params.maxIterations = 100; + params.relativeErrorTol = 1e-5; + params.absoluteErrorTol = 1.0; + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + params.verbosity = NonlinearOptimizerParams::ERROR; + params.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_CHOLESKY; + + LevenbergMarquardtOptimizer optimizer(graph, *loaded_values, params); + Values result; gttic_(SmartProjectionFactorExample_kitti); - LevenbergMarquardtOptimizer optimizer(graph, *loaded_values, params); result = optimizer.optimize(); gttoc_(SmartProjectionFactorExample_kitti); tictoc_finishedIteration_(); @@ -232,4 +274,3 @@ int main(int argc, char** argv) { exit(0); } -