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/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridNonlinearFactor.h>
#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
#include <gtsam/hybrid/HybridNonlinearISAM.h>
#include <gtsam/hybrid/HybridSmoother.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"
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 () |
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) | |
Unit tests for end-to-end Hybrid Estimation.
Definition in file testHybridEstimation.cpp.
|
static |
Definition at line 449 of file testHybridEstimation.cpp.
|
static |
Definition at line 423 of file testHybridEstimation.cpp.
Get the discrete sequence from the integer x
.
K | Template parameter so we can set the correct bitset size. |
x | The integer to convert to a discrete binary sequence. |
Definition at line 248 of file testHybridEstimation.cpp.
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).
graph | The HybridGaussianFactorGraph to eliminate. |
Definition at line 268 of file testHybridEstimation.cpp.
int main | ( | ) |
Definition at line 650 of file testHybridEstimation.cpp.
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.
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. |
Definition at line 211 of file testHybridEstimation.cpp.
TEST | ( | HybridEstimation | , |
CorrectnessViaSampling | |||
) |
Definition at line 497 of file testHybridEstimation.cpp.
TEST | ( | HybridEstimation | , |
eliminateSequentialRegression | |||
) |
Definition at line 462 of file testHybridEstimation.cpp.
TEST | ( | HybridEstimation | , |
Full | |||
) |
Definition at line 50 of file testHybridEstimation.cpp.
TEST | ( | HybridEstimation | , |
IncrementalSmoother | |||
) |
Definition at line 92 of file testHybridEstimation.cpp.
TEST | ( | HybridEstimation | , |
ISAM | |||
) |
Definition at line 146 of file testHybridEstimation.cpp.
TEST | ( | HybridEstimation | , |
ModeSelection | |||
) |
Definition at line 531 of file testHybridEstimation.cpp.
TEST | ( | HybridEstimation | , |
ModeSelection2 | |||
) |
Definition at line 590 of file testHybridEstimation.cpp.
TEST | ( | HybridEstimation | , |
Probability | |||
) |
Definition at line 313 of file testHybridEstimation.cpp.
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.