HybridFactorGraph.cpp

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 * See LICENSE for the license information
 
 * -------------------------------------------------------------------------- */
 
#include <gtsam/hybrid/HybridFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
 
namespace gtsam {
 
/* ************************************************************************* */
std::set<DiscreteKey> HybridFactorGraph::discreteKeys() const {
  std::set<DiscreteKey> keys;
  for (auto& factor : factors_) {
    if (auto p = std::dynamic_pointer_cast<DiscreteFactor>(factor)) {
      for (const DiscreteKey& key : p->discreteKeys()) {
        keys.insert(key);
      }
    } else if (auto p = std::dynamic_pointer_cast<HybridFactor>(factor)) {
      for (const DiscreteKey& key : p->discreteKeys()) {
        keys.insert(key);
      }
    }
  }
  return keys;
}
 
/* ************************************************************************* */
KeySet HybridFactorGraph::discreteKeySet() const {
  KeySet keys;
  std::set<DiscreteKey> key_set = discreteKeys();
  std::transform(key_set.begin(), key_set.end(),
                 std::inserter(keys, keys.begin()),
                 [](const DiscreteKey& k) { return k.first; });
  return keys;
}
 
/* ************************************************************************* */
const KeySet HybridFactorGraph::continuousKeySet() const {
  KeySet keys;
  for (auto& factor : factors_) {
    if (auto p = std::dynamic_pointer_cast<HybridFactor>(factor)) {
      for (const Key& key : p->continuousKeys()) {
        keys.insert(key);
      }
    } else if (auto p = std::dynamic_pointer_cast<GaussianFactor>(factor)) {
      keys.insert(p->keys().begin(), p->keys().end());
    } else if (auto p = std::dynamic_pointer_cast<NonlinearFactor>(factor)) {
      keys.insert(p->keys().begin(), p->keys().end());
    }
  }
  return keys;
}
 
/* ************************************************************************* */
 
}  // namespace gtsam