diff --git a/examples/City10000.h b/examples/City10000.h new file mode 100644 index 000000000..f90760c40 --- /dev/null +++ b/examples/City10000.h @@ -0,0 +1,110 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file City10000.h + * @brief Class for City10000 dataset + * @author Varun Agrawal + * @date February 3, 2025 + */ + +#include + +#include + +using namespace gtsam; + +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(); + +class City10000Dataset { + std::ifstream in_; + + /// Read a `line` from the dataset, separated by the `delimiter`. + std::vector readLine(const std::string& line, + const std::string& delimiter = " ") const { + std::vector parts; + auto start = 0U; + auto end = line.find(delimiter); + while (end != std::string::npos) { + parts.push_back(line.substr(start, end - start)); + start = end + delimiter.length(); + end = line.find(delimiter, start); + } + return parts; + } + + public: + City10000Dataset(const std::string& filename) : in_(filename) { + if (!in_.is_open()) { + std::cerr << "Failed to open file: " << filename << std::endl; + } + } + + /// Parse line from file + std::pair, std::pair> parseLine( + const std::string& line) const { + std::vector parts = readLine(line); + + 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}}; + } + + /// Read and parse the next line. + bool next(std::vector* poseArray, std::pair* keys) { + std::string line; + if (getline(in_, line)) { + std::tie(*poseArray, *keys) = parseLine(line); + return true; + } else + return false; + } +}; + +/** + * @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") { + 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"; diff --git a/examples/ISAM2_City10000.cpp b/examples/ISAM2_City10000.cpp index aaad608ac..2c210ab44 100644 --- a/examples/ISAM2_City10000.cpp +++ b/examples/ISAM2_City10000.cpp @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file Hybrid_City10000.cpp - * @brief Example of using hybrid estimation + * @file ISAM2_City10000.cpp + * @brief Example of using ISAM2 estimation * with multiple odometry measurements. * @author Varun Agrawal * @date January 22, 2025 @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -32,178 +33,189 @@ #include #include -using namespace std; +#include "City10000.h" + using namespace gtsam; -using namespace boost::algorithm; using symbol_shorthand::X; -// Testing params -const size_t max_loop_count = 2000; // 200 //2000 //8000 +// Experiment Class +class Experiment { + /// The City10000 dataset + City10000Dataset dataset_; -const bool is_with_ambiguity = false; // run original iSAM2 without ambiguities -// const bool is_with_ambiguity = true; // run original iSAM2 with ambiguities + public: + // Parameters with default values + size_t maxLoopCount = 2000; // 200 //2000 //8000 -noiseModel::Diagonal::shared_ptr prior_noise_model = - noiseModel::Diagonal::Sigmas( - (Vector(3) << 0.0001, 0.0001, 0.0001).finished()); + // false: run original iSAM2 without ambiguities + // true: run original iSAM2 with ambiguities + bool isWithAmbiguity; -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()); + private: + ISAM2 isam2_; + NonlinearFactorGraph graph_; + Values initial_; + Values results; -/** - * @brief Write the results of optimization to filename. - * - * @param results The Values object with the final results. - * @param num_poses The number of poses to write to the file. - * @param filename The file name to save the results to. - */ -void write_results(const Values& results, size_t num_poses, - const std::string& filename = "ISAM2_city10000.txt") { - ofstream outfile; - outfile.open(filename); - - for (size_t i = 0; i < num_poses; ++i) { - Pose2 out_pose = results.at(X(i)); - - outfile << out_pose.x() << " " << out_pose.y() << " " << out_pose.theta() - << std::endl; + public: + /// Construct with filename of experiment to run + explicit Experiment(const std::string& filename, bool isWithAmbiguity = false) + : dataset_(filename), isWithAmbiguity(isWithAmbiguity) { + ISAM2Params parameters; + parameters.optimizationParams = gtsam::ISAM2GaussNewtonParams(0.0); + parameters.relinearizeThreshold = 0.01; + parameters.relinearizeSkip = 1; + isam2_ = ISAM2(parameters); + } + + /// @brief Run the main experiment with a given maxLoopCount. + void run() { + // Initialize local variables + size_t index = 0; + + std::list timeList; + + // Set up initial prior + Pose2 priorPose(0, 0, 0); + initial_.insert(X(0), priorPose); + graph_.addPrior(X(0), priorPose, kPriorNoiseModel); + + // Initial update + isam2_.update(graph_, initial_); + graph_.resize(0); + initial_.clear(); + results = isam2_.calculateBestEstimate(); + + // Start main loop + size_t keyS = 0; + size_t keyT = 0; + clock_t startTime = clock(); + + std::vector poseArray; + std::pair keys; + + while (dataset_.next(&poseArray, &keys) && index < maxLoopCount) { + keyS = keys.first; + keyT = keys.second; + size_t numMeasurements = poseArray.size(); + + Pose2 odomPose; + if (isWithAmbiguity) { + // Get wrong intentionally + int id = index % numMeasurements; + odomPose = Pose2(poseArray[id]); + } else { + odomPose = poseArray[0]; + } + + if (keyS == keyT - 1) { // new X(key) + initial_.insert(X(keyT), results.at(X(keyS)) * odomPose); + graph_.add( + BetweenFactor(X(keyS), X(keyT), odomPose, kPoseNoiseModel)); + + } else { // loop + int id = index % numMeasurements; + if (isWithAmbiguity && id % 2 == 0) { + graph_.add(BetweenFactor(X(keyS), X(keyT), odomPose, + kPoseNoiseModel)); + } else { + graph_.add(BetweenFactor( + X(keyS), X(keyT), odomPose, + noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10.0))); + } + index++; + } + + isam2_.update(graph_, initial_); + graph_.resize(0); + initial_.clear(); + results = isam2_.calculateBestEstimate(); + + // Print loop index and time taken in processor clock ticks + if (index % 50 == 0 && keyS != keyT - 1) { + std::cout << "index: " << index << std::endl; + std::cout << "accTime: " << timeList.back() / CLOCKS_PER_SEC + << std::endl; + } + + if (keyS == keyT - 1) { + clock_t curTime = clock(); + timeList.push_back(curTime - startTime); + } + + if (timeList.size() % 100 == 0 && (keyS == keyT - 1)) { + std::string stepFileIdx = std::to_string(100000 + timeList.size()); + + std::ofstream stepOutfile; + std::string stepFileName = "step_files/ISAM2_City10000_S" + stepFileIdx; + stepOutfile.open(stepFileName + ".txt"); + for (size_t i = 0; i < (keyT + 1); ++i) { + Pose2 outPose = results.at(X(i)); + stepOutfile << outPose.x() << " " << outPose.y() << " " + << outPose.theta() << std::endl; + } + stepOutfile.close(); + } + } + + clock_t endTime = clock(); + clock_t totalTime = endTime - startTime; + std::cout << "totalTime: " << totalTime / CLOCKS_PER_SEC << std::endl; + + /// Write results to file + writeResult(results, (keyT + 1), "ISAM2_City10000.txt"); + + std::ofstream outfileTime; + std::string timeFileName = "ISAM2_City10000_time.txt"; + outfileTime.open(timeFileName); + for (auto accTime : timeList) { + outfileTime << accTime << std::endl; + } + outfileTime.close(); + std::cout << "Written cumulative time to: " << timeFileName << " file." + << std::endl; + } +}; + +/* ************************************************************************* */ +// Function to parse command-line arguments +void parseArguments(int argc, char* argv[], size_t& maxLoopCount, + bool& isWithAmbiguity) { + for (int i = 1; i < argc; ++i) { + std::string arg = argv[i]; + if (arg == "--max-loop-count" && i + 1 < argc) { + maxLoopCount = std::stoul(argv[++i]); + } else if (arg == "--is-with-ambiguity" && i + 1 < argc) { + isWithAmbiguity = bool(std::stoul(argv[++i])); + } else if (arg == "--help") { + std::cout << "Usage: " << argv[0] << " [options]\n" + << "Options:\n" + << " --max-loop-count Set the maximum loop " + "count (default: 2000)\n" + << " --is-with-ambiguity Set whether to use " + "ambiguous measurements " + "(default: false)\n" + << " --help Show this help message\n"; + std::exit(0); + } } - outfile.close(); - std::cout << "output written to " << filename << std::endl; } /* ************************************************************************* */ 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 + 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 - // ifstream in("../data/mh_All_city10000_groundtruth.txt"); + // Parse command-line arguments + parseArguments(argc, argv, experiment.maxLoopCount, + experiment.isWithAmbiguity); - size_t pose_count = 0; - size_t index = 0; - - std::list time_list; - - ISAM2Params parameters; - parameters.optimizationParams = gtsam::ISAM2GaussNewtonParams(0.0); - parameters.relinearizeThreshold = 0.01; - parameters.relinearizeSkip = 1; - - ISAM2* isam2 = new ISAM2(parameters); - - NonlinearFactorGraph* graph = new NonlinearFactorGraph(); - - Values init_values; - Values results; - - double x = 0.0; - double y = 0.0; - double rad = 0.0; - - Pose2 prior_pose(x, y, rad); - - init_values.insert(X(0), prior_pose); - pose_count++; - - graph->addPrior(X(0), prior_pose, prior_noise_model); - - isam2->update(*graph, init_values); - graph->resize(0); - init_values.clear(); - results = isam2->calculateBestEstimate(); - - //* - size_t key_s = 0; - size_t key_t = 0; - - clock_t start_time = clock(); - string str; - while (getline(in, str) && index < max_loop_count) { - // cout << str << endl; - 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); - } - - Pose2 odom_pose; - if (is_with_ambiguity) { - // Get wrong intentionally - int id = index % num_measurements; - odom_pose = Pose2(pose_array[id]); - } else { - odom_pose = pose_array[0]; - } - - if (key_s == key_t - 1) { // new X(key) - init_values.insert(X(key_t), results.at(X(key_s)) * odom_pose); - pose_count++; - } else { // loop - index++; - } - graph->add( - BetweenFactor(X(key_s), X(key_t), odom_pose, pose_noise_model)); - - isam2->update(*graph, init_values); - graph->resize(0); - init_values.clear(); - results = isam2->calculateBestEstimate(); - - // Print loop index and time taken in processor clock ticks - if (index % 50 == 0 && key_s != key_t - 1) { - std::cout << "index: " << index << std::endl; - std::cout << "acc_time: " << time_list.back() / CLOCKS_PER_SEC - << std::endl; - } - - if (key_s == key_t - 1) { - clock_t cur_time = clock(); - time_list.push_back(cur_time - start_time); - } - - if (time_list.size() % 100 == 0 && (key_s == key_t - 1)) { - string step_file_idx = std::to_string(100000 + time_list.size()); - - ofstream step_outfile; - string step_file_name = "step_files/ISAM2_city10000_S" + step_file_idx; - step_outfile.open(step_file_name + ".txt"); - for (size_t i = 0; i < (key_t + 1); ++i) { - Pose2 out_pose = results.at(X(i)); - step_outfile << out_pose.x() << " " << out_pose.y() << " " - << out_pose.theta() << endl; - } - step_outfile.close(); - } - } - - clock_t end_time = clock(); - clock_t total_time = end_time - start_time; - cout << "total_time: " << total_time / CLOCKS_PER_SEC << endl; - - /// Write results to file - write_results(results, (key_t + 1)); - - ofstream outfile_time; - std::string time_file_name = "ISAM2_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; + // Run the experiment + experiment.run(); return 0; } diff --git a/examples/plot_city10000.m b/examples/plot_city10000.m index 6d6a4637c..f30635d85 100644 --- a/examples/plot_city10000.m +++ b/examples/plot_city10000.m @@ -2,12 +2,13 @@ clear; gt = dlmread('Data/ISAM2_GT_city10000.txt'); -eh_poses = dlmread('../build/examples/ISAM2_city10000.txt'); +% Generate by running `make ISAM2_City10000.run` +eh_poses = dlmread('../build/examples/ISAM2_City10000.txt'); -h_poses = dlmread('../build/examples/HybridISAM_city10000.txt'); +% Generate by running `make Hybrid_City10000.run` +h_poses = dlmread('../build/examples/Hybrid_City10000.txt'); % Plot the same number of GT poses as estimated ones -% gt = gt(1:size(eh_poses, 1), :); gt = gt(1:size(h_poses, 1), :); eh_poses = eh_poses(1:size(h_poses, 1), :); @@ -16,13 +17,18 @@ figure(1) hold on; axis equal; axis([-65 65 -75 60]) +% title('City10000 result with Hybrid Factor Graphs'); plot(gt(:,1), gt(:,2), '--', 'LineWidth', 4, 'color', [0.1 0.7 0.1 0.5]); -% hold off; - -% figure(2) -% hold on; -% axis equal; -% axis([-65 65 -75 60]) -plot(eh_poses(:,1), eh_poses(:,2), '-', 'LineWidth', 2, 'color', [0.9 0.1 0. 0.4]); plot(h_poses(:,1), h_poses(:,2), '-', 'LineWidth', 2, 'color', [0.1 0.1 0.9 0.4]); +legend('Ground truth', 'Hybrid Factor Graphs'); +hold off; + +figure(2) +hold on; +axis equal; +axis([-65 65 -75 60]) +% title('City10000 result with ISAM2'); +plot(gt(:,1), gt(:,2), '--', 'LineWidth', 4, 'color', [0.1 0.7 0.1 0.5]); +plot(eh_poses(:,1), eh_poses(:,2), '-', 'LineWidth', 2, 'color', [0.9 0.1 0. 0.4]); +legend('Ground truth', 'ISAM2'); hold off; diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 307d1486c..931e603a7 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -523,6 +523,10 @@ namespace gtsam { // Check if value is less than the threshold and // we haven't exceeded the maximum number of leaves. + // TODO(Varun): Bug since we can have a case where we need to prune higher + // probabilities after we have reached N. + // E.g. N=3 for [0.2, 0.2, 0.1, 0.2, 0.3] + // will give [0.2, 0.2, 0.0, 0.2, 0.0] if (value < threshold || total >= N) { return 0.0; } else { diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index fb8b215cf..6ce2206c0 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -95,6 +95,10 @@ HybridBayesNet HybridBayesNet::prune( "HybrdiBayesNet::prune: Unknown HybridConditional type."); } +#if GTSAM_HYBRID_TIMING + gttoc_(HybridPruning); +#endif + // Add the pruned discrete conditionals to the result. for (const DiscreteConditional::shared_ptr &discrete : prunedBN) result.push_back(discrete); diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp index 51fd1f1db..a67777b52 100644 --- a/gtsam/hybrid/HybridSmoother.cpp +++ b/gtsam/hybrid/HybridSmoother.cpp @@ -89,8 +89,14 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &newFactors, ordering = *given_ordering; } +#if GTSAM_HYBRID_TIMING + gttic_(HybridSmootherEliminate); +#endif // Eliminate. HybridBayesNet bayesNetFragment = *updatedGraph.eliminateSequential(ordering); +#if GTSAM_HYBRID_TIMING + gttoc_(HybridSmootherEliminate); +#endif #ifdef DEBUG_SMOOTHER_DETAIL for (auto conditional : bayesNetFragment) { @@ -110,12 +116,18 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &newFactors, /// Prune if (maxNrLeaves) { +#if GTSAM_HYBRID_TIMING + gttic_(HybridSmootherPrune); +#endif // `pruneBayesNet` sets the leaves with 0 in discreteFactor to nullptr in // all the conditionals with the same keys in bayesNetFragment. DiscreteValues newlyFixedValues; bayesNetFragment = bayesNetFragment.prune(*maxNrLeaves, marginalThreshold_, &newlyFixedValues); fixedValues_.insert(newlyFixedValues); +#if GTSAM_HYBRID_TIMING + gttoc_(HybridSmootherPrune); +#endif } #ifdef DEBUG_SMOOTHER @@ -158,7 +170,7 @@ HybridSmoother::addConditionals(const HybridGaussianFactorGraph &newFactors, // in the previous `hybridBayesNet` to the graph // New conditionals to add to the graph - gtsam::HybridBayesNet newConditionals; + HybridBayesNet newConditionals; // NOTE(Varun) Using a for-range loop doesn't work since some of the // conditionals are invalid pointers