12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

move GTDKeyFormatter to types.h

release/4.3a0
Varun Agrawal 2022-10-07 15:29:20 -04:00
parent 812ae30366
commit 6bd16d995e
3 changed files with 48 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
gtsam/base/types.cpp
View File

@ -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<uint8_t>::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 */

2
gtsam/base/types.h
View File

@ -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

46
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -55,52 +55,6 @@ namespace gtsam {
template class EliminateableFactorGraph<HybridGaussianFactorGraph>;
std::string GTDKeyFormatter(const Key &key) {
constexpr size_t kMax_uchar_ = std::numeric_limits<uint8_t>::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) {