testHybridMotionModel.cpp File Reference

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.

Namespaces
	test_direct_factor_graph

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 HybridGaussianFactorGraph	test_direct_factor_graph::CreateFactorGraph (const gtsam::Values &values, const std::vector< double > &means, const std::vector< double > &sigmas, DiscreteKey &m1, double measurement_noise=1e-3)
	Create a Factor Graph by directly specifying all the factors instead of creating conditionals first. This way we can directly provide the likelihoods and then perform linearization. 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 (HybridGaussianFactorGraph, DifferentCovariances)
	Test components with differing covariances but the same means. The factor graph is -X1--X2 | M1. More...
	TEST (HybridGaussianFactorGraph, DifferentMeans)
	Test components with differing means but the same covariances. The factor graph is -X1--X2 | M1. More...
	TEST (HybridGaussianFactorGraph, TwoStateModel)
	TEST (HybridGaussianFactorGraph, TwoStateModel2)
	TEST (HybridGaussianFactorGraph, TwoStateModel3)
	TEST (HybridGaussianFactorGraph, 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 547 of file testHybridMotionModel.cpp.

TEST() [1/6]

TEST	(	HybridGaussianFactorGraph	,
		DifferentCovariances
	)

Test components with differing covariances but the same means. The factor graph is -X1--X2 | M1.

Definition at line 517 of file testHybridMotionModel.cpp.

TEST() [2/6]

TEST	(	HybridGaussianFactorGraph	,
		DifferentMeans
	)

Test components with differing means but the same covariances. The factor graph is -X1--X2 | M1.

Definition at line 442 of file testHybridMotionModel.cpp.

TEST() [3/6]

TEST	(	HybridGaussianFactorGraph	,
		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() [4/6]

TEST	(	HybridGaussianFactorGraph	,
		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() [5/6]

TEST	(	HybridGaussianFactorGraph	,
		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() [6/6]

TEST	(	HybridGaussianFactorGraph	,
		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.