diff --git a/gtsam/base/types.cpp b/gtsam/base/types.cpp index edc449b12..f96e037c1 100644 --- a/gtsam/base/types.cpp +++ b/gtsam/base/types.cpp @@ -64,4 +64,50 @@ std::string demangle(const char* name) { return demangled_name; } +std::string GTDKeyFormatter(const Key &key) { + constexpr size_t kMax_uchar_ = std::numeric_limits::max(); + constexpr size_t key_bits = sizeof(gtsam::Key) * 8; + constexpr size_t ch1_bits = sizeof(uint8_t) * 8; + constexpr size_t ch2_bits = sizeof(uint8_t) * 8; + constexpr size_t link_bits = sizeof(uint8_t) * 8; + constexpr size_t joint_bits = sizeof(uint8_t) * 8; + constexpr size_t time_bits = + key_bits - ch1_bits - ch2_bits - link_bits - joint_bits; + + constexpr gtsam::Key ch1_mask = gtsam::Key(kMax_uchar_) + << (key_bits - ch1_bits); + constexpr gtsam::Key ch2_mask = gtsam::Key(kMax_uchar_) + << (key_bits - ch1_bits - ch2_bits); + constexpr gtsam::Key link_mask = gtsam::Key(kMax_uchar_) + << (time_bits + joint_bits); + constexpr gtsam::Key joint_mask = gtsam::Key(kMax_uchar_) << time_bits; + constexpr gtsam::Key time_mask = + ~(ch1_mask | ch2_mask | link_mask | joint_mask); + + uint8_t c1_, c2_, link_idx_, joint_idx_; + uint64_t t_; + + c1_ = (uint8_t)((key & ch1_mask) >> (key_bits - ch1_bits)); + c2_ = (uint8_t)((key & ch2_mask) >> (key_bits - ch1_bits - ch2_bits)); + link_idx_ = (uint8_t)((key & link_mask) >> (time_bits + joint_bits)); + joint_idx_ = (uint8_t)((key & joint_mask) >> time_bits); + t_ = key & time_mask; + + std::string s = ""; + if (c1_ != 0) { + s += c1_; + } + if (c2_ != 0) { + s += c2_; + } + if (link_idx_ != kMax_uchar_) { + s += "[" + std::to_string((int)(link_idx_)) + "]"; + } + if (joint_idx_ != kMax_uchar_) { + s += "(" + std::to_string((int)(joint_idx_)) + ")"; + } + s += std::to_string(t_); + return s; +} + } /* namespace gtsam */ diff --git a/gtsam/base/types.h b/gtsam/base/types.h index a0d24f1a6..dcb389c9b 100644 --- a/gtsam/base/types.h +++ b/gtsam/base/types.h @@ -74,6 +74,8 @@ namespace gtsam { /// The index type for Eigen objects typedef ptrdiff_t DenseIndex; + std::string GTDKeyFormatter(const Key &key); + /* ************************************************************************* */ /** * Helper class that uses templates to select between two types based on diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 286129f8b..041603fbd 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -55,52 +55,6 @@ namespace gtsam { template class EliminateableFactorGraph; -std::string GTDKeyFormatter(const Key &key) { - constexpr size_t kMax_uchar_ = std::numeric_limits::max(); - constexpr size_t key_bits = sizeof(gtsam::Key) * 8; - constexpr size_t ch1_bits = sizeof(uint8_t) * 8; - constexpr size_t ch2_bits = sizeof(uint8_t) * 8; - constexpr size_t link_bits = sizeof(uint8_t) * 8; - constexpr size_t joint_bits = sizeof(uint8_t) * 8; - constexpr size_t time_bits = - key_bits - ch1_bits - ch2_bits - link_bits - joint_bits; - - constexpr gtsam::Key ch1_mask = gtsam::Key(kMax_uchar_) - << (key_bits - ch1_bits); - constexpr gtsam::Key ch2_mask = gtsam::Key(kMax_uchar_) - << (key_bits - ch1_bits - ch2_bits); - constexpr gtsam::Key link_mask = gtsam::Key(kMax_uchar_) - << (time_bits + joint_bits); - constexpr gtsam::Key joint_mask = gtsam::Key(kMax_uchar_) << time_bits; - constexpr gtsam::Key time_mask = - ~(ch1_mask | ch2_mask | link_mask | joint_mask); - - uint8_t c1_, c2_, link_idx_, joint_idx_; - uint64_t t_; - - c1_ = (uint8_t)((key & ch1_mask) >> (key_bits - ch1_bits)); - c2_ = (uint8_t)((key & ch2_mask) >> (key_bits - ch1_bits - ch2_bits)); - link_idx_ = (uint8_t)((key & link_mask) >> (time_bits + joint_bits)); - joint_idx_ = (uint8_t)((key & joint_mask) >> time_bits); - t_ = key & time_mask; - - std::string s = ""; - if (c1_ != 0) { - s += c1_; - } - if (c2_ != 0) { - s += c2_; - } - if (link_idx_ != kMax_uchar_) { - s += "[" + std::to_string((int)(link_idx_)) + "]"; - } - if (joint_idx_ != kMax_uchar_) { - s += "(" + std::to_string((int)(joint_idx_)) + ")"; - } - s += std::to_string(t_); - return s; -} - /* ************************************************************************ */ static GaussianMixtureFactor::Sum &addGaussian( GaussianMixtureFactor::Sum &sum, const GaussianFactor::shared_ptr &factor) {