44 using namespace gtsam;
49 TEST(HybridEstimation, Full) {
51 std::vector<double> measurements = {0, 1, 2, 2, 2, 3};
53 std::vector<size_t> discrete_seq = {1, 1, 0, 0, 1};
56 Switching switching(
K, 1.0, 0.1, measurements,
"1/1 1/1");
60 for (
size_t k = 0; k <
K; k++) {
61 hybridOrdering.push_back(
X(k));
63 for (
size_t k = 0; k <
K - 1; k++) {
64 hybridOrdering.push_back(
M(k));
77 for (
size_t k = 0; k <
K - 1; k++) {
78 expected_discrete[
M(k)] = discrete_seq[k];
82 Values expected_continuous;
83 for (
size_t k = 0; k <
K; k++) {
84 expected_continuous.
insert(
X(k), measurements[k]);
91 TEST(HybridEstimation, IncrementalSmoother) {
93 std::vector<double> measurements = {0, 1, 2, 2, 2, 2, 3, 4, 5, 6, 6,
94 7, 8, 9, 9, 9, 10, 11, 11, 11, 11};
96 std::vector<size_t> discrete_seq = {1, 1, 0, 0, 0, 1, 1, 1, 1, 0,
97 1, 1, 1, 0, 0, 1, 1, 0, 0, 0};
100 Switching switching(
K, 1.0, 0.1, measurements,
"1/1 1/1");
111 for (
size_t k = 1; k <
K; k++) {
131 for (
size_t k = 0; k <
K - 1; k++) {
132 expected_discrete[
M(k)] = discrete_seq[k];
136 Values expected_continuous;
137 for (
size_t k = 0; k <
K; k++) {
138 expected_continuous.
insert(
X(k), measurements[k]);
147 std::vector<double> measurements = {0, 1, 2, 2, 2, 2, 3, 4, 5, 6, 6,
148 7, 8, 9, 9, 9, 10, 11, 11, 11, 11};
150 std::vector<size_t> discrete_seq = {1, 1, 0, 0, 0, 1, 1, 1, 1, 0,
151 1, 1, 1, 0, 0, 1, 1, 0, 0, 0};
154 Switching switching(
K, 1.0, 0.1, measurements,
"1/1 1/1");
167 for (
size_t k = 1; k <
K; k++) {
186 for (
size_t k = 0; k <
K - 1; k++) {
187 expected_discrete[
M(k)] = discrete_seq[k];
191 Values expected_continuous;
192 for (
size_t k = 0; k <
K; k++) {
193 expected_continuous.
insert(
X(k), measurements[k]);
211 size_t K,
const std::vector<double>& measurements,
212 const std::vector<size_t>& discrete_seq,
double measurement_sigma = 0.1,
213 double between_sigma = 1.0) {
215 Values linearizationPoint;
218 auto measurement_noise = noiseModel::Isotropic::Sigma(1, measurement_sigma);
219 for (
size_t k = 0; k <
K; k++) {
222 linearizationPoint.
insert<
double>(
X(k),
static_cast<double>(k + 1));
228 auto motion_noise_model = noiseModel::Isotropic::Sigma(1, between_sigma);
229 for (
size_t k = 0; k <
K - 1; k++) {
230 auto motion_model = std::make_shared<MotionModel>(
231 X(k),
X(k + 1), discrete_seq.at(k), motion_noise_model);
248 std::bitset<
K - 1>
seq =
x;
249 std::vector<size_t> discrete_seq(
K - 1);
250 for (
size_t i = 0;
i <
K - 1;
i++) {
252 discrete_seq[
K - 2 -
i] =
seq[
i];
270 const auto [bayesNet, remainingGraph] =
271 graph.eliminatePartialSequential(continuous);
273 auto last_conditional = bayesNet->
at(bayesNet->size() - 1);
274 DiscreteKeys discrete_keys = last_conditional->discreteKeys();
276 const std::vector<DiscreteValues> assignments =
277 DiscreteValues::CartesianProduct(discrete_keys);
279 std::reverse(discrete_keys.begin(), discrete_keys.end());
281 vector<VectorValues::shared_ptr> vector_values;
284 vector_values.push_back(std::make_shared<VectorValues>(
values));
290 std::vector<double> probPrimes;
296 if (
delta.size() == 0) {
297 probPrimes.push_back(0.0);
305 return probPrimeTree;
312 TEST(HybridEstimation, Probability) {
313 constexpr
size_t K = 4;
314 std::vector<double> measurements = {0, 1, 2, 2};
315 double between_sigma = 1.0, measurement_sigma = 0.1;
319 Switching switching(
K, between_sigma, measurement_sigma, measurements,
325 auto [bayesNet, discreteGraph] =
326 graph.eliminatePartialSequential(continuous_ordering);
330 auto discreteBayesNet = discreteGraph->eliminateSequential(discrete_ordering);
333 for (
auto discrete_conditional : *discreteBayesNet) {
334 bayesNet->add(discrete_conditional);
336 auto discreteConditional = discreteBayesNet->at(0)->asDiscrete();
342 discrete_seq[
M(0)] = 1;
343 discrete_seq[
M(1)] = 1;
344 discrete_seq[
M(2)] = 0;
355 TEST(HybridEstimation, ProbabilityMultifrontal) {
356 constexpr
size_t K = 4;
357 std::vector<double> measurements = {0, 1, 2, 2};
359 double between_sigma = 1.0, measurement_sigma = 0.1;
363 Switching switching(
K, between_sigma, measurement_sigma, measurements,
372 const auto [bayesTree, discreteGraph] =
373 graph.eliminatePartialMultifrontal(continuous_ordering);
376 Key last_continuous_key =
377 continuous_ordering.at(continuous_ordering.size() - 1);
378 auto last_conditional = (*bayesTree)[last_continuous_key]->conditional();
379 DiscreteKeys discrete_keys = last_conditional->discreteKeys();
382 auto discreteBayesTree = discreteGraph->eliminateMultifrontal(discrete);
386 auto discrete_clique = (*discreteBayesTree)[discrete.at(0)];
388 std::set<HybridBayesTreeClique::shared_ptr> clique_set;
389 for (
auto node : bayesTree->nodes()) {
390 clique_set.insert(node.second);
394 for (
auto clique : clique_set) {
395 if (clique->conditional()->parents() ==
396 discrete_clique->conditional()->frontals()) {
397 discreteBayesTree->addClique(clique, discrete_clique);
402 auto clique_it = std::find(clique->parent()->children.begin(),
403 clique->parent()->children.end(), clique);
404 clique->parent()->children.erase(clique_it);
405 discreteBayesTree->addClique(clique, clique->parent());
409 HybridValues hybrid_values = discreteBayesTree->optimize();
413 discrete_seq[
M(0)] = 1;
414 discrete_seq[
M(1)] = 1;
415 discrete_seq[
M(2)] = 0;
426 constexpr
double sigma = 0.5;
427 const auto noise_model = noiseModel::Isotropic::Sigma(1,
sigma);
435 const auto zero_motion =
436 std::make_shared<BetweenFactor<double>>(
X(0),
X(1), 0, noise_model);
437 const auto one_motion =
438 std::make_shared<BetweenFactor<double>>(
X(0),
X(1), 1, noise_model);
441 std::vector<NonlinearFactor::shared_ptr>{zero_motion, one_motion});
453 double z0 = 0.0,
z1 = 1.0;
462 TEST(HybridEstimation, eliminateSequentialRegression) {
475 auto dc1 = bn1->back()->asDiscrete();
483 auto dc2 = bn2->back()->asDiscrete();
497 TEST(HybridEstimation, CorrectnessViaSampling) {
505 std::mt19937_64
rng(11);
508 auto compute_ratio = [&](
const HybridValues& sample) ->
double {
509 return bn->evaluate(sample) / fg->probPrime(sample);
515 double expected_ratio = compute_ratio(sample);
520 constexpr
int num_samples = 10;
521 for (
size_t i = 0;
i < num_samples;
i++) {
537 double noise_tight,
double noise_loose,
size_t d,
size_t tight_index) {
540 constexpr
double log2pi = 1.8378770664093454835606594728112;
542 double logNormalizationConstant =
log(1.0 / noise_tight);
543 double logConstant = -0.5 *
d * log2pi + logNormalizationConstant;
547 if (assignment.at(
mode) != tight_index) {
548 double factor_log_constant = -0.5 *
d * log2pi +
log(1.0 / noise_loose);
553 for (
size_t i = 0;
i <
d;
i++) {
554 c(
i) =
std::sqrt(2.0 * (logConstant - factor_log_constant));
558 return std::make_shared<JacobianFactor>(_gfg);
560 return dynamic_pointer_cast<JacobianFactor>(gf);
563 auto updated_components = gmf->factors().apply(
func);
564 auto updated_gmf = std::make_shared<GaussianMixtureFactor>(
565 gmf->continuousKeys(), gmf->discreteKeys(), updated_components);
571 TEST(HybridEstimation, ModeSelection) {
575 auto measurement_model = noiseModel::Isotropic::Sigma(1, 0.1);
576 auto motion_model = noiseModel::Isotropic::Sigma(1, 1.0);
582 modes.emplace_back(
M(0), 2);
586 double noise_tight = 0.5, noise_loose = 5.0;
588 auto model0 = std::make_shared<MotionModel>(
589 X(0),
X(1), 0.0, noiseModel::Isotropic::Sigma(
d, noise_loose)),
590 model1 = std::make_shared<MotionModel>(
591 X(0),
X(1), 0.0, noiseModel::Isotropic::Sigma(
d, noise_tight));
593 std::vector<NonlinearFactor::shared_ptr> components = {model0,
model1};
617 GaussianConditional::sharedMeanAndStddev(
Z(0), -I_1x1,
X(0), Z_1x1, 0.1));
619 GaussianConditional::sharedMeanAndStddev(
Z(0), -I_1x1,
X(1), Z_1x1, 0.1));
622 {GaussianConditional::sharedMeanAndStddev(
Z(0), I_1x1,
X(0), -I_1x1,
X(1),
624 GaussianConditional::sharedMeanAndStddev(
Z(0), I_1x1,
X(0), -I_1x1,
X(1),
625 Z_1x1, noise_tight)}));
637 TEST(HybridEstimation, ModeSelection2) {
642 double noise_tight = 0.5, noise_loose = 5.0;
648 GaussianConditional::sharedMeanAndStddev(
Z(0), -I_3x3,
X(0),
Z_3x1, 0.1));
650 GaussianConditional::sharedMeanAndStddev(
Z(0), -I_3x3,
X(1),
Z_3x1, 0.1));
653 {GaussianConditional::sharedMeanAndStddev(
Z(0), I_3x3,
X(0), -I_3x3,
X(1),
655 GaussianConditional::sharedMeanAndStddev(
Z(0), I_3x3,
X(0), -I_3x3,
X(1),
656 Z_3x1, noise_tight)}));
670 auto measurement_model = noiseModel::Isotropic::Sigma(
d, 0.1);
671 auto motion_model = noiseModel::Isotropic::Sigma(
d, 1.0);
677 modes.emplace_back(
M(0), 2);
679 auto model0 = std::make_shared<BetweenFactor<Vector3>>(
680 X(0),
X(1),
Z_3x1, noiseModel::Isotropic::Sigma(
d, noise_loose)),
682 X(0),
X(1),
Z_3x1, noiseModel::Isotropic::Sigma(
d, noise_tight));
684 std::vector<NonlinearFactor::shared_ptr> components = {model0,
model1};