From fa371e141579ffd44bcf016387ff3a280c3e7127 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 6 Feb 2025 16:24:45 -0500 Subject: [PATCH] move more things to City10000 header --- examples/City10000.h | 33 ++++++++++++- examples/Hybrid_City10000.cpp | 90 ++++++----------------------------- 2 files changed, 46 insertions(+), 77 deletions(-) diff --git a/examples/City10000.h b/examples/City10000.h index 84ef6b4c5..61bf72188 100644 --- a/examples/City10000.h +++ b/examples/City10000.h @@ -25,6 +25,16 @@ using namespace gtsam; using namespace boost::algorithm; +auto kOpenLoopModel = noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10); +const double kOpenLoopConstant = kOpenLoopModel->negLogConstant(); + +auto kPriorNoiseModel = noiseModel::Diagonal::Sigmas( + (Vector(3) << 0.0001, 0.0001, 0.0001).finished()); + +auto kPoseNoiseModel = noiseModel::Diagonal::Sigmas( + (Vector(3) << 1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0).finished()); +const double kPoseNoiseConstant = kPoseNoiseModel->negLogConstant(); + class City10000Dataset { std::ifstream in_; @@ -64,4 +74,25 @@ class City10000Dataset { } else return false; } -}; \ No newline at end of file +}; + +/** + * @brief Write the result of optimization to file. + * + * @param result The Values object with the final result. + * @param num_poses The number of poses to write to the file. + * @param filename The file name to save the result to. + */ +void writeResult(const Values& result, size_t numPoses, + const std::string& filename = "Hybrid_city10000.txt") const { + std::ofstream outfile; + outfile.open(filename); + + for (size_t i = 0; i < numPoses; ++i) { + Pose2 outPose = result.at(X(i)); + outfile << outPose.x() << " " << outPose.y() << " " << outPose.theta() + << std::endl; + } + outfile.close(); + std::cout << "Output written to " << filename << std::endl; +} diff --git a/examples/Hybrid_City10000.cpp b/examples/Hybrid_City10000.cpp index b8318af08..76771ad36 100644 --- a/examples/Hybrid_City10000.cpp +++ b/examples/Hybrid_City10000.cpp @@ -29,36 +29,28 @@ #include #include -#include -#include #include #include #include #include #include +#include "City10000.h" + using namespace gtsam; -using namespace boost::algorithm; using symbol_shorthand::L; using symbol_shorthand::M; using symbol_shorthand::X; -auto kOpenLoopModel = noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10); -const double kOpenLoopConstant = kOpenLoopModel->negLogConstant(); - -auto kPriorNoiseModel = noiseModel::Diagonal::Sigmas( - (Vector(3) << 0.0001, 0.0001, 0.0001).finished()); - -auto kPoseNoiseModel = noiseModel::Diagonal::Sigmas( - (Vector(3) << 1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0).finished()); -const double kPoseNoiseConstant = kPoseNoiseModel->negLogConstant(); - // Experiment Class class Experiment { + /// The City10000 dataset + City10000Dataset dataset_; + public: // Parameters with default values - size_t maxLoopCount = 3000; + size_t maxLoopCount = 8000; // 3000: {1: 62s, 2: 21s, 3: 20s, 4: 31s, 5: 39s} No DT optimizations // 3000: {1: 65s, 2: 20s, 3: 16s, 4: 21s, 5: 28s} With DT optimizations @@ -73,32 +65,10 @@ class Experiment { double marginalThreshold = 0.9999; private: - std::string filename_; HybridSmoother smoother_; HybridNonlinearFactorGraph newFactors_, allFactors_; Values initial_; - /** - * @brief Write the result of optimization to file. - * - * @param result The Values object with the final result. - * @param num_poses The number of poses to write to the file. - * @param filename The file name to save the result to. - */ - void writeResult(const Values& result, size_t numPoses, - const std::string& filename = "Hybrid_city10000.txt") const { - std::ofstream outfile; - outfile.open(filename); - - for (size_t i = 0; i < numPoses; ++i) { - Pose2 outPose = result.at(X(i)); - outfile << outPose.x() << " " << outPose.y() << " " << outPose.theta() - << std::endl; - } - outfile.close(); - std::cout << "Output written to " << filename << std::endl; - } - /** * @brief Create a hybrid loop closure factor where * 0 - loose noise model and 1 - loop noise model. @@ -135,7 +105,7 @@ class Experiment { } /// @brief Perform smoother update and optimize the graph. - auto smootherUpdate(size_t maxNrHypotheses) { + clock_t smootherUpdate(size_t maxNrHypotheses) { std::cout << "Smoother update: " << newFactors_.size() << std::endl; gttic_(SmootherUpdate); clock_t beforeUpdate = clock(); @@ -148,7 +118,7 @@ class Experiment { } /// @brief Re-linearize, solve ALL, and re-initialize smoother. - auto reInitialize() { + clock_t reInitialize() { std::cout << "================= Re-Initialize: " << allFactors_.size() << std::endl; clock_t beforeUpdate = clock(); @@ -164,40 +134,13 @@ class Experiment { return afterUpdate - beforeUpdate; } - // Parse line from file - std::pair, std::pair> parseLine( - const std::string& line) const { - std::vector parts; - split(parts, line, is_any_of(" ")); - - size_t keyS = stoi(parts[1]); - size_t keyT = stoi(parts[3]); - - int numMeasurements = stoi(parts[5]); - std::vector poseArray(numMeasurements); - for (int i = 0; i < numMeasurements; ++i) { - double x = stod(parts[6 + 3 * i]); - double y = stod(parts[7 + 3 * i]); - double rad = stod(parts[8 + 3 * i]); - poseArray[i] = Pose2(x, y, rad); - } - return {poseArray, {keyS, keyT}}; - } - public: /// Construct with filename of experiment to run explicit Experiment(const std::string& filename) - : filename_(filename), smoother_(marginalThreshold) {} + : dataset_(filename), smoother_(marginalThreshold) {} /// @brief Run the main experiment with a given maxLoopCount. void run() { - // Prepare reading - std::ifstream in(filename_); - if (!in.is_open()) { - std::cerr << "Failed to open file: " << filename_ << std::endl; - return; - } - // Initialize local variables size_t discreteCount = 0, index = 0, loopCount = 0, updateCount = 0; @@ -221,9 +164,11 @@ class Experiment { Values result; size_t keyS = 0, keyT = 0; clock_t startTime = clock(); - std::string line; - while (getline(in, line) && index < maxLoopCount) { - auto [poseArray, keys] = parseLine(line); + + std::vector poseArray; + std::pair keys; + + while (dataset_.next(&poseArray, &keys) && index < maxLoopCount) { keyS = keys.first; keyT = keys.second; size_t numMeasurements = poseArray.size(); @@ -312,13 +257,6 @@ class Experiment { // Write results to file writeResult(result, keyT + 1, "Hybrid_City10000.txt"); - // TODO Write to file - // for (size_t i = 0; i < smoother_update_times.size(); i++) { - // auto p = smoother_update_times.at(i); - // std::cout << p.first << ", " << p.second / CLOCKS_PER_SEC << - // std::endl; - // } - // Write timing info to file std::ofstream outfileTime; std::string timeFileName = "Hybrid_City10000_time.txt";