LabeledSymbol.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
19 
20 #include <iostream>
21 #include <limits>
22 
23 namespace gtsam {
24 
25 using namespace std;
26 
27 /* ************************************************************************* */
29 : c_(0), label_(0), j_(0) {}
30 
31 /* ************************************************************************* */
33 : c_(key.c_), label_(key.label_), j_(key.j_) {}
34 
35 /* ************************************************************************* */
36 LabeledSymbol::LabeledSymbol(unsigned char c, unsigned char label, std::uint64_t j)
37 : c_(c), label_(label), j_(j) {}
38 
39 /* ************************************************************************* */
41  const size_t keyBits = sizeof(gtsam::Key) * 8;
42  const size_t chrBits = sizeof(unsigned char) * 8;
43  const size_t lblBits = sizeof(unsigned char) * 8;
44  const size_t indexBits = keyBits - chrBits - lblBits;
47  const gtsam::Key indexMask = ~(chrMask | lblMask);
48  c_ = (unsigned char)((key & chrMask) >> (indexBits + lblBits));
49  label_ = (unsigned char)((key & lblMask) >> indexBits);
50  j_ = key & indexMask;
51 }
52 
53 /* ************************************************************************* */
54 LabeledSymbol::operator gtsam::Key() const {
55  const size_t keyBits = sizeof(gtsam::Key) * 8;
56  const size_t chrBits = sizeof(unsigned char) * 8;
57  const size_t lblBits = sizeof(unsigned char) * 8;
58  const size_t indexBits = keyBits - chrBits - lblBits;
61  const gtsam::Key indexMask = ~(chrMask | lblMask);
62  if(j_ > indexMask)
63  throw std::invalid_argument("Symbol index is too large");
64  gtsam::Key key = (gtsam::Key(c_) << (indexBits + lblBits)) | (gtsam::Key(label_) << indexBits) | j_;
65  return key;
66 }
67 
68 /* ************************************************************************* */
69 void LabeledSymbol::print(const std::string& s) const {
70  std::cout << s << ": " << (std::string) (*this) << std::endl;
71 }
72 
73 /* ************************************************************************* */
74 LabeledSymbol::operator std::string() const {
75  char buffer[100];
76  snprintf(buffer, 100, "%c%c%llu", c_, label_,
77  static_cast<unsigned long long>(j_));
78  return std::string(buffer);
79 }
80 
81 /* ************************************************************************* */
82 bool LabeledSymbol::operator<(const LabeledSymbol& comp) const {
83  return c_ < comp.c_
84  || (comp.c_ == c_ && label_ < comp.label_)
85  || (comp.c_ == c_ && comp.label_ == label_ && j_ < comp.j_);
86 }
87 
88 /* ************************************************************************* */
89 bool LabeledSymbol::operator==(const LabeledSymbol& comp) const {
90  return comp.c_ == c_ && comp.label_ == label_ && comp.j_ == j_;
91 }
92 
93 /* ************************************************************************* */
94 bool LabeledSymbol::operator!=(const LabeledSymbol& comp) const {
95  return comp.c_ != c_ || comp.label_ != label_ || comp.j_ != j_;
96 }
97 
98 /* ************************************************************************* */
100  return comp == (gtsam::Key)(*this);
101 }
102 
103 /* ************************************************************************* */
105  return comp != (gtsam::Key)(*this);
106 }
107 
108 /* ************************************************************************* */
110 
111 std::function<bool(gtsam::Key)> LabeledSymbol::TypeTest(unsigned char c) {
112  // Use lambda function to check equality
113  auto equals = [](unsigned char s, unsigned char c) { return s == c; };
114  return std::bind(
115  equals,
116  std::bind(&LabeledSymbol::chr, std::bind(make, std::placeholders::_1)),
117  c);
118 }
119 
120 std::function<bool(gtsam::Key)> LabeledSymbol::LabelTest(unsigned char label) {
121  // Use lambda function to check equality
122  auto equals = [](unsigned char s, unsigned char c) { return s == c; };
123  return std::bind(
124  equals,
125  std::bind(&LabeledSymbol::label, std::bind(make, std::placeholders::_1)),
126  label);
127 }
128 
129 std::function<bool(gtsam::Key)> LabeledSymbol::TypeLabelTest(unsigned char c, unsigned char label) {
130  // Use lambda functions for && and ==
131  auto logical_and = [](bool is_type, bool is_label) { return is_type == is_label; };
132  auto equals = [](unsigned char s, unsigned char c) { return s == c; };
133  return std::bind(logical_and,
134  std::bind(equals,
135  std::bind(&LabeledSymbol::chr,
136  std::bind(make, std::placeholders::_1)),
137  c),
138  std::bind(equals,
139  std::bind(&LabeledSymbol::label,
140  std::bind(make, std::placeholders::_1)),
141  label));
142 }
143 
144 /* ************************************************************************* */
145 GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const LabeledSymbol &symbol) {
146  os << StreamedKey(symbol);
147  return os;
148 }
149 
150 /* ************************************************************************* */
151 
152 } // \namespace gtsam
153 
gtsam::LabeledSymbol::j_
std::uint64_t j_
Definition: LabeledSymbol.h:40
gtsam::LabeledSymbol::chr
unsigned char chr() const
Retrieve key character.
Definition: LabeledSymbol.h:84
s
RealScalar s
Definition: level1_cplx_impl.h:126
gtsam::operator<<
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
gtsam::LabeledSymbol
Definition: LabeledSymbol.h:37
gtsam::LabeledSymbol::print
void print(const std::string &s="") const
Prints the LabeledSymbol with an optional prefix string.
Definition: LabeledSymbol.cpp:69
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
gtsam::indexMask
static const Key indexMask
Definition: Symbol.cpp:33
gtsam::indexBits
static const size_t indexBits
Definition: Symbol.cpp:31
buffer
Definition: pytypes.h:2270
os
ofstream os("timeSchurFactors.csv")
gtsam::LabeledSymbol::TypeTest
static std::function< bool(Key)> TypeTest(unsigned char c)
Checks only the type.
Definition: LabeledSymbol.cpp:111
gtsam::LabeledSymbol::key
Key key() const
return the integer version
Definition: LabeledSymbol.h:78
gtsam::LabeledSymbol::LabelTest
static std::function< bool(Key)> LabelTest(unsigned char label)
Checks only the robot ID (label_)
Definition: LabeledSymbol.cpp:120
gtsam::LabeledSymbol::c_
unsigned char c_
Definition: LabeledSymbol.h:39
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::symbol
Key symbol(unsigned char c, std::uint64_t j)
Definition: inference/Symbol.h:139
gtsam::LabeledSymbol::operator==
bool operator==(const LabeledSymbol &comp) const
Definition: LabeledSymbol.cpp:89
LabeledSymbol.h
gtsam::chrBits
static const size_t chrBits
Definition: Symbol.cpp:30
gtsam::LabeledSymbol::operator<
bool operator<(const LabeledSymbol &comp) const
Definition: LabeledSymbol.cpp:82
gtsam::equals
Definition: Testable.h:112
gtsam::LabeledSymbol::label
unsigned char label() const
Retrieve label character.
Definition: LabeledSymbol.h:81
key
const gtsam::Symbol key('X', 0)
gtsam::StreamedKey
To use the key_formatter on Keys, they must be wrapped in a StreamedKey.
Definition: Key.h:64
gtsam
traits
Definition: SFMdata.h:40
gtsam::LabeledSymbol::operator!=
bool operator!=(const LabeledSymbol &comp) const
Definition: LabeledSymbol.cpp:94
gtsam::LabeledSymbol::LabeledSymbol
LabeledSymbol()
Default constructor.
Definition: LabeledSymbol.cpp:28
std
Definition: BFloat16.h:88
gtsam::chrMask
static const Key chrMask
Definition: Symbol.cpp:32
gtsam::LabeledSymbol::TypeLabelTest
static std::function< bool(Key)> TypeLabelTest(unsigned char c, unsigned char label)
Checks both type and the robot ID.
Definition: LabeledSymbol.cpp:129
gtsam::LabeledSymbol::label_
unsigned char label_
Definition: LabeledSymbol.h:39
gtsam::keyBits
static const size_t keyBits
Definition: Symbol.cpp:29
uint64_t
unsigned __int64 uint64_t
Definition: ms_stdint.h:95
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
max
#define max(a, b)
Definition: datatypes.h:20
gtsam::make
static LabeledSymbol make(gtsam::Key key)
Definition: LabeledSymbol.cpp:109


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:33:02