12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #2010 from borglab/city10000 

Updates to City10000 Example
release/4.3a0
Varun Agrawal 2025-02-07 12:28:05 -05:00 committed by GitHub
parent 885e8ef9f6 a7b38e9ba0
commit 2998d988dd
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
7 changed files with 332 additions and 246 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

110
examples/City10000.h Normal file
View File

@ -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 <gtsam/geometry/Pose2.h>
#include <fstream>
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<std::string> readLine(const std::string& line,
const std::string& delimiter = " ") const {
std::vector<std::string> 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::vector<Pose2>, std::pair<size_t, size_t>> parseLine(
const std::string& line) const {
std::vector<std::string> parts = readLine(line);
size_t keyS = stoi(parts[1]);
size_t keyT = stoi(parts[3]);
int numMeasurements = stoi(parts[5]);
std::vector<Pose2> 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<Pose2>* poseArray, std::pair<size_t, size_t>* 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<Pose2>(X(i));
outfile << outPose.x() << " " << outPose.y() << " " << outPose.theta()
<< std::endl;
}
outfile.close();
std::cout << "Output written to " << filename << std::endl;
}

90
examples/Hybrid_City10000.cpp
View File

@ -29,36 +29,28 @@
#include <gtsam/slam/dataset.h>
#include <time.h>
#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>
#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<Pose2>(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::vector<Pose2>, std::pair<size_t, size_t>> parseLine(
const std::string& line) const {
std::vector<std::string> 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<Pose2> 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<Pose2> poseArray;
std::pair<size_t, size_t> 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";

330
examples/ISAM2_City10000.cpp
View File

@ -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 <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/ISAM2Params.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/BetweenFactor.h>
@ -32,178 +33,189 @@
#include <string>
#include <vector>
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<Pose2>(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<double> timeList;
// Set up initial prior
Pose2 priorPose(0, 0, 0);
initial_.insert(X(0), priorPose);
graph_.addPrior<Pose2>(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<Pose2> poseArray;
std::pair<size_t, size_t> 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<Pose2>(X(keyS)) * odomPose);
graph_.add(
BetweenFactor<Pose2>(X(keyS), X(keyT), odomPose, kPoseNoiseModel));
} else { // loop
int id = index % numMeasurements;
if (isWithAmbiguity && id % 2 == 0) {
graph_.add(BetweenFactor<Pose2>(X(keyS), X(keyT), odomPose,
kPoseNoiseModel));
} else {
graph_.add(BetweenFactor<Pose2>(
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<Pose2>(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 <value> Set the maximum loop "
"count (default: 2000)\n"
<< " --is-with-ambiguity <value=0/1> 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<double> 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<Pose2>(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<string> parts;
split(parts, str, is_any_of(" "));
key_s = stoi(parts[1]);
key_t = stoi(parts[3]);
int num_measurements = stoi(parts[5]);
vector<Pose2> 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<Pose2>(X(key_s)) * odom_pose);
pose_count++;
} else { // loop
index++;
}
graph->add(
BetweenFactor<Pose2>(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<Pose2>(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;
}

26
examples/plot_city10000.m
View File

@ -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;

4
gtsam/discrete/DecisionTreeFactor.cpp
View File

@ -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 {

4
gtsam/hybrid/HybridBayesNet.cpp
View File

@ -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);

14
gtsam/hybrid/HybridSmoother.cpp
View File

@ -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