diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample_kitti.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample_kitti.cpp index 50dcf4cc6..795f780a4 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample_kitti.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample_kitti.cpp @@ -38,7 +38,6 @@ // In GTSAM, measurement functions are represented as 'factors'. Several common factors // have been provided with the library for solving robotics SLAM problems. #include -#include #include // Standard headers, added last, so we know headers above work on their own @@ -57,6 +56,27 @@ typedef PriorFactor Pose3Prior; static SharedNoiseModel prior_model(noiseModel::Diagonal::Sigmas(Vector_(6, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01))); +Values::shared_ptr loadPoseValues(const string& filename) { + Values::shared_ptr values(new Values()); + + // 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]; + } + values->insert(Symbol('x',pose_id), Pose3(Matrix_(4, 4, pose_matrix))); + } + + fin.close(); + return values; +} + /* ************************************************************************* */ Values::shared_ptr loadPoseValues(const string& filename, list keys) { Values::shared_ptr values(new Values()); @@ -87,8 +107,12 @@ Values::shared_ptr loadPoseValues(const string& filename, list keys) { // main int main(int argc, char** argv) { + bool debug = false; + unsigned int minimumNumViews = 1; + string HOME = getenv("HOME"); - string input_dir = HOME + "/data/kitti/loop_closures_merged/"; + //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; @@ -109,7 +133,7 @@ int main(int argc, char** argv) { // read all measurements tracked by VO cout << "Loading stereo_factors.txt" << endl; int count = 0; - int currentLandmark = 0; + Key currentLandmark = 0; int numLandmarks = 0; Key r, l; double uL, uR, v, x, y, z; @@ -117,88 +141,50 @@ int main(int argc, char** argv) { std::vector views; std::vector measurements; Values values; - bool updateGraph = false; while (fin >> r >> l >> uL >> uR >> v >> x >> y >> z) { - cout << "CurrentLandmark " << currentLandmark << " Landmark " << l << std::endl; + if (debug) cout << "CurrentLandmark " << currentLandmark << " Landmark " << l << std::endl; - if (currentLandmark != l && views.size() < 3) { + if (currentLandmark != l && views.size() < minimumNumViews) { // New landmark. Not enough views for previous landmark so move on. - cout << "New landmark " << l << " with not enough view for previous one" << std::endl; + if (debug) cout << "New landmark " << l << " with not enough view for previous one" << std::endl; currentLandmark = l; views.clear(); measurements.clear(); } else if (currentLandmark != l) { // New landmark. Add previous landmark and associated views to new factor - cout << "New landmark " << l << " with "<< views.size() << " views for previous landmark " << currentLandmark << std::endl; + if (debug) cout << "New landmark " << l << " with "<< views.size() << " views for previous landmark " << currentLandmark << std::endl; - cout << "Keys "; + if (debug) cout << "Keys "; BOOST_FOREACH(const Key& k, views) { allViews.push_back(k); - cout << k << " "; + if (debug) cout << k << " "; } - cout << endl; + if (debug) cout << endl; - cout << "Measurements "; - BOOST_FOREACH(const Point2& p, measurements) { - cout << p << " "; + if (debug) { + cout << "Measurements "; + BOOST_FOREACH(const Point2& p, measurements) { + cout << p << " "; + } + cout << endl; } - cout << endl; - SmartFactor::shared_ptr smartFactor(new SmartFactor(measurements, pixel_sigma, views, K)); numLandmarks++; + SmartFactor::shared_ptr smartFactor(new SmartFactor(measurements, pixel_sigma, views, K)); graph.push_back(smartFactor); - if (numLandmarks > 4) { - updateGraph = true; - } - currentLandmark = l; views.clear(); measurements.clear(); } else { // We have new view for current landmark, so add it to the list later - cout << "New view for landmark " << l << " (" << views.size() << " total)" << std::endl; + if (debug) cout << "New view for landmark " << l << " (" << views.size() << " total)" << std::endl; } + + // Add view for new landmark views += X(r); measurements += Point2(uL,v); - // Optimize - if (updateGraph) { - - cout << "Optimizing... " << endl; - - // Get all view in the graph and populate poses from VO output - // TODO: Handle loop closures properly - allViews.unique(); - cout << "All Keys "; - values = *loadPoseValues(input_dir+"camera_poses.txt", allViews); - BOOST_FOREACH(const Key& k, allViews) { - cout << k << " "; - } - cout << endl; - - // Optimize! - LevenbergMarquardtParams params; - params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - params.verbosity = NonlinearOptimizerParams::ERROR; - - Values result; - gttic_(SmartProjectionFactorExample_kitti); - LevenbergMarquardtOptimizer optimizer(graph, values, params); - result = optimizer.optimize(); - gttoc_(SmartProjectionFactorExample_kitti); - tictoc_finishedIteration_(); - - values.print("before optimization "); - result.print("results of kitti optimization "); - tictoc_print_(); - - updateGraph = false; - values.clear(); - allViews.clear(); - graph = NonlinearFactorGraph(); - } - count++; if(count==100000) { cout << "Loading graph... " << graph.size() << endl; @@ -207,6 +193,42 @@ int main(int argc, char** argv) { } cout << "Graph size: " << graph.size() << endl; + gtsam::Values::shared_ptr loaded_values = loadPoseValues(input_dir+"camera_poses.txt"); + graph.add(Pose3Prior(X(0),loaded_values->at(X(0)), prior_model)); + + // Get all view in the graph and populate poses from VO output + // TODO: Handle loop closures properly + cout << "All Keys (" << allViews.size() << ")" << endl; + allViews.unique(); + cout << "All Keys (" << allViews.size() << ")" << endl; + + /* + values = *loadPoseValues(input_dir+"camera_poses.txt", allViews); + BOOST_FOREACH(const Key& k, allViews) { + if (debug) cout << k << " "; + } + cout << endl; + */ + + cout << "Optimizing... " << endl; + + // Optimize! + LevenbergMarquardtParams params; + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + params.verbosity = NonlinearOptimizerParams::ERROR; + + Values result; + gttic_(SmartProjectionFactorExample_kitti); + LevenbergMarquardtOptimizer optimizer(graph, *loaded_values, params); + result = optimizer.optimize(); + gttoc_(SmartProjectionFactorExample_kitti); + tictoc_finishedIteration_(); + + cout << "===================================================" << endl; + values.print("before optimization "); + result.print("results of kitti optimization "); + tictoc_print_(); + cout << "===================================================" << endl; exit(0); }