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

kill test since we no longer need it

release/4.3a0
Varun Agrawal 2025-02-01 02:02:18 -05:00
parent 7dc5b4bbd0
commit 1e48f82a43
1 changed files with 0 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
gtsam/hybrid/tests/testHybridSmoother.cpp
View File

@ -70,56 +70,6 @@ Switching InitializeEstimationProblem(
} // namespace estimation_fixture
/****************************************************************************/
// Test HybridSmoother::addConditionals
TEST(HybridSmoother, AddConditionals) {
using namespace estimation_fixture;
size_t K = 7;
// Switching example of robot moving in 1D
// with given measurements and equal mode priors.
HybridNonlinearFactorGraph graph;
Values initial;
Switching switching = InitializeEstimationProblem(
K, 1.0, 0.1, measurements, "1/1 1/1", &graph, &initial);
HybridSmoother smoother(0.5);
constexpr size_t maxNrLeaves = 5;
// Loop over timesteps from 1...K-1
for (size_t k = 1; k < K; k++) {
if (k > 1) graph.push_back(switching.modeChain.at(k - 1)); // Mode chain
graph.push_back(switching.binaryFactors.at(k - 1)); // Motion Model
graph.push_back(switching.unaryFactors.at(k)); // Measurement
// Add a loop closure factor every 3rd step between `k-3` and `k`.
if (k % 3 == 0) {
size_t l = int(k / 3);
auto open = std::make_shared<MotionModel>(
X(k - 3), X(k), 0.0, noiseModel::Isotropic::Sigma(1, 10.0)),
close = std::make_shared<MotionModel>(
X(k - 3), X(k), 0.0, noiseModel::Isotropic::Sigma(1, 0.1));
HybridNonlinearFactor loopFactor(
DiscreteKey(L(l), 2),
std::vector<NoiseModelFactor::shared_ptr>{open, close});
graph.push_back(loopFactor);
}
initial.insert(X(k), switching.linearizationPoint.at<double>(X(k)));
HybridGaussianFactorGraph linearized = *graph.linearize(initial);
smoother.update(linearized, maxNrLeaves);
// Clear all the factors from the graph
graph.resize(0);
}
//TODO(Varun) recreate scenario which would fail before commit b627fc422.
}
/****************************************************************************/
// Test approximate inference with an additional pruning step.
TEST(HybridSmoother, IncrementalSmoother) {