38 using namespace gtsam;
49 TEST(HybridGaussianElimination, IncrementalElimination) {
95 TEST(HybridGaussianElimination, IncrementalInference) {
114 auto discreteConditional_m0 =
isam[
M(0)]->conditional()->asDiscrete();
132 const auto [expectedHybridBayesTree, expectedRemainingGraph] =
136 auto x0_conditional = dynamic_pointer_cast<HybridGaussianConditional>(
137 isam[
X(0)]->conditional()->inner());
138 auto expected_x0_conditional =
139 dynamic_pointer_cast<HybridGaussianConditional>(
140 (*expectedHybridBayesTree)[
X(0)]->conditional()->inner());
144 auto x1_conditional = dynamic_pointer_cast<HybridGaussianConditional>(
145 isam[
X(1)]->conditional()->inner());
146 auto expected_x1_conditional =
147 dynamic_pointer_cast<HybridGaussianConditional>(
148 (*expectedHybridBayesTree)[
X(1)]->conditional()->inner());
152 auto x2_conditional = dynamic_pointer_cast<HybridGaussianConditional>(
153 isam[
X(2)]->conditional()->inner());
154 auto expected_x2_conditional =
155 dynamic_pointer_cast<HybridGaussianConditional>(
156 (*expectedHybridBayesTree)[
X(2)]->conditional()->inner());
163 expectedRemainingGraph->BaseEliminateable::eliminateMultifrontal(
167 m00[
M(0)] = 0, m00[
M(1)] = 0;
169 *(*discreteBayesTree)[
M(1)]->conditional()->asDiscrete();
170 double m00_prob = decisionTree(m00);
172 auto discreteConditional =
isam[
M(1)]->conditional()->asDiscrete();
177 assignment[
M(0)] = 0;
178 assignment[
M(1)] = 0;
180 assignment[
M(0)] = 1;
181 assignment[
M(1)] = 0;
183 assignment[
M(0)] = 0;
184 assignment[
M(1)] = 1;
186 assignment[
M(0)] = 1;
187 assignment[
M(1)] = 1;
192 auto expectedChordal =
193 expectedRemainingGraph->BaseEliminateable::eliminateMultifrontal();
194 auto actualConditional = dynamic_pointer_cast<DecisionTreeFactor>(
195 isam[
M(1)]->conditional()->inner());
198 vector<double> probs = {0.095292197, 0.31417524, 0.28275772, 0.30777485};
199 auto expectedConditional =
200 std::make_shared<DecisionTreeFactor>(discrete_keys, probs);
206 TEST(HybridGaussianElimination, Approx_inference) {
212 for (
size_t i = 1;
i < 4;
i++) {
219 for (
size_t i = 4;
i <= 7;
i++) {
225 for (
size_t j = 0;
j < 4;
j++) {
230 const auto [unprunedHybridBayesTree, unprunedRemainingGraph] =
233 size_t maxNrLeaves = 5;
234 incrementalHybrid.
update(graph1);
236 incrementalHybrid.
prune(maxNrLeaves);
272 auto discreteConditional_m0 = *dynamic_pointer_cast<DiscreteConditional>(
273 incrementalHybrid[
M(0)]->conditional()->inner());
277 auto count = [](
const double &
value,
int count) {
278 return value > 0 ? count + 1 : count;
285 auto &unprunedLastDensity = *dynamic_pointer_cast<HybridGaussianConditional>(
286 unprunedHybridBayesTree->clique(
X(3))->conditional()->inner());
287 auto &lastDensity = *dynamic_pointer_cast<HybridGaussianConditional>(
288 incrementalHybrid[
X(3)]->conditional()->inner());
290 std::vector<std::pair<DiscreteValues, double>> assignments =
291 discreteConditional_m0.enumerate();
293 for (
auto &&av : assignments) {
295 const double value = av.second;
298 EXPECT(lastDensity(assignment) ==
nullptr);
300 CHECK(lastDensity(assignment));
302 *lastDensity(assignment)));
309 TEST(HybridGaussianElimination, Incremental_approximate) {
316 for (
size_t i = 1;
i < 4;
i++) {
323 for (
size_t i = 5;
i <= 7;
i++) {
328 size_t maxComponents = 5;
329 incrementalHybrid.
update(graph1);
330 incrementalHybrid.
prune(maxComponents);
336 2, incrementalHybrid[
X(0)]->conditional()->asHybrid()->nrComponents());
338 3, incrementalHybrid[
X(1)]->conditional()->asHybrid()->nrComponents());
340 5, incrementalHybrid[
X(2)]->conditional()->asHybrid()->nrComponents());
342 5, incrementalHybrid[
X(3)]->conditional()->asHybrid()->nrComponents());
351 incrementalHybrid.
prune(maxComponents);
357 5, incrementalHybrid[
X(3)]->conditional()->asHybrid()->nrComponents());
359 5, incrementalHybrid[
X(4)]->conditional()->asHybrid()->nrComponents());
375 auto priorNoise = noiseModel::Diagonal::Sigmas(
380 auto poseNoise = noiseModel::Isotropic::Sigma(3, 0.1);
417 auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
418 std::vector<NoiseModelFactor::shared_ptr>
components;
527 auto discreteTree = inc[
M(3)]->conditional()->asDiscrete();
536 expected_assignment[
M(1)] = 1;
537 expected_assignment[
M(2)] = 1;
538 expected_assignment[
M(3)] = 1;
544 auto lastConditional = inc[
X(3)]->conditional()->asHybrid();