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, Full)
	TEST (HybridEstimation, IncrementalSmoother)
	TEST (HybridEstimation, Probability)
	TEST (HybridEstimation, ProbabilityMultifrontal)
	TEST (HybridEstimation, eliminateSequentialRegression)
	TEST (HybridEstimation, CorrectnessViaSampling)
	TEST (HybridEstimation, ModeSelection)
	TEST (HybridEstimation, ModeSelection2)

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 394 of file testHybridEstimation.cpp.

createHybridNonlinearFactorGraph()

static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph ( )

static

Definition at line 368 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 192 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( | 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 212 of file testHybridEstimation.cpp.

main()

int main

(

void

)

Definition at line 649 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 480 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 155 of file testHybridEstimation.cpp.

TEST() [1/8]

TEST	(	HybridEstimation	,
		Full
	)

Definition at line 49 of file testHybridEstimation.cpp.

TEST() [2/8]

TEST	(	HybridEstimation	,
		IncrementalSmoother
	)

Definition at line 91 of file testHybridEstimation.cpp.

TEST() [3/8]

TEST	(	HybridEstimation	,
		Probability
	)

Definition at line 257 of file testHybridEstimation.cpp.

TEST() [4/8]

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 300 of file testHybridEstimation.cpp.

TEST() [5/8]

TEST	(	HybridEstimation	,
		eliminateSequentialRegression
	)

Definition at line 407 of file testHybridEstimation.cpp.

TEST() [6/8]

TEST	(	HybridEstimation	,
		CorrectnessViaSampling
	)

Definition at line 442 of file testHybridEstimation.cpp.

TEST() [7/8]

TEST	(	HybridEstimation	,
		ModeSelection
	)

Definition at line 516 of file testHybridEstimation.cpp.

TEST() [8/8]

TEST	(	HybridEstimation	,
		ModeSelection2
	)

Definition at line 582 of file testHybridEstimation.cpp.