33 using namespace gtsam;
42 GaussianMixtureFactor::const_iterator const_it = factor.
begin();
44 GaussianMixtureFactor::iterator it = factor.
begin();
53 auto A1 = Matrix::Zero(2, 1);
54 auto A2 = Matrix::Zero(2, 2);
55 auto A3 = Matrix::Zero(2, 3);
56 auto b = Matrix::Zero(2, 1);
59 auto model = noiseModel::Diagonal::Sigmas(
sigmas,
true);
61 auto f10 = std::make_shared<JacobianFactor>(
X(1),
A1,
X(2),
A2,
b);
62 auto f11 = std::make_shared<JacobianFactor>(
X(1),
A1,
X(2),
A2,
b);
63 auto f20 = std::make_shared<JacobianFactor>(
X(1),
A1,
X(3),
A3,
b);
64 auto f21 = std::make_shared<JacobianFactor>(
X(1),
A1,
X(3),
A3,
b);
65 auto f22 = std::make_shared<JacobianFactor>(
X(1),
A1,
X(3),
A3,
b);
66 std::vector<GaussianFactor::shared_ptr> factorsA{f10, f11};
67 std::vector<GaussianFactor::shared_ptr> factorsB{f20, f21, f22};
84 sum += mixtureFactorA;
85 sum += mixtureFactorB;
91 auto actual = sum(
mode);
92 EXPECT(actual.at(0) == f11);
93 EXPECT(actual.at(1) == f22);
99 auto A1 = Matrix::Zero(2, 1);
100 auto A2 = Matrix::Zero(2, 2);
101 auto b = Matrix::Zero(2, 1);
102 auto f10 = std::make_shared<JacobianFactor>(
X(1),
A1,
X(2),
A2,
b);
103 auto f11 = std::make_shared<JacobianFactor>(
X(1),
A1,
X(2),
A2,
b);
104 std::vector<GaussianFactor::shared_ptr>
factors{f10, f11};
109 R
"(Hybrid [x1 x2; 1]{
143 keys.push_back(
X(0));
144 keys.push_back(
X(1));
147 dKeys.emplace_back(
M(0), 2);
148 dKeys.emplace_back(
M(1), 2);
150 auto gaussians = std::make_shared<GaussianConditional>();
162 auto A01 = Matrix2::Identity();
163 auto A02 = Matrix2::Identity();
165 auto A11 = Matrix2::Identity();
166 auto A12 = Matrix2::Identity() * 2;
168 auto b = Vector2::Zero();
170 auto f0 = std::make_shared<JacobianFactor>(
X(1), A01,
X(2), A02,
b);
171 auto f1 = std::make_shared<JacobianFactor>(
X(1),
A11,
X(2),
A12,
b);
172 std::vector<GaussianFactor::shared_ptr>
factors{f0,
f1};
178 continuousValues.insert(
X(2),
Vector2(1, 1));
183 std::vector<DiscreteKey> discrete_keys = {
m1};
185 std::vector<double> errors = {1, 4};
192 discreteValues[
m1.first] = 1;
194 4.0, mixtureFactor.error({continuousValues, discreteValues}),