testHybridEstimation.cpp File Reference

Unit tests for end-to-end Hybrid Estimation. More...

#include <gtsam/discrete/DiscreteBayesNet.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearISAM.h>
#include <gtsam/hybrid/HybridSmoother.h>
#include <gtsam/hybrid/MixtureFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <CppUnitLite/TestHarness.h>
#include <bitset>
#include "Switching.h"

Include dependency graph for testHybridEstimation.cpp:

Go to the source code of this file.

Functions
static HybridGaussianFactorGraph::shared_ptr	createHybridGaussianFactorGraph ()
static HybridNonlinearFactorGraph	createHybridNonlinearFactorGraph ()
template<size_t K>
std::vector< size_t >	getDiscreteSequence (size_t x)
	Get the discrete sequence from the integer x. More...
AlgebraicDecisionTree< Key >	getProbPrimeTree (const HybridGaussianFactorGraph &graph)
	Helper method to get the tree of unnormalized probabilities as per the elimination scheme. More...
int	main ()
std::shared_ptr< GaussianMixtureFactor >	mixedVarianceFactor (const MixtureFactor &mf, const Values &initial, const Key &mode, double noise_tight, double noise_loose, size_t d, size_t tight_index)
GaussianFactorGraph::shared_ptr	specificModesFactorGraph (size_t K, const std::vector< double > &measurements, const std::vector< size_t > &discrete_seq, double measurement_sigma=0.1, double between_sigma=1.0)
	A function to get a specific 1D robot motion problem as a linearized factor graph. This is the problem P(X|Z, M), i.e. estimating the continuous positions given the measurements and discrete sequence. More...
	TEST (HybridEstimation, CorrectnessViaSampling)
	TEST (HybridEstimation, eliminateSequentialRegression)
	TEST (HybridEstimation, Full)
	TEST (HybridEstimation, IncrementalSmoother)
	TEST (HybridEstimation, ISAM)
	TEST (HybridEstimation, ModeSelection)
	TEST (HybridEstimation, ModeSelection2)
	TEST (HybridEstimation, Probability)
	TEST (HybridEstimation, ProbabilityMultifrontal)

Detailed Description

Unit tests for end-to-end Hybrid Estimation.

Author

Varun Agrawal

Definition in file testHybridEstimation.cpp.

Function Documentation

createHybridGaussianFactorGraph()

static HybridGaussianFactorGraph::shared_ptr createHybridGaussianFactorGraph ( )

static

Definition at line 449 of file testHybridEstimation.cpp.

createHybridNonlinearFactorGraph()

static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph ( )

static

Definition at line 423 of file testHybridEstimation.cpp.

getDiscreteSequence()

template<size_t K>

std::vector<size_t> getDiscreteSequence

(

size_t

x

)

Get the discrete sequence from the integer x.

Template Parameters

K	Template parameter so we can set the correct bitset size.

Parameters

x	The integer to convert to a discrete binary sequence.

Returns

std::vector<size_t>

Definition at line 247 of file testHybridEstimation.cpp.

getProbPrimeTree()

AlgebraicDecisionTree<Key> getProbPrimeTree

(

const HybridGaussianFactorGraph &

graph

)

Helper method to get the tree of unnormalized probabilities as per the elimination scheme.

Used as a helper to compute q(\mu | M, Z) which is used by both P(X | M, Z) and P(M | Z).

Parameters

graph

The HybridGaussianFactorGraph to eliminate.

Returns

AlgebraicDecisionTree<Key>

Definition at line 267 of file testHybridEstimation.cpp.

main()

int main

(

)

Definition at line 704 of file testHybridEstimation.cpp.

mixedVarianceFactor()

std::shared_ptr<GaussianMixtureFactor> mixedVarianceFactor	(	const MixtureFactor &	mf,
		const Values &	initial,
		const Key &	mode,
		double	noise_tight,
		double	noise_loose,
		size_t	d,
		size_t	tight_index
	)

Helper function to add the constant term corresponding to the difference in noise models.

Definition at line 535 of file testHybridEstimation.cpp.

specificModesFactorGraph()

GaussianFactorGraph::shared_ptr specificModesFactorGraph	(	size_t	K,
		const std::vector< double > &	measurements,
		const std::vector< size_t > &	discrete_seq,
		double	measurement_sigma = 0.1,
		double	between_sigma = 1.0
	)

A function to get a specific 1D robot motion problem as a linearized factor graph. This is the problem P(X|Z, M), i.e. estimating the continuous positions given the measurements and discrete sequence.

Parameters

K	The number of timesteps.
measurements	The vector of measurements for each timestep.
discrete_seq	The discrete sequence governing the motion of the robot.
measurement_sigma	Noise model sigma for measurements.
between_sigma	Noise model sigma for the between factor.

Returns

GaussianFactorGraph::shared_ptr

Definition at line 210 of file testHybridEstimation.cpp.

TEST() [1/9]

TEST	(	HybridEstimation	,
		CorrectnessViaSampling
	)

Definition at line 497 of file testHybridEstimation.cpp.

TEST() [2/9]

TEST	(	HybridEstimation	,
		eliminateSequentialRegression
	)

Definition at line 462 of file testHybridEstimation.cpp.

TEST() [3/9]

TEST	(	HybridEstimation	,
		Full
	)

Definition at line 49 of file testHybridEstimation.cpp.

TEST() [4/9]

TEST	(	HybridEstimation	,
		IncrementalSmoother
	)

Definition at line 91 of file testHybridEstimation.cpp.

TEST() [5/9]

TEST	(	HybridEstimation	,
		ISAM
	)

Definition at line 145 of file testHybridEstimation.cpp.

TEST() [6/9]

TEST	(	HybridEstimation	,
		ModeSelection
	)

Definition at line 571 of file testHybridEstimation.cpp.

TEST() [7/9]

TEST	(	HybridEstimation	,
		ModeSelection2
	)

Definition at line 637 of file testHybridEstimation.cpp.

TEST() [8/9]

TEST	(	HybridEstimation	,
		Probability
	)

Definition at line 312 of file testHybridEstimation.cpp.

TEST() [9/9]

TEST	(	HybridEstimation	,
		ProbabilityMultifrontal
	)

Test for correctness of different branches of the P'(Continuous | Discrete) in the multi-frontal setting. The values should match those of P'(Continuous) for each discrete mode.

Definition at line 355 of file testHybridEstimation.cpp.