testHybridGaussianFactor.cpp File Reference

Unit tests for HybridGaussianFactor. 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 testHybridGaussianFactor.cpp:

Go to the source code of this file.

Namespaces
	test_constructor
	test_direct_factor_graph
	test_gmm
	test_two_state_estimation

Functions
void	test_two_state_estimation::addMeasurement (HybridBayesNet &hbn, Key z_key, Key x_key, double sigma)
std::pair< double, double >	test_two_state_estimation::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	test_two_state_estimation::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	test_two_state_estimation::CreateHybridMotionModel (double mu0, double mu1, double sigma0, double sigma1)
	Create hybrid motion model p(x1 | x0, m1) More...
static HybridBayesNet	test_gmm::GetGaussianMixtureModel (double mu0, double mu1, double sigma0, double sigma1)
DiscreteKey	test_constructor::m1 (1, 2)
DiscreteKey	test_two_state_estimation::m1 (M(1), 2)
int	main ()
double	test_gmm::prob_m_z (double mu0, double mu1, double sigma0, double sigma1, double z)
	TEST (HybridGaussianFactor, Constructor)
	TEST (HybridGaussianFactor, ConstructorVariants)
	TEST (HybridGaussianFactor, DifferentCovariancesFG)
	Test components with differing covariances but the same means. The factor graph is -X1--X2 | M1. More...
	TEST (HybridGaussianFactor, DifferentMeansFG)
	Test components with differing means but the same covariances. The factor graph is -X1--X2 | M1. More...
	TEST (HybridGaussianFactor, Error)
	TEST (HybridGaussianFactor, GaussianMixtureModel)
	TEST (HybridGaussianFactor, GaussianMixtureModel2)
	TEST (HybridGaussianFactor, HybridGaussianConditional)
	TEST (HybridGaussianFactor, Printing)
	TEST (HybridGaussianFactor, Sum)
	TEST (HybridGaussianFactor, TwoStateModel)
	TEST (HybridGaussianFactor, TwoStateModel2)
	TEST (HybridGaussianFactor, TwoStateModel3)
	TEST (HybridGaussianFactor, TwoStateModel4)

Variables
auto	test_constructor::A1 = Matrix::Zero(2, 1)
auto	test_constructor::A2 = Matrix::Zero(2, 2)
auto	test_constructor::b = Matrix::Zero(2, 1)
auto	test_constructor::f10 = std::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b)
auto	test_constructor::f11 = std::make_shared<JacobianFactor>(X(1), A1, X(2), A2, b)

Detailed Description

Unit tests for HybridGaussianFactor.

Author

Varun Agrawal

Fan Jiang

Frank Dellaert

Date

December 2021

Definition in file testHybridGaussianFactor.cpp.

Function Documentation

main()

int main

(

)

Definition at line 889 of file testHybridGaussianFactor.cpp.

TEST() [1/14]

TEST	(	HybridGaussianFactor	,
		Constructor
	)

Definition at line 49 of file testHybridGaussianFactor.cpp.

TEST() [2/14]

TEST	(	HybridGaussianFactor	,
		ConstructorVariants
	)

Definition at line 71 of file testHybridGaussianFactor.cpp.

TEST() [3/14]

TEST	(	HybridGaussianFactor	,
		DifferentCovariancesFG
	)

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

Definition at line 852 of file testHybridGaussianFactor.cpp.

TEST() [4/14]

TEST	(	HybridGaussianFactor	,
		DifferentMeansFG
	)

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

Definition at line 777 of file testHybridGaussianFactor.cpp.

TEST() [5/14]

TEST	(	HybridGaussianFactor	,
		Error
	)

Definition at line 151 of file testHybridGaussianFactor.cpp.

TEST() [6/14]

TEST	(	HybridGaussianFactor	,
		GaussianMixtureModel
	)

Test a simple Gaussian Mixture Model represented as P(m)P(z|m) where m is a discrete variable and z is a continuous variable. m is binary and depending on m, we have 2 different means μ1 and μ2 for the Gaussian distribution around which we sample z.

The resulting factor graph should eliminate to a Bayes net which represents a sigmoid function.

Definition at line 240 of file testHybridGaussianFactor.cpp.

TEST() [7/14]

TEST	(	HybridGaussianFactor	,
		GaussianMixtureModel2
	)

Test a simple Gaussian Mixture Model represented as P(m)P(z|m) where m is a discrete variable and z is a continuous variable. m is binary and depending on m, we have 2 different means and covariances each for the Gaussian distribution around which we sample z.

The resulting factor graph should eliminate to a Bayes net which represents a Gaussian-like function where m1>m0 close to 3.1333.

Definition at line 312 of file testHybridGaussianFactor.cpp.

TEST() [8/14]

TEST	(	HybridGaussianFactor	,
		HybridGaussianConditional
	)

Definition at line 133 of file testHybridGaussianFactor.cpp.

TEST() [9/14]

TEST	(	HybridGaussianFactor	,
		Printing
	)

Definition at line 123 of file testHybridGaussianFactor.cpp.

TEST() [10/14]

TEST	(	HybridGaussianFactor	,
		Sum
	)

Definition at line 86 of file testHybridGaussianFactor.cpp.

TEST() [11/14]

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 466 of file testHybridGaussianFactor.cpp.

TEST() [12/14]

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 522 of file testHybridGaussianFactor.cpp.

TEST() [13/14]

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 616 of file testHybridGaussianFactor.cpp.

TEST() [14/14]

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 703 of file testHybridGaussianFactor.cpp.