42 using namespace gtsam;
51 HybridGaussianFactor::const_iterator const_it = factor.
begin();
53 HybridGaussianFactor::iterator it = factor.
begin();
61 auto A1 = Matrix::Zero(2, 1);
62 auto A2 = Matrix::Zero(2, 2);
63 auto b = Matrix::Zero(2, 1);
65 auto f10 = std::make_shared<JacobianFactor>(
X(1),
A1,
X(2),
A2,
b);
66 auto f11 = std::make_shared<JacobianFactor>(
X(1),
A1,
X(2),
A2,
b);
75 std::vector<GaussianFactorValuePair> pairs{{
f10, 0.0}, {
f11, 0.0}};
90 auto A3 = Matrix::Zero(2, 3);
91 auto f20 = std::make_shared<JacobianFactor>(
X(1),
A1,
X(3),
A3,
b);
92 auto f21 = std::make_shared<JacobianFactor>(
X(1),
A1,
X(3),
A3,
b);
93 auto f22 = std::make_shared<JacobianFactor>(
X(1),
A1,
X(3),
A3,
b);
109 sum += hybridFactorA;
110 sum += hybridFactorB;
116 auto actual = sum(
mode);
118 EXPECT(actual.at(1) == f22);
127 R
"(HybridGaussianFactor
162 dKeys.emplace_back(
M(0), 2);
163 dKeys.emplace_back(
M(1), 2);
165 auto gaussians = std::make_shared<GaussianConditional>();
177 auto A01 = Matrix2::Identity();
178 auto A02 = Matrix2::Identity();
180 auto A11 = Matrix2::Identity();
181 auto A12 = Matrix2::Identity() * 2;
183 auto b = Vector2::Zero();
185 auto f0 = std::make_shared<JacobianFactor>(
X(1), A01,
X(2), A02,
b);
186 auto f1 = std::make_shared<JacobianFactor>(
X(1),
A11,
X(2),
A12,
b);
191 continuousValues.insert(
X(2),
Vector2(1, 1));
195 hybridFactor.errorTree(continuousValues);
197 std::vector<DiscreteKey> discrete_keys = {
m1};
199 std::vector<double> errors = {1, 4};
206 discreteValues[
m1.first] = 1;
208 4.0, hybridFactor.error({continuousValues, discreteValues}), 1
e-9);
228 double measurement_noise = 1
e-3) {
229 auto model0 = noiseModel::Isotropic::Sigma(1,
sigmas[0]);
230 auto model1 = noiseModel::Isotropic::Sigma(1,
sigmas[1]);
231 auto prior_noise = noiseModel::Isotropic::Sigma(1, measurement_noise);
234 std::make_shared<BetweenFactor<double>>(
X(0),
X(1),
means[0], model0)
243 std::vector<GaussianFactorValuePair>
factors{{
f0, model0->negLogConstant()},
271 double x1 = 0.0,
x2 = 1.75;
275 std::vector<double>
means = {0.0, 2.0},
sigmas = {1
e-0, 1
e-0};
302 auto prior_noise = noiseModel::Isotropic::Sigma(1, 1
e-3);
346 double x1 = 1.0,
x2 = 1.0;
350 std::vector<double>
means = {0.0, 0.0},
sigmas = {1e2, 1
e-2};