From 39ee58a9628d95cdf93585e67501c2ea3f15bb52 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 30 Jan 2025 20:51:32 -0500 Subject: [PATCH] update hybrid City10000 example --- examples/Hybrid_City10000.cpp | 438 ++++++++++++++++++---------------- 1 file changed, 232 insertions(+), 206 deletions(-) diff --git a/examples/Hybrid_City10000.cpp b/examples/Hybrid_City10000.cpp index d6834192c..ca7f03e74 100644 --- a/examples/Hybrid_City10000.cpp +++ b/examples/Hybrid_City10000.cpp @@ -43,247 +43,273 @@ using symbol_shorthand::L; using symbol_shorthand::M; using symbol_shorthand::X; -// Testing params -const size_t max_loop_count = 2000; // 2000; // 200 //2000 //8000 +const size_t kMaxLoopCount = 2000; // Example default value +const size_t kMaxNrHypotheses = 10; -noiseModel::Diagonal::shared_ptr prior_noise_model = - noiseModel::Diagonal::Sigmas( - (Vector(3) << 0.0001, 0.0001, 0.0001).finished()); +auto kOpenLoopModel = noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10); -noiseModel::Diagonal::shared_ptr pose_noise_model = - noiseModel::Diagonal::Sigmas( - (Vector(3) << 1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0).finished()); +auto kPriorNoiseModel = noiseModel::Diagonal::Sigmas( + (Vector(3) << 0.0001, 0.0001, 0.0001).finished()); -/** - * @brief Write the result of optimization to filename. - * - * @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 write_result(const Values& result, size_t num_poses, - const std::string& filename = "Hybrid_city10000.txt") { - ofstream outfile; - outfile.open(filename); +auto kPoseNoiseModel = noiseModel::Diagonal::Sigmas( + (Vector(3) << 1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0).finished()); - for (size_t i = 0; i < num_poses; ++i) { - Pose2 out_pose = result.at(X(i)); +// Experiment Class +class Experiment { + private: + std::string filename_; + HybridSmoother smoother_; + HybridNonlinearFactorGraph graph_; + Values initial_; + Values result_; - outfile << out_pose.x() << " " << out_pose.y() << " " << out_pose.theta() - << std::endl; + /** + * @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") { + 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; } - 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. - * - * @param loop_counter - * @param key_s - * @param key_t - * @param measurement - * @return HybridNonlinearFactor - */ -HybridNonlinearFactor HybridLoopClosureFactor(size_t loop_counter, size_t key_s, - size_t key_t, - const Pose2& measurement) { - DiscreteKey l(L(loop_counter), 2); + /** + * @brief Create a hybrid loop closure factor where + * 0 - loose noise model and 1 - loop noise model. + */ + HybridNonlinearFactor hybridLoopClosureFactor(size_t loopCounter, size_t keyS, + size_t keyT, + const Pose2& measurement) { + DiscreteKey l(L(loopCounter), 2); - auto open_loop_model = noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10); - auto f0 = std::make_shared>( - X(key_s), X(key_t), measurement, open_loop_model); - auto f1 = std::make_shared>( - X(key_s), X(key_t), measurement, pose_noise_model); - std::vector factors{ - {f0, open_loop_model->negLogConstant()}, - {f1, pose_noise_model->negLogConstant()}}; - HybridNonlinearFactor mixtureFactor(l, {f0, f1}); - return mixtureFactor; -} + auto f0 = std::make_shared>( + X(keyS), X(keyT), measurement, kOpenLoopModel); + auto f1 = std::make_shared>( + X(keyS), X(keyT), measurement, kPoseNoiseModel); -HybridNonlinearFactor HybridOdometryFactor( - size_t num_measurements, size_t key_s, size_t key_t, const DiscreteKey& m, - const std::vector& pose_array, - const SharedNoiseModel& pose_noise_model) { - auto f0 = std::make_shared>( - X(key_s), X(key_t), pose_array[0], pose_noise_model); - auto f1 = std::make_shared>( - X(key_s), X(key_t), pose_array[1], pose_noise_model); - std::vector factors{{f0, 0.0}, {f1, 0.0}}; - HybridNonlinearFactor mixtureFactor(m, factors); - // HybridNonlinearFactor mixtureFactor(m, {f0, f1}); - return mixtureFactor; -} + std::vector factors{ + {f0, kOpenLoopModel->negLogConstant()}, + {f1, kPoseNoiseModel->negLogConstant()}}; + HybridNonlinearFactor mixtureFactor(l, factors); + return mixtureFactor; + } -void SmootherUpdate(HybridSmoother& smoother, HybridNonlinearFactorGraph& graph, - const Values& initial, size_t maxNrHypotheses, - Values* result) { - HybridGaussianFactorGraph linearized = *graph.linearize(initial); - smoother.update(linearized, maxNrHypotheses); - graph.resize(0); - // HybridValues delta = smoother.hybridBayesNet().optimize(); - // result->insert_or_assign(initial.retract(delta.continuous())); -} + /// @brief Create hybrid odometry factor with discrete measurement choices. + HybridNonlinearFactor hybridOdometryFactor( + size_t numMeasurements, size_t keyS, size_t keyT, const DiscreteKey& m, + const std::vector& poseArray) { + auto f0 = std::make_shared>( + X(keyS), X(keyT), poseArray[0], kPoseNoiseModel); + auto f1 = std::make_shared>( + X(keyS), X(keyT), poseArray[1], kPoseNoiseModel); -/* ************************************************************************* */ -int main(int argc, char* argv[]) { - ifstream in(findExampleDataFile("T1_city10000_04.txt")); - // ifstream in("../data/mh_T1_city10000_04.txt"); //Type #1 only - // ifstream in("../data/mh_T3b_city10000_10.txt"); //Type #3 only - // ifstream in("../data/mh_T1_T3_city10000_04.txt"); //Type #1 + Type #3 + std::vector factors{ + {f0, kPoseNoiseModel->negLogConstant()}, + {f1, kPoseNoiseModel->negLogConstant()}}; + HybridNonlinearFactor mixtureFactor(m, factors); + return mixtureFactor; + } - // ifstream in("../data/mh_All_city10000_groundtruth.txt"); + /// @brief Perform smoother update and optimize the graph. + void smootherUpdate(HybridSmoother& smoother, + HybridNonlinearFactorGraph& graph, const Values& initial, + size_t kMaxNrHypotheses, Values* result) { + HybridGaussianFactorGraph linearized = *graph.linearize(initial); + smoother.update(linearized, kMaxNrHypotheses); + // throw if x0 not in hybridBayesNet_: + const KeySet& keys = smoother.hybridBayesNet().keys(); + if (keys.find(X(0)) == keys.end()) { + throw std::runtime_error("x0 not in hybridBayesNet_"); + } + graph.resize(0); + // HybridValues delta = smoother.hybridBayesNet().optimize(); + // result->insert_or_assign(initial.retract(delta.continuous())); + } - size_t discrete_count = 0, index = 0; - size_t loop_count = 0; - size_t nrHybridFactors = 0; + public: + /// Construct with filename of experiment to run + explicit Experiment(const std::string& filename) + : filename_(filename), smoother_(0.99) {} - std::list time_list; - - HybridSmoother smoother(0.99); - - HybridNonlinearFactorGraph graph; - - Values initial, result; - - size_t maxNrHypotheses = 3; - - double x = 0.0; - double y = 0.0; - double rad = 0.0; - - Pose2 prior_pose(x, y, rad); - - initial.insert(X(0), prior_pose); - - graph.push_back(PriorFactor(X(0), prior_pose, prior_noise_model)); - - std::vector> smoother_update_times; - - clock_t before_update = clock(); - SmootherUpdate(smoother, graph, initial, maxNrHypotheses, &result); - clock_t after_update = clock(); - smoother_update_times.push_back({index, after_update - before_update}); - - size_t key_s, key_t{0}; - - clock_t start_time = clock(); - std::string str; - while (getline(in, str) && index < max_loop_count) { - vector parts; - split(parts, str, is_any_of(" ")); - - key_s = stoi(parts[1]); - key_t = stoi(parts[3]); - - int num_measurements = stoi(parts[5]); - vector pose_array(num_measurements); - for (int i = 0; i < num_measurements; ++i) { - x = stod(parts[6 + 3 * i]); - y = stod(parts[7 + 3 * i]); - rad = stod(parts[8 + 3 * i]); - pose_array[i] = Pose2(x, y, rad); + /// @brief Run the main experiment with a given maxLoopCount. + void run(size_t maxLoopCount) { + // Prepare reading + ifstream in(filename_); + if (!in.is_open()) { + cerr << "Failed to open file: " << filename_ << endl; + return; } - // Flag if we should run smoother update - bool smoother_update = false; + // Initialize local variables + size_t discreteCount = 0, index = 0; + size_t loopCount = 0; - // Take the first one as the initial estimate - Pose2 odom_pose = pose_array[0]; - if (key_s == key_t - 1) { // odometry + std::list timeList; - if (num_measurements > 1) { - DiscreteKey m(M(discrete_count), num_measurements); + // Set up initial prior + double x = 0.0; + double y = 0.0; + double rad = 0.0; - // Add hybrid factor which considers both measurements - HybridNonlinearFactor mixtureFactor = HybridOdometryFactor( - num_measurements, key_s, key_t, m, pose_array, pose_noise_model); - graph.push_back(mixtureFactor); + Pose2 priorPose(x, y, rad); + initial_.insert(X(0), priorPose); + graph_.push_back(PriorFactor(X(0), priorPose, kPriorNoiseModel)); - discrete_count++; + // Initial update + clock_t beforeUpdate = clock(); + smootherUpdate(smoother_, graph_, initial_, kMaxNrHypotheses, &result_); + clock_t afterUpdate = clock(); + std::vector> smootherUpdateTimes; + smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate}); - smoother_update = true; + // Start main loop + size_t keyS = 0, keyT = 0; + clock_t startTime = clock(); + std::string line; + while (getline(in, line) && index < maxLoopCount) { + std::vector parts; + split(parts, line, is_any_of(" ")); - } else { - graph.add(BetweenFactor(X(key_s), X(key_t), odom_pose, - pose_noise_model)); + keyS = stoi(parts[1]); + keyT = stoi(parts[3]); + + int numMeasurements = stoi(parts[5]); + std::vector poseArray(numMeasurements); + for (int i = 0; i < numMeasurements; ++i) { + x = stod(parts[6 + 3 * i]); + y = stod(parts[7 + 3 * i]); + rad = stod(parts[8 + 3 * i]); + poseArray[i] = Pose2(x, y, rad); } - initial.insert(X(key_t), initial.at(X(key_s)) * odom_pose); + // Flag to decide whether to run smoother update + bool doSmootherUpdate = false; - } else { // loop - HybridNonlinearFactor loop_factor = - HybridLoopClosureFactor(loop_count, key_s, key_t, odom_pose); - graph.add(loop_factor); + // Take the first one as the initial estimate + Pose2 odomPose = poseArray[0]; + if (keyS == keyT - 1) { + // Odometry factor + if (numMeasurements > 1) { + // Add hybrid factor + DiscreteKey m(M(discreteCount), numMeasurements); + HybridNonlinearFactor mixtureFactor = + hybridOdometryFactor(numMeasurements, keyS, keyT, m, poseArray); + graph_.push_back(mixtureFactor); + discreteCount++; + doSmootherUpdate = true; + // std::cout << "mixtureFactor: " << keyS << " " << keyT << std::endl; + } else { + graph_.add(BetweenFactor(X(keyS), X(keyT), odomPose, + kPoseNoiseModel)); + } + // Insert next pose initial guess + initial_.insert(X(keyT), initial_.at(X(keyS)) * odomPose); + } else { + // Loop closure + HybridNonlinearFactor loopFactor = + hybridLoopClosureFactor(loopCount, keyS, keyT, odomPose); + // print loop closure event keys: + // std::cout << "Loop closure: " << keyS << " " << keyT << std::endl; + graph_.add(loopFactor); + doSmootherUpdate = true; + loopCount++; + } - smoother_update = true; + if (doSmootherUpdate) { + gttic_(SmootherUpdate); + beforeUpdate = clock(); + smootherUpdate(smoother_, graph_, initial_, kMaxNrHypotheses, &result_); + afterUpdate = clock(); + smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate}); + gttoc_(SmootherUpdate); + doSmootherUpdate = false; + } - loop_count++; + // Record timing for odometry edges only + if (keyS == keyT - 1) { + clock_t curTime = clock(); + timeList.push_back(curTime - startTime); + } + + // Print some status every 100 steps + if (index % 100 == 0) { + std::cout << "Index: " << index << std::endl; + if (!timeList.empty()) { + std::cout << "Acc_time: " << timeList.back() / CLOCKS_PER_SEC + << " seconds" << std::endl; + // delta.discrete().print("The Discrete Assignment"); + tictoc_finishedIteration_(); + tictoc_print_(); + } + } + + index++; } - if (smoother_update) { - gttic_(SmootherUpdate); - before_update = clock(); - SmootherUpdate(smoother, graph, initial, maxNrHypotheses, &result); - after_update = clock(); - smoother_update_times.push_back({index, after_update - before_update}); - gttoc_(SmootherUpdate); - } + // Final update + beforeUpdate = clock(); + smootherUpdate(smoother_, graph_, initial_, kMaxNrHypotheses, &result_); + afterUpdate = clock(); + smootherUpdateTimes.push_back({index, afterUpdate - beforeUpdate}); - // Print loop index and time taken in processor clock ticks - // if (index % 50 == 0 && key_s != key_t - 1) { - if (index % 100 == 0) { - std::cout << "index: " << index << std::endl; - std::cout << "acc_time: " << time_list.back() / CLOCKS_PER_SEC - << std::endl; - // delta.discrete().print("The Discrete Assignment"); - tictoc_finishedIteration_(); - tictoc_print_(); - } + // Final optimize + gttic_(HybridSmootherOptimize); + HybridValues delta = smoother_.optimize(); + gttoc_(HybridSmootherOptimize); - if (key_s == key_t - 1) { - clock_t cur_time = clock(); - time_list.push_back(cur_time - start_time); - } + result_.insert_or_assign(initial_.retract(delta.continuous())); - index += 1; + std::cout << "Final error: " << smoother_.hybridBayesNet().error(delta) + << std::endl; + + clock_t endTime = clock(); + clock_t totalTime = endTime - startTime; + std::cout << "Total time: " << totalTime / CLOCKS_PER_SEC << " seconds" + << std::endl; + + // 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 + ofstream outfileTime; + std::string timeFileName = "Hybrid_City10000_time.txt"; + outfileTime.open(timeFileName); + for (auto accTime : timeList) { + outfileTime << accTime << std::endl; + } + outfileTime.close(); + std::cout << "Output " << timeFileName << " file." << std::endl; } +}; - before_update = clock(); - SmootherUpdate(smoother, graph, initial, maxNrHypotheses, &result); - after_update = clock(); - smoother_update_times.push_back({index, after_update - before_update}); +/* ************************************************************************* */ +int main() { + Experiment experiment(findExampleDataFile("T1_city10000_04.txt")); + // Experiment experiment("../data/mh_T1_city10000_04.txt"); //Type #1 only + // Experiment experiment("../data/mh_T3b_city10000_10.txt"); //Type #3 only + // Experiment experiment("../data/mh_T1_T3_city10000_04.txt"); //Type #1 + + // Type #3 - gttic_(HybridSmootherOptimize); - HybridValues delta = smoother.hybridBayesNet().optimize(); - gttoc_(HybridSmootherOptimize); - result.insert_or_assign(initial.retract(delta.continuous())); - - std::cout << "Final error: " << smoother.hybridBayesNet().error(delta) - << std::endl; - clock_t end_time = clock(); - clock_t total_time = end_time - start_time; - cout << "total_time: " << total_time / CLOCKS_PER_SEC << " seconds" << endl; - - /// Write result to file - write_result(result, (key_t + 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; - // } - ofstream outfile_time; - std::string time_file_name = "Hybrid_City10000_time.txt"; - outfile_time.open(time_file_name); - for (auto acc_time : time_list) { - outfile_time << acc_time << std::endl; - } - outfile_time.close(); - cout << "output " << time_file_name << " file." << endl; - std::cout << nrHybridFactors << std::endl; + // Run the experiment + experiment.run(kMaxLoopCount); return 0; }