Tests hybrid inference with a simple switching motion model. More...

#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridGaussianConditional.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridValues.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <CppUnitLite/TestHarness.h>
#include <memory>

Include dependency graph for testHybridMotionModel.cpp:

Go to the source code of this file.

Functions
void	addMeasurement (HybridBayesNet &hbn, Key z_key, Key x_key, double sigma)

std::pair< double, double >	approximateDiscreteMarginal (const HybridBayesNet &hbn, const HybridGaussianConditional::shared_ptr &hybridMotionModel, const VectorValues &given, size_t N=100000)
	Approximate the discrete marginal P(m1) using importance sampling. More...

HybridBayesNet	CreateBayesNet (const HybridGaussianConditional::shared_ptr &hybridMotionModel, bool add_second_measurement=false)
	Create two state Bayes network with 1 or two measurement models. More...

static HybridGaussianConditional::shared_ptr	CreateHybridMotionModel (double mu0, double mu1, double sigma0, double sigma1)
	Create hybrid motion model p(x1 \| x0, m1) More...

DiscreteKey	m1 (M(1), 2)

int	main ()

	TEST (HybridGaussianFactor, TwoStateModel)

	TEST (HybridGaussianFactor, TwoStateModel2)

	TEST (HybridGaussianFactor, TwoStateModel3)

	TEST (HybridGaussianFactor, TwoStateModel4)

Detailed Description

Tests hybrid inference with a simple switching motion model.

Author: Varun Agrawal; Fan Jiang; Frank Dellaert

Date: December 2021

Definition in file testHybridMotionModel.cpp.

Function Documentation

◆ addMeasurement()

void addMeasurement	(	HybridBayesNet &	hbn,
		Key	z_key,
		Key	x_key,
		double	sigma
	)

Definition at line 49 of file testHybridMotionModel.cpp.

◆ approximateDiscreteMarginal()

std::pair<double, double> approximateDiscreteMarginal	(	const HybridBayesNet &	hbn,
		const HybridGaussianConditional::shared_ptr &	hybridMotionModel,
		const VectorValues &	given,
		size_t	N = `100000`
	)

Approximate the discrete marginal P(m1) using importance sampling.

Create importance sampling network q(x0,x1,m) = p(x1|x0,m1) q(x0) P(m1), using q(x0) = N(z0, sigmaQ) to sample x0.

Definition at line 88 of file testHybridMotionModel.cpp.

◆ CreateBayesNet()

HybridBayesNet CreateBayesNet	(	const HybridGaussianConditional::shared_ptr &	hybridMotionModel,
		bool	add_second_measurement = `false`
	)

Create two state Bayes network with 1 or two measurement models.

Definition at line 65 of file testHybridMotionModel.cpp.

◆ CreateHybridMotionModel()

static HybridGaussianConditional::shared_ptr CreateHybridMotionModel	(	double	mu0,
		double	mu1,
		double	sigma0,
		double	sigma1
	)

static

Create hybrid motion model p(x1 | x0, m1)

Definition at line 56 of file testHybridMotionModel.cpp.

◆ m1()

DiscreteKey m1	(	M(1)	,
		2
	)

◆ main()

int main ( )

Definition at line 389 of file testHybridMotionModel.cpp.

◆ TEST() [1/4]

TEST	(	HybridGaussianFactor	,
		TwoStateModel
	)

Test a model p(z0|x0)p(z1|x1)p(x1|x0,m1)P(m1).

p(x1|x0,m1) has mode-dependent mean but same covariance.

Converting to a factor graph gives us ϕ(x0;z0)ϕ(x1;z1)ϕ(x1,x0,m1)P(m1)

If we only have a measurement on x0, then the posterior probability of m1 should be 0.5/0.5. Getting a measurement on z1 gives use more information.

Definition at line 127 of file testHybridMotionModel.cpp.

◆ TEST() [2/4]

TEST	(	HybridGaussianFactor	,
		TwoStateModel2
	)

Test a model P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1).

P(x1|x0,m1) has different means and different covariances.

Converting to a factor graph gives us ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)P(m1)

If we only have a measurement on z0, then the P(m1) should be 0.5/0.5. Getting a measurement on z1 gives use more information.

Definition at line 181 of file testHybridMotionModel.cpp.

◆ TEST() [3/4]

TEST	(	HybridGaussianFactor	,
		TwoStateModel3
	)

Test a model p(z0|x0)p(x1|x0,m1)p(z1|x1)p(m1).

p(x1|x0,m1) has the same means but different covariances.

Converting to a factor graph gives us ϕ(x0)ϕ(x1,x0,m1)ϕ(x1)p(m1)

If we only have a measurement on z0, then the p(m1) should be 0.5/0.5. Getting a measurement on z1 gives use more information.

Definition at line 284 of file testHybridMotionModel.cpp.

◆ TEST() [4/4]

TEST	(	HybridGaussianFactor	,
		TwoStateModel4
	)

Same model, P(z0|x0)P(x1|x0,m1)P(z1|x1)P(m1), but now with very informative measurements and vastly different motion model: either stand still or move far. This yields a very informative posterior.

Definition at line 369 of file testHybridMotionModel.cpp.

Functions