52 size_t n, std::function<
Key(
int)> keyFunc =
X,
53 std::function<
Key(
int)> dKeyFunc =
M) {
59 for (
size_t t = 1;
t <
n;
t++) {
61 {keyFunc(
t), keyFunc(
t + 1)}, {{dKeyFunc(
t), 2}},
62 {std::make_shared<JacobianFactor>(keyFunc(
t), I_3x3, keyFunc(
t + 1),
64 std::make_shared<JacobianFactor>(keyFunc(
t), I_3x3, keyFunc(
t + 1),
65 I_3x3, Vector3::Ones())}));
73 return std::make_shared<HybridGaussianFactorGraph>(std::move(hfg));
86 std::vector<Key> &input) {
89 std::vector<int> levels(input.size());
90 std::function<void(std::vector<Key>::iterator, std::vector<Key>::iterator,
92 bsg = [&bsg, &new_order, &levels, &input](
93 std::vector<Key>::iterator begin,
94 std::vector<Key>::iterator
end,
int lvl) {
96 std::vector<Key>::iterator pivot =
99 new_order.push_back(*pivot);
101 bsg(begin, pivot, lvl + 1);
102 bsg(pivot + 1,
end, lvl + 1);
104 new_order.push_back(*begin);
109 bsg(input.begin(), input.end(), 0);
112 return {new_order, levels};
135 Switching(
size_t K,
double between_sigma = 1.0,
double prior_sigma = 0.1,
136 std::vector<double> measurements = {},
137 std::string discrete_transition_prob =
"1/2 3/2")
142 for (
size_t k = 0; k <
K; k++) {
143 modes.emplace_back(
M(k), 2);
147 if (measurements.size() == 0) {
148 for (
size_t k = 0; k <
K; k++) {
149 measurements.push_back(k);
156 X(0), measurements.at(0), Isotropic::Sigma(1, prior_sigma));
159 for (
size_t k = 0; k <
K - 1; k++) {
162 std::vector<NonlinearFactor::shared_ptr> components;
163 for (
auto &&
f : motion_models) {
164 components.push_back(std::dynamic_pointer_cast<NonlinearFactor>(
f));
167 keys, DiscreteKeys{
modes[k]}, components);
171 auto measurement_noise = Isotropic::Sigma(1, prior_sigma);
172 for (
size_t k = 1; k <
K; k++) {
174 X(k), measurements.at(k), measurement_noise);
181 for (
size_t k = 0; k <
K; k++) {
192 double sigma = 1.0) {
195 std::make_shared<MotionModel>(
X(k),
X(k + 1), 0.0, noise_model),
197 std::make_shared<MotionModel>(
X(k),
X(k + 1), 1.0, noise_model);
198 return {still, moving};
207 template <
typename FACTORGRAPH>
209 std::string discrete_transition_prob =
"1/2 3/2") {
210 fg->template emplace_shared<DiscreteDistribution>(
modes[0],
"1/1");
211 for (
size_t k = 0; k <
K - 2; k++) {
212 auto parents = {
modes[k]};
213 fg->template emplace_shared<DiscreteConditional>(
214 modes[k + 1], parents, discrete_transition_prob);