diff --git a/.cproject b/.cproject
index 21cfaf8a6..fb4a2edae 100644
--- a/.cproject
+++ b/.cproject
@@ -468,6 +468,7 @@
make
+
testBayesTree.run
true
false
@@ -475,7 +476,6 @@
make
-
testSymbolicBayesNet.run
true
false
@@ -483,6 +483,7 @@
make
+
testSymbolicFactorGraph.run
true
false
@@ -714,6 +715,7 @@
make
+
testGraph.run
true
false
@@ -751,6 +753,14 @@
true
true
+
+make
+-j2
+testKey.run
+true
+true
+true
+
make
-j2
diff --git a/cpp/Key.h b/cpp/Key.h
index 6bbcba20a..168274fbd 100644
--- a/cpp/Key.h
+++ b/cpp/Key.h
@@ -8,11 +8,14 @@
#pragma once
+#include
#include
#include
#include
#include
+#include "Testable.h"
+
#define ALPHA '\224'
namespace gtsam {
@@ -21,12 +24,11 @@ namespace gtsam {
* TypedSymbol key class is templated on
* 1) the type T it is supposed to retrieve, for extra type checking
* 2) the character constant used for its string representation
- * TODO: make Testable
*/
template
- class TypedSymbol {
+ class TypedSymbol : Testable > {
- private:
+ protected:
size_t j_;
public:
@@ -49,6 +51,10 @@ namespace gtsam {
bool operator== (const TypedSymbol& compare) const { return j_==compare.j_;}
int compare(const TypedSymbol& compare) const {return j_-compare.j_;}
+ // Testable Requirements
+ void print(const std::string& name) const {} //FIXME
+ bool equals(const TypedSymbol& expected, double tol) const { return (*this)==expected; }
+
private:
/** Serialization function */
@@ -59,6 +65,72 @@ namespace gtsam {
}
};
+ /**
+ * TypedLabeledSymbol is a variation of the TypedSymbol that allows
+ * for a runtime label to be placed on the label, so as to express
+ * "Pose 5 for robot 3"
+ * Labels should be kept to base datatypes (int, char, etc) to
+ * minimize cost of comparisons
+ *
+ * The labels will be compared first when comparing Keys, followed by the
+ * index
+ */
+ template
+ class TypedLabeledSymbol : public TypedSymbol, Testable > {
+
+ protected:
+ // Label
+ L label_;
+
+ public:
+
+ // Constructors:
+
+ TypedLabeledSymbol() {}
+ TypedLabeledSymbol(size_t j, L label):TypedSymbol(j), label_(label) {}
+
+ // Get stuff:
+
+ L label() const { return label_;}
+ const char* c_str() const { return (std::string)(*this).c_str();}
+ operator std::string() const
+ { return (boost::format("%c%label%d") % C % label_ % this->j_).str(); }
+ std::string latex() const
+ { return (boost::format("%c%label_{%d}") % C % label_ % this->j_).str(); }
+
+ // logic:
+
+ bool operator< (const TypedLabeledSymbol& compare) const {
+ if (label_ == compare.label_) // sort by label first
+ return this->j_j_==compare.j_ && label_ == compare.label_;}
+ int compare(const TypedLabeledSymbol& compare) const {
+ if (label_ == compare.label_) // sort by label first
+ return this->j_-compare.j_;
+ else
+ return label_-compare.label_;
+ }
+
+ // Testable Requirements
+ void print(const std::string& name) const {} // FIXME
+ bool equals(const TypedLabeledSymbol& expected, double tol) const
+ { return (*this)==expected; }
+
+ private:
+
+ /** Serialization function */
+ friend class boost::serialization::access;
+ template
+ void serialize(Archive & ar, const unsigned int version) {
+ ar & BOOST_SERIALIZATION_NVP(this->j_);
+ ar & BOOST_SERIALIZATION_NVP(label_);
+ }
+ };
+
/**
* Character and index key used in VectorConfig, GaussianFactorGraph,
diff --git a/cpp/Makefile.am b/cpp/Makefile.am
index 17388672d..9859a193c 100644
--- a/cpp/Makefile.am
+++ b/cpp/Makefile.am
@@ -131,13 +131,15 @@ testSubgraphPreconditioner_LDADD = libgtsam.la
headers += Key.h NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h
headers += NonlinearFactor.h
-check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer
+check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testKey
testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp
testNonlinearFactor_LDADD = libgtsam.la
testNonlinearFactorGraph_SOURCES = $(example) testNonlinearFactorGraph.cpp
testNonlinearFactorGraph_LDADD = libgtsam.la
testNonlinearOptimizer_SOURCES = $(example) testNonlinearOptimizer.cpp
testNonlinearOptimizer_LDADD = libgtsam.la
+testKey_SOURCES = testKey.cpp
+testKey_LDADD = libgtsam.la
# Nonlinear constraints
headers += SQPOptimizer.h SQPOptimizer-inl.h
diff --git a/cpp/testKey.cpp b/cpp/testKey.cpp
new file mode 100644
index 000000000..168b345cf
--- /dev/null
+++ b/cpp/testKey.cpp
@@ -0,0 +1,55 @@
+/*
+ * @file testKey.cpp
+ * @author Alex Cunningham
+ */
+
+#include
+#include "Key.h"
+
+using namespace gtsam;
+
+class Pose3;
+
+/* ************************************************************************* */
+TEST ( TypedSymbol, basic_operations ) {
+ typedef TypedSymbol Key;
+
+ Key key1(0),
+ key2(0),
+ key3(1),
+ key4(2);
+
+ CHECK(key1.index()==0);
+ CHECK(key1 == key2);
+ CHECK(assert_equal(key1, key2));
+ CHECK(!(key1 == key3));
+ CHECK(key1 < key3);
+ CHECK(key3 < key4);
+}
+
+/* ************************************************************************* */
+TEST ( TypedLabledSymbol, basic_operations ) {
+ typedef TypedLabeledSymbol RobotKey;
+
+ RobotKey key1(0, 1),
+ key2(0, 1),
+ key3(1, 1),
+ key4(2, 1),
+ key5(0, 2),
+ key6(1, 2);
+
+ CHECK(key1.label()==1);
+ CHECK(key1.index()==0);
+ CHECK(key1 == key2);
+ CHECK(assert_equal(key1, key2));
+ CHECK(!(key1 == key3));
+ CHECK(key1 < key3);
+ CHECK(key3 < key4);
+ CHECK(!(key1 == key5));
+ CHECK(key1 < key5);
+ CHECK(key5 < key6);
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */