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 =
137 dynamic_pointer_cast<GaussianMixture>(
isam[
X(0)]->conditional()->inner());
138 auto expected_x0_conditional = dynamic_pointer_cast<GaussianMixture>(
139 (*expectedHybridBayesTree)[
X(0)]->conditional()->inner());
143 auto x1_conditional =
144 dynamic_pointer_cast<GaussianMixture>(
isam[
X(1)]->conditional()->inner());
145 auto expected_x1_conditional = dynamic_pointer_cast<GaussianMixture>(
146 (*expectedHybridBayesTree)[
X(1)]->conditional()->inner());
150 auto x2_conditional =
151 dynamic_pointer_cast<GaussianMixture>(
isam[
X(2)]->conditional()->inner());
152 auto expected_x2_conditional = dynamic_pointer_cast<GaussianMixture>(
153 (*expectedHybridBayesTree)[
X(2)]->conditional()->inner());
160 expectedRemainingGraph->BaseEliminateable::eliminateMultifrontal(
164 m00[
M(0)] = 0, m00[
M(1)] = 0;
166 *(*discreteBayesTree)[
M(1)]->conditional()->asDiscrete();
167 double m00_prob = decisionTree(m00);
169 auto discreteConditional =
isam[
M(1)]->conditional()->asDiscrete();
174 assignment[
M(0)] = 0;
175 assignment[
M(1)] = 0;
177 assignment[
M(0)] = 1;
178 assignment[
M(1)] = 0;
180 assignment[
M(0)] = 0;
181 assignment[
M(1)] = 1;
183 assignment[
M(0)] = 1;
184 assignment[
M(1)] = 1;
189 auto expectedChordal =
190 expectedRemainingGraph->BaseEliminateable::eliminateMultifrontal();
191 auto actualConditional = dynamic_pointer_cast<DecisionTreeFactor>(
192 isam[
M(1)]->conditional()->inner());
195 vector<double> probs = {0.095292197, 0.31417524, 0.28275772, 0.30777485};
196 auto expectedConditional =
197 std::make_shared<DecisionTreeFactor>(discrete_keys, probs);
203 TEST(HybridGaussianElimination, Approx_inference) {
209 for (
size_t i = 1;
i < 4;
i++) {
216 for (
size_t i = 4;
i <= 7;
i++) {
222 for (
size_t j = 0;
j < 4;
j++) {
227 const auto [unprunedHybridBayesTree, unprunedRemainingGraph] =
230 size_t maxNrLeaves = 5;
231 incrementalHybrid.
update(graph1);
233 incrementalHybrid.
prune(maxNrLeaves);
269 auto discreteConditional_m0 = *dynamic_pointer_cast<DiscreteConditional>(
270 incrementalHybrid[
M(0)]->conditional()->inner());
274 auto count = [](
const double &
value,
int count) {
275 return value > 0 ? count + 1 : count;
282 auto &unprunedLastDensity = *dynamic_pointer_cast<GaussianMixture>(
283 unprunedHybridBayesTree->clique(
X(3))->conditional()->inner());
284 auto &lastDensity = *dynamic_pointer_cast<GaussianMixture>(
285 incrementalHybrid[
X(3)]->conditional()->inner());
287 std::vector<std::pair<DiscreteValues, double>> assignments =
288 discreteConditional_m0.enumerate();
290 for (
auto &&av : assignments) {
292 const double value = av.second;
295 EXPECT(lastDensity(assignment) ==
nullptr);
297 CHECK(lastDensity(assignment));
299 *lastDensity(assignment)));
306 TEST(HybridGaussianElimination, Incremental_approximate) {
313 for (
size_t i = 1;
i < 4;
i++) {
320 for (
size_t i = 5;
i <= 7;
i++) {
325 size_t maxComponents = 5;
326 incrementalHybrid.
update(graph1);
327 incrementalHybrid.
prune(maxComponents);
333 2, incrementalHybrid[
X(0)]->conditional()->asMixture()->nrComponents());
335 3, incrementalHybrid[
X(1)]->conditional()->asMixture()->nrComponents());
337 5, incrementalHybrid[
X(2)]->conditional()->asMixture()->nrComponents());
339 5, incrementalHybrid[
X(3)]->conditional()->asMixture()->nrComponents());
348 incrementalHybrid.
prune(maxComponents);
354 5, incrementalHybrid[
X(3)]->conditional()->asMixture()->nrComponents());
356 5, incrementalHybrid[
X(4)]->conditional()->asMixture()->nrComponents());
372 auto priorNoise = noiseModel::Diagonal::Sigmas(
377 auto poseNoise = noiseModel::Isotropic::Sigma(3, 0.1);
415 auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0);
416 auto still = std::make_shared<PlanarMotionModel>(
W(0),
W(1),
Pose2(0, 0, 0),
418 moving = std::make_shared<PlanarMotionModel>(
W(0),
W(1),
odometry,
420 std::vector<PlanarMotionModel::shared_ptr> components = {moving, still};
421 auto mixtureFactor = std::make_shared<MixtureFactor>(
455 contKeys = {
W(1),
W(2)};
456 still = std::make_shared<PlanarMotionModel>(
W(1),
W(2),
Pose2(0, 0, 0),
459 std::make_shared<PlanarMotionModel>(
W(1),
W(2),
odometry, noise_model);
460 components = {moving, still};
461 mixtureFactor = std::make_shared<MixtureFactor>(
498 contKeys = {
W(2),
W(3)};
499 still = std::make_shared<PlanarMotionModel>(
W(2),
W(3),
Pose2(0, 0, 0),
502 std::make_shared<PlanarMotionModel>(
W(2),
W(3),
odometry, noise_model);
503 components = {moving, still};
504 mixtureFactor = std::make_shared<MixtureFactor>(
533 auto discreteTree = inc[
M(3)]->conditional()->asDiscrete();
542 expected_assignment[
M(1)] = 1;
543 expected_assignment[
M(2)] = 1;
544 expected_assignment[
M(3)] = 1;
550 auto lastConditional = inc[
X(3)]->conditional()->asMixture();