Go to the documentation of this file.
42 using namespace gtsam;
50 auto measurement_model = noiseModel::Isotropic::Sigma(1,
sigma);
52 -I_1x1, measurement_model);
57 double mu0,
double mu1,
double sigma0,
double sigma1) {
58 std::vector<std::pair<Vector, double>> motionModels{{
Vector1(mu0), sigma0},
60 return std::make_shared<HybridGaussianConditional>(
m1,
X(1), I_1x1,
X(0),
67 bool add_second_measurement =
false) {
74 if (add_second_measurement) {
95 q.push_back(hybridMotionModel);
97 X(0), given.
at(
Z(0)), 3.0));
101 double w0 = 0.0, w1 = 0.0;
102 std::mt19937_64
rng(42);
103 for (
int i = 0;
i <
N;
i++) {
106 double weight = hbn.
evaluate(sample) /
q.evaluate(sample);
107 (sample.
atDiscrete(
M(1)) == 0) ? w0 += weight : w1 += weight;
109 double pm1 = w1 / (w0 + w1);
110 std::cout <<
"p(m0) = " << 100 * (1.0 - pm1) << std::endl;
111 std::cout <<
"p(m1) = " << 100 * pm1 << std::endl;
112 return {1.0 - pm1, pm1};
128 double mu0 = 1.0, mu1 = 3.0;
182 double mu0 = 1.0, mu1 = 3.0;
183 double sigma0 = 0.5,
sigma1 = 2.0;
210 eliminated->discretePosterior(
vv)));
217 auto p_m = eliminated->at(2)->asDiscrete();
225 given.insert(
Z(1),
z1);
246 eliminated->discretePosterior(
vv)));
258 given.insert_or_assign(
Z(1),
z1);
286 double sigma0 = 0.5,
sigma1 = 2.0;
315 auto p_m = bn->at(2)->asDiscrete();
323 given.insert(
Z(1),
z1);
350 given.insert_or_assign(
Z(1),
z1);
370 double mu0 = 0.0, mu1 = 10.0;
371 double sigma0 = 0.2,
sigma1 = 5.0;
405 double measurement_noise = 1
e-3) {
406 auto model0 = noiseModel::Isotropic::Sigma(1,
sigmas[0]);
407 auto model1 = noiseModel::Isotropic::Sigma(1,
sigmas[1]);
408 auto prior_noise = noiseModel::Isotropic::Sigma(1, measurement_noise);
411 std::make_shared<BetweenFactor<double>>(
X(0),
X(1),
means[0], model0)
414 std::make_shared<BetweenFactor<double>>(
X(0),
X(1),
means[1],
model1)
420 std::vector<GaussianFactorValuePair>
factors{{
f0, model0->negLogConstant()},
448 double x1 = 0.0,
x2 = 1.75;
452 std::vector<double>
means = {0.0, 2.0},
sigmas = {1
e-0, 1
e-0};
479 auto prior_noise = noiseModel::Isotropic::Sigma(1, 1
e-3);
523 double x1 = 1.0,
x2 = 1.0;
527 std::vector<double>
means = {0.0, 0.0},
sigmas = {1e2, 1
e-2};
static int runAllTests(TestResult &result)
std::shared_ptr< BayesNetType > eliminateSequential(OptionalOrderingType orderingType={}, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Linear Factor Graph where all factors are Gaussians.
std::shared_ptr< This > shared_ptr
A hybrid conditional in the Conditional Linear Gaussian scheme.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
double error(const HybridValues &values) const
Concept check for values that can be used in unit tests.
#define EXPECT(condition)
double evaluate(const HybridValues &values) const
Evaluate hybrid probability density for given HybridValues.
iterator insert(const std::pair< Key, Vector > &key_value)
const GaussianFactorGraph factors
static HybridGaussianConditional::shared_ptr CreateHybridMotionModel(double mu0, double mu1, double sigma0, double sigma1)
Create hybrid motion model p(x1 | x0, m1)
A Bayes net of Gaussian Conditionals indexed by discrete keys.
static const VectorValues vv
Point2Pair means(const std::vector< Point2Pair > &abPointPairs)
Calculate the two means of a set of Point2 pairs.
A set of GaussianFactors, indexed by a set of discrete keys.
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.
Provides additional testing facilities for common data structures.
static const double sigma
void addMeasurement(HybridBayesNet &hbn, Key z_key, Key x_key, double sigma)
const sharedFactor at(size_t i) const
Eigen::Matrix< double, 1, 1 > Vector1
EIGEN_DEVICE_FUNC const Scalar & q
void insert(Key j, const Vector &value)
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
AlgebraicDecisionTree< Key > discretePosterior(const VectorValues &continuousValues) const
Compute normalized posterior P(M|X=x) and return as a tree.
TEST(HybridGaussianFactorGraph, TwoStateModel)
void push_back(std::shared_ptr< HybridConditional > conditional)
Add a hybrid conditional using a shared_ptr.
Linearized Hybrid factor graph that uses type erasure.
size_t & atDiscrete(Key j)
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
std::pair< Key, size_t > DiscreteKey
Implementation of a discrete-conditioned hybrid factor. Implements a joint discrete-continuous factor...
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
static const HybridValues hv0
The matrix class, also used for vectors and row-vectors.
static HybridGaussianFactorGraph 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....
std::shared_ptr< HybridBayesNet > shared_ptr
AlgebraicDecisionTree< Key > discretePosterior(const VectorValues &continuousValues) const
Computer posterior P(M|X=x) when all continuous values X are given. This is efficient as this simply ...
HybridGaussianFactorGraph toFactorGraph(const VectorValues &measurements) const
noiseModel::Isotropic::shared_ptr model1
std::uint64_t Key
Integer nonlinear key type.
static const HybridValues hv1
HybridBayesNet CreateBayesNet(const HybridGaussianConditional::shared_ptr &hybridMotionModel, bool add_second_measurement=false)
Create two state Bayes network with 1 or two measurement models.
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
void emplace_shared(Args &&...args)
Matrix< RealScalar, Dynamic, Dynamic > M
gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:07:38