From c71d07bbfdf68333d5146f406fed4c9e4d4d6c58 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 21 Jan 2023 11:39:42 -0800 Subject: [PATCH] Removed filter from examples and timing scripts --- examples/StereoVOExample_large.cpp | 3 +- .../examples/ConcurrentCalibration.cpp | 46 ++++++++++--------- .../examples/SmartProjectionFactorExample.cpp | 3 +- .../examples/SmartRangeExample_plaza1.cpp | 10 ++-- .../examples/SmartRangeExample_plaza2.cpp | 10 ++-- .../SmartStereoProjectionFactorExample.cpp | 24 +++++----- timing/timeLago.cpp | 13 +++--- 7 files changed, 56 insertions(+), 53 deletions(-) diff --git a/examples/StereoVOExample_large.cpp b/examples/StereoVOExample_large.cpp index af81db7030..c4a3a8de42 100644 --- a/examples/StereoVOExample_large.cpp +++ b/examples/StereoVOExample_large.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -113,7 +114,7 @@ int main(int argc, char** argv) { Values result = optimizer.optimize(); cout << "Final result sample:" << endl; - Values pose_values = result.filter(); + Values pose_values = utilities::allPose3s(result); pose_values.print("Final camera poses:\n"); return 0; diff --git a/gtsam_unstable/examples/ConcurrentCalibration.cpp b/gtsam_unstable/examples/ConcurrentCalibration.cpp index 95629fb43b..75dc49eeec 100644 --- a/gtsam_unstable/examples/ConcurrentCalibration.cpp +++ b/gtsam_unstable/examples/ConcurrentCalibration.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -35,12 +36,15 @@ using namespace std; using namespace gtsam; +using symbol_shorthand::K; +using symbol_shorthand::L; +using symbol_shorthand::X; int main(int argc, char** argv){ Values initial_estimate; NonlinearFactorGraph graph; - const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,1); + const auto model = noiseModel::Isotropic::Sigma(2,1); string calibration_loc = findExampleDataFile("VO_calibration00s.txt"); string pose_loc = findExampleDataFile("VO_camera_poses00s.txt"); @@ -54,13 +58,13 @@ int main(int argc, char** argv){ calibration_file >> fx >> fy >> s >> u0 >> v0 >> b; //create stereo camera calibration object - const Cal3_S2::shared_ptr K(new Cal3_S2(fx,fy,s,u0,v0)); - const Cal3_S2::shared_ptr noisy_K(new Cal3_S2(fx*1.2,fy*1.2,s,u0-10,v0+10)); + const Cal3_S2 true_K(fx,fy,s,u0,v0); + const Cal3_S2 noisy_K(fx*1.2,fy*1.2,s,u0-10,v0+10); - initial_estimate.insert(Symbol('K', 0), *noisy_K); + initial_estimate.insert(K(0), noisy_K); - noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 1e-5, 100, 100).finished()); - graph.addPrior(Symbol('K', 0), *noisy_K, calNoise); + auto calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 1e-5, 100, 100).finished()); + graph.addPrior(K(0), noisy_K, calNoise); ifstream pose_file(pose_loc.c_str()); @@ -75,7 +79,7 @@ int main(int argc, char** argv){ initial_estimate.insert(Symbol('x', pose_id), Pose3(m)); } - noiseModel::Isotropic::shared_ptr poseNoise = noiseModel::Isotropic::Sigma(6, 0.01); + auto poseNoise = noiseModel::Isotropic::Sigma(6, 0.01); graph.addPrior(Symbol('x', pose_id), Pose3(m), poseNoise); // camera and landmark keys @@ -83,22 +87,22 @@ int main(int argc, char** argv){ // pixel coordinates uL, uR, v (same for left/right images due to rectification) // landmark coordinates X, Y, Z in camera frame, resulting from triangulation - double uL, uR, v, X, Y, Z; + double uL, uR, v, _X, Y, Z; ifstream factor_file(factor_loc.c_str()); cout << "Reading stereo factors" << endl; //read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation - while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { -// graph.emplace_shared >(StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K); + while (factor_file >> x >> l >> uL >> uR >> v >> _X >> Y >> Z) { +// graph.emplace_shared >(StereoPoint2(uL, uR, v), model, X(x), L(l), K); - graph.emplace_shared >(Point2(uL,v), model, Symbol('x', x), Symbol('l', l), Symbol('K', 0)); + graph.emplace_shared >(Point2(uL,v), model, X(x), L(l), K(0)); //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it - if (!initial_estimate.exists(Symbol('l', l))) { - Pose3 camPose = initial_estimate.at(Symbol('x', x)); + if (!initial_estimate.exists(L(l))) { + Pose3 camPose = initial_estimate.at(X(x)); //transformFrom() transforms the input Point3 from the camera pose space, camPose, to the global space - Point3 worldPoint = camPose.transformFrom(Point3(X, Y, Z)); - initial_estimate.insert(Symbol('l', l), worldPoint); + Point3 worldPoint = camPose.transformFrom(Point3(_X, Y, Z)); + initial_estimate.insert(L(l), worldPoint); } } @@ -123,7 +127,7 @@ int main(int argc, char** argv){ double currentError; - stream_K << optimizer.iterations() << " " << optimizer.values().at(Symbol('K',0)).vector().transpose() << endl; + stream_K << optimizer.iterations() << " " << optimizer.values().at(K(0)).vector().transpose() << endl; // Iterative loop @@ -132,7 +136,7 @@ int main(int argc, char** argv){ currentError = optimizer.error(); optimizer.iterate(); - stream_K << optimizer.iterations() << " " << optimizer.values().at(Symbol('K',0)).vector().transpose() << endl; + stream_K << optimizer.iterations() << " " << optimizer.values().at(K(0)).vector().transpose() << endl; if(params.verbosity >= NonlinearOptimizerParams::ERROR) cout << "newError: " << optimizer.error() << endl; } while(optimizer.iterations() < params.maxIterations && @@ -142,13 +146,13 @@ int main(int argc, char** argv){ Values result = optimizer.values(); cout << "Final result sample:" << endl; - Values pose_values = result.filter(); + Values pose_values = utilities::allPose3s(result); pose_values.print("Final camera poses:\n"); - Values(result.filter()).print("Final K\n"); + result.at(K(0)).print("Final K\n"); - noisy_K->print("Initial noisy K\n"); - K->print("Initial correct K\n"); + noisy_K.print("Initial noisy K\n"); + true_K.print("Initial correct K\n"); return 0; } diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index e290cef7e5..b3b04cca32 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -30,6 +30,7 @@ #include #include #include +#include #include #include #include @@ -114,7 +115,7 @@ int main(int argc, char** argv){ Values result = optimizer.optimize(); cout << "Final result sample:" << endl; - Values pose_values = result.filter(); + Values pose_values = utilities::allPose3s(result); pose_values.print("Final camera poses:\n"); return 0; diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp index 64afa80309..2fccf6b18d 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza1.cpp @@ -234,9 +234,8 @@ int main(int argc, char** argv) { } } countK = 0; - for(const auto it: result.filter()) - os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1" - << endl; + for (const auto& [key, point] : result.extract()) + os2 << key << "\t" << point.x() << "\t" << point.y() << "\t1" << endl; if (smart) { for(size_t jj: ids) { Point2 landmark = smartFactors[jj]->triangulate(result); @@ -257,9 +256,8 @@ int main(int argc, char** argv) { // Write result to file Values result = isam.calculateEstimate(); ofstream os("rangeResult.txt"); - for(const auto it: result.filter()) - os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t" - << it.value.theta() << endl; + for (const auto& [key, pose] : result.extract()) + os << key << "\t" << pose.x() << "\t" << pose.y() << "\t" << pose.theta() << endl; exit(0); } diff --git a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp index 95aff85a7f..1e6f77b31e 100644 --- a/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp +++ b/gtsam_unstable/examples/SmartRangeExample_plaza2.cpp @@ -202,13 +202,11 @@ int main(int argc, char** argv) { // Write result to file Values result = isam.calculateEstimate(); ofstream os2("rangeResultLM.txt"); - for(const auto it: result.filter()) - os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1" - << endl; + for (const auto& [key, point] : result.extract()) + os2 << key << "\t" << point.x() << "\t" << point.y() << "\t1" << endl; ofstream os("rangeResult.txt"); - for(const auto it: result.filter()) - os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t" - << it.value.theta() << endl; + for (const auto& [key, pose] : result.extract()) + os << key << "\t" << pose.x() << "\t" << pose.y() << "\t" << pose.theta() << endl; exit(0); } diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index 2426ca2012..804e4cf5b8 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -43,6 +44,7 @@ using namespace std; using namespace gtsam; +using symbol_shorthand::X; int main(int argc, char** argv){ @@ -84,16 +86,16 @@ int main(int argc, char** argv){ if(add_initial_noise){ m(1,3) += (pose_id % 10)/10.0; } - initial_estimate.insert(Symbol('x', pose_id), Pose3(m)); + initial_estimate.insert(X(pose_id), Pose3(m)); } - Values initial_pose_values = initial_estimate.filter(); + const auto initialPoses = initial_estimate.extract(); if (output_poses) { init_pose3Out.open(init_poseOutput.c_str(), ios::out); - for (size_t i = 1; i <= initial_pose_values.size(); i++) { + for (size_t i = 1; i <= initialPoses.size(); i++) { init_pose3Out << i << " " - << initial_pose_values.at(Symbol('x', i)).matrix().format( + << initialPoses.at(X(i)).matrix().format( Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", " ")) << endl; } } @@ -103,7 +105,7 @@ int main(int argc, char** argv){ // pixel coordinates uL, uR, v (same for left/right images due to rectification) // landmark coordinates X, Y, Z in camera frame, resulting from triangulation - double uL, uR, v, X, Y, Z; + double uL, uR, v, _X, Y, Z; ifstream factor_file(factor_loc.c_str()); cout << "Reading stereo factors" << endl; @@ -112,21 +114,21 @@ int main(int argc, char** argv){ SmartFactor::shared_ptr factor(new SmartFactor(model)); size_t current_l = 3; // hardcoded landmark ID from first measurement - while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { + while (factor_file >> x >> l >> uL >> uR >> v >> _X >> Y >> Z) { if(current_l != l) { graph.push_back(factor); factor = SmartFactor::shared_ptr(new SmartFactor(model)); current_l = l; } - factor->add(StereoPoint2(uL,uR,v), Symbol('x',x), K); + factor->add(StereoPoint2(uL,uR,v), X(x), K); } - Pose3 first_pose = initial_estimate.at(Symbol('x',1)); + Pose3 first_pose = initial_estimate.at(X(1)); //constrain the first pose such that it cannot change from its original value during optimization // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky // QR is much slower than Cholesky, but numerically more stable - graph.emplace_shared >(Symbol('x',1),first_pose); + graph.emplace_shared >(X(1),first_pose); LevenbergMarquardtParams params; params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; @@ -138,13 +140,13 @@ int main(int argc, char** argv){ Values result = optimizer.optimize(); cout << "Final result sample:" << endl; - Values pose_values = result.filter(); + Values pose_values = utilities::allPose3s(result); pose_values.print("Final camera poses:\n"); if(output_poses){ pose3Out.open(poseOutput.c_str(),ios::out); for(size_t i = 1; i<=pose_values.size(); i++){ - pose3Out << i << " " << pose_values.at(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0, + pose3Out << i << " " << pose_values.at(X(i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", " ")) << endl; } cout << "Writing output" << endl; diff --git a/timing/timeLago.cpp b/timing/timeLago.cpp index c3ee6ff4bc..bb506de36e 100644 --- a/timing/timeLago.cpp +++ b/timing/timeLago.cpp @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file timeVirtual.cpp - * @brief Time the overhead of using virtual destructors and methods + * @file timeLago.cpp + * @brief Time the LAGO initialization method * @author Richard Roberts * @date Dec 3, 2010 */ @@ -36,16 +36,15 @@ int main(int argc, char *argv[]) { Values::shared_ptr solution; NonlinearFactorGraph::shared_ptr g; string inputFile = findExampleDataFile("w10000"); - SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0).finished()); + auto model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0).finished()); boost::tie(g, solution) = load2D(inputFile, model); // add noise to create initial estimate Values initial; - Values::ConstFiltered poses = solution->filter(); - SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0).finished()); + auto noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0).finished()); Sampler sampler(noise); - for(const Values::ConstFiltered::KeyValuePair& it: poses) - initial.insert(it.key, it.value.retract(sampler.sample())); + for(const auto& [key,pose]: solution->extract()) + initial.insert(key, pose.retract(sampler.sample())); // Add prior on the pose having index (key) = 0 noiseModel::Diagonal::shared_ptr priorModel = //