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

cleanup

release/4.3a0
Varun Agrawal 2025-01-29 19:00:31 -05:00
parent ec5ef222c9
commit 5b37647e11
2 changed files with 14 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
examples/Hybrid_City10000.cpp
View File

@ -119,7 +119,6 @@ void SmootherUpdate(HybridSmoother& smoother, HybridNonlinearFactorGraph& graph,
const Values& initial, size_t maxNrHypotheses, const Values& initial, size_t maxNrHypotheses,
Values* result) { Values* result) {
HybridGaussianFactorGraph linearized = *graph.linearize(initial); HybridGaussianFactorGraph linearized = *graph.linearize(initial);
// std::cout << "index: " << index << std::endl;
smoother.update(linearized, maxNrHypotheses); smoother.update(linearized, maxNrHypotheses);
graph.resize(0); graph.resize(0);
// HybridValues delta = smoother.hybridBayesNet().optimize(); // HybridValues delta = smoother.hybridBayesNet().optimize();

21
examples/plot_city10000.m
View File

@ -2,8 +2,10 @@ clear;
gt = dlmread('Data/ISAM2_GT_city10000.txt'); gt = dlmread('Data/ISAM2_GT_city10000.txt');
% Generate by running `make ISAM2_City10000.run`
eh_poses = dlmread('../build/examples/ISAM2_city10000.txt'); eh_poses = dlmread('../build/examples/ISAM2_city10000.txt');
% Generate by running `make Hybrid_City10000.run`
h_poses = dlmread('../build/examples/HybridISAM_city10000.txt'); h_poses = dlmread('../build/examples/HybridISAM_city10000.txt');
% Plot the same number of GT poses as estimated ones % Plot the same number of GT poses as estimated ones
@ -16,13 +18,18 @@ figure(1)
hold on; hold on;
axis equal; axis equal;
axis([-65 65 -75 60]) 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]); 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]); 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; hold off;