Moved LabeledSymbol from MastSLAM with some key-related utilities
Alex Cunningham
parent
62f6089119
commit
9e2b11800a
26
gtsam.h
26
gtsam.h
|
|
@ -1450,6 +1450,32 @@ size_t symbol(char chr, size_t index);
|
|||
char symbolChr(size_t key);
|
||||
size_t symbolIndex(size_t key);
|
||||
|
||||
// Key utilities
|
||||
gtsam::KeySet keyIntersection(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB);
|
||||
gtsam::KeySet keyDifference(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB);
|
||||
bool hasKeyIntersection(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB);
|
||||
|
||||
// Default keyformatter
|
||||
void printKeySet(const gtsam::KeySet& keys);
|
||||
void printKeySet(const gtsam::KeySet& keys, string s);
|
||||
|
||||
#include <gtsam/nonlinear/LabeledSymbol.h>
|
||||
class LabeledSymbol {
|
||||
LabeledSymbol(size_t full_key);
|
||||
LabeledSymbol(const gtsam::LabeledSymbol& key);
|
||||
LabeledSymbol(unsigned char valType, unsigned char label, size_t j);
|
||||
|
||||
size_t key() const;
|
||||
unsigned char label() const;
|
||||
unsigned char chr() const;
|
||||
size_t index() const;
|
||||
|
||||
gtsam::LabeledSymbol upper() const;
|
||||
gtsam::LabeledSymbol lower() const;
|
||||
|
||||
void print(string s) const;
|
||||
};
|
||||
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
class Ordering {
|
||||
// Standard Constructors and Named Constructors
|
||||
|
|
|
|||
|
|
@ -0,0 +1,191 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, 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
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file LabeledSymbol.h
|
||||
* @date Jan 12, 2010
|
||||
* @author: Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include <boost/mpl/char.hpp>
|
||||
#include <boost/format.hpp>
|
||||
#include <boost/function.hpp>
|
||||
#include <boost/lambda/bind.hpp>
|
||||
#include <boost/lambda/construct.hpp>
|
||||
#include <boost/lambda/lambda.hpp>
|
||||
|
||||
#include <boost/lexical_cast.hpp>
|
||||
|
||||
#include <gtsam/nonlinear/LabeledSymbol.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace std;
|
||||
|
||||
/* ************************************************************************* */
|
||||
LabeledSymbol::LabeledSymbol()
|
||||
: c_(0), label_(0), j_(0) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
LabeledSymbol::LabeledSymbol(const LabeledSymbol& key)
|
||||
: c_(key.c_), label_(key.label_), j_(key.j_) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
LabeledSymbol::LabeledSymbol(unsigned char c, unsigned char label, size_t j)
|
||||
: c_(c), label_(label), j_(j) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
LabeledSymbol::LabeledSymbol(gtsam::Key key) {
|
||||
const size_t keyBits = sizeof(gtsam::Key) * 8;
|
||||
const size_t chrBits = sizeof(unsigned char) * 8;
|
||||
const size_t lblBits = sizeof(unsigned char) * 8;
|
||||
const size_t indexBits = keyBits - chrBits - lblBits;
|
||||
const gtsam::Key chrMask = gtsam::Key(std::numeric_limits<unsigned char>::max()) << (indexBits + lblBits);
|
||||
const gtsam::Key lblMask = gtsam::Key(std::numeric_limits<unsigned char>::max()) << indexBits;
|
||||
const gtsam::Key indexMask = ~(chrMask | lblMask);
|
||||
c_ = (unsigned char)((key & chrMask) >> (indexBits + lblBits));
|
||||
label_ = (unsigned char)((key & lblMask) >> indexBits);
|
||||
j_ = key & indexMask;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
LabeledSymbol::operator gtsam::Key() const {
|
||||
const size_t keyBits = sizeof(gtsam::Key) * 8;
|
||||
const size_t chrBits = sizeof(unsigned char) * 8;
|
||||
const size_t lblBits = sizeof(unsigned char) * 8;
|
||||
const size_t indexBits = keyBits - chrBits - lblBits;
|
||||
const gtsam::Key chrMask = gtsam::Key(std::numeric_limits<unsigned char>::max()) << (indexBits + lblBits);
|
||||
const gtsam::Key lblMask = gtsam::Key(std::numeric_limits<unsigned char>::max()) << indexBits;
|
||||
const gtsam::Key indexMask = ~(chrMask | lblMask);
|
||||
if(j_ > indexMask)
|
||||
throw std::invalid_argument("Symbol index is too large");
|
||||
gtsam::Key key = (gtsam::Key(c_) << (indexBits + lblBits)) | (gtsam::Key(label_) << indexBits) | j_;
|
||||
return key;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void LabeledSymbol::print(const std::string& s) const {
|
||||
std::cout << s << ": " << (std::string) (*this) << std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
LabeledSymbol::operator std::string() const {
|
||||
return str(boost::format("%c%c%d") % c_ % label_ % j_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool LabeledSymbol::operator<(const LabeledSymbol& comp) const {
|
||||
return c_ < comp.c_
|
||||
|| (comp.c_ == c_ && label_ < comp.label_)
|
||||
|| (comp.c_ == c_ && comp.label_ == label_ && j_ < comp.j_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool LabeledSymbol::operator==(const LabeledSymbol& comp) const {
|
||||
return comp.c_ == c_ && comp.label_ == label_ && comp.j_ == j_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool LabeledSymbol::operator!=(const LabeledSymbol& comp) const {
|
||||
return comp.c_ != c_ || comp.label_ != label_ || comp.j_ != j_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool LabeledSymbol::operator==(gtsam::Key comp) const {
|
||||
return comp == (gtsam::Key)(*this);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool LabeledSymbol::operator!=(gtsam::Key comp) const {
|
||||
return comp != (gtsam::Key)(*this);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::function<bool(gtsam::Key)> LabeledSymbol::TypeTest(unsigned char c) {
|
||||
namespace bl = boost::lambda;
|
||||
return bl::bind(&LabeledSymbol::chr, bl::bind(bl::constructor<LabeledSymbol>(), bl::_1)) == c;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::function<bool(gtsam::Key)> LabeledSymbol::LabelTest(unsigned char label) {
|
||||
namespace bl = boost::lambda;
|
||||
return bl::bind(&LabeledSymbol::label, bl::bind(bl::constructor<LabeledSymbol>(), bl::_1)) == label;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::function<bool(gtsam::Key)> LabeledSymbol::TypeLabelTest(unsigned char c, unsigned char label) {
|
||||
namespace bl = boost::lambda;
|
||||
return bl::bind(&LabeledSymbol::chr, bl::bind(bl::constructor<LabeledSymbol>(), bl::_1)) == c &&
|
||||
bl::bind(&LabeledSymbol::label, bl::bind(bl::constructor<LabeledSymbol>(), bl::_1)) == label;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::string _multirobotKeyFormatter(gtsam::Key key) {
|
||||
const LabeledSymbol asLabeledSymbol(key);
|
||||
if(asLabeledSymbol.chr() > 0 && asLabeledSymbol.label() > 0)
|
||||
return (std::string)asLabeledSymbol;
|
||||
|
||||
const gtsam::Symbol asSymbol(key);
|
||||
if (asLabeledSymbol.chr() > 0)
|
||||
return (std::string)asSymbol;
|
||||
else
|
||||
return boost::lexical_cast<std::string>(key);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void printKeySet(const gtsam::KeySet& keys, const std::string& s, const KeyFormatter& keyFormatter) {
|
||||
cout << s << " ";
|
||||
if (keys.empty())
|
||||
cout << "(none)" << endl;
|
||||
else {
|
||||
BOOST_FOREACH(const gtsam::Key& key, keys)
|
||||
cout << keyFormatter(key) << " ";
|
||||
cout << endl;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
gtsam::KeySet keyIntersection(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB) {
|
||||
gtsam::KeySet intersection;
|
||||
if (keysA.empty() || keysB.empty())
|
||||
return intersection;
|
||||
BOOST_FOREACH(const gtsam::Key& key, keysA)
|
||||
if (keysB.count(key))
|
||||
intersection.insert(key);
|
||||
return intersection;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool hasKeyIntersection(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB) {
|
||||
if (keysA.empty() || keysB.empty())
|
||||
return false;
|
||||
BOOST_FOREACH(const gtsam::Key& key, keysA)
|
||||
if (keysB.count(key))
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
gtsam::KeySet keyDifference(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB) {
|
||||
if (keysA.empty() || keysB.empty())
|
||||
return keysA;
|
||||
|
||||
gtsam::KeySet difference;
|
||||
BOOST_FOREACH(const gtsam::Key& key, keysA)
|
||||
if (!keysB.count(key))
|
||||
difference.insert(key);
|
||||
return difference;
|
||||
}
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
|
|
@ -0,0 +1,147 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, 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
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file LabeledSymbol.h
|
||||
* @date Jan 12, 2010
|
||||
* @author: Alex Cunningham
|
||||
* @author: Frank Dellaert
|
||||
* @author: Richard Roberts
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/nonlinear/Symbol.h>
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Customized version of gtsam::Symbol for multi-robot use
|
||||
*/
|
||||
class LabeledSymbol {
|
||||
protected:
|
||||
unsigned char c_, label_;
|
||||
size_t j_;
|
||||
|
||||
public:
|
||||
/** Default constructor */
|
||||
LabeledSymbol();
|
||||
|
||||
/** Copy constructor */
|
||||
LabeledSymbol(const LabeledSymbol& key);
|
||||
|
||||
/** Constructor */
|
||||
LabeledSymbol(unsigned char c, unsigned char label, size_t j);
|
||||
|
||||
/** Constructor that decodes an integer gtsam::Key */
|
||||
LabeledSymbol(gtsam::Key key);
|
||||
|
||||
/** Cast to integer */
|
||||
operator gtsam::Key() const;
|
||||
|
||||
// Testable Requirements
|
||||
void print(const std::string& s = "") const;
|
||||
|
||||
bool equals(const LabeledSymbol& expected, double tol = 0.0) const {
|
||||
return (*this) == expected;
|
||||
}
|
||||
|
||||
/** return the integer version */
|
||||
gtsam::Key key() const { return (gtsam::Key) *this; }
|
||||
|
||||
/** Retrieve label character */
|
||||
inline unsigned char label() const { return label_; }
|
||||
|
||||
/** Retrieve key character */
|
||||
inline unsigned char chr() const { return c_; }
|
||||
|
||||
/** Retrieve key index */
|
||||
inline size_t index() const { return j_; }
|
||||
|
||||
/** Create a string from the key */
|
||||
operator std::string() const;
|
||||
|
||||
/** Comparison for use in maps */
|
||||
bool operator<(const LabeledSymbol& comp) const;
|
||||
bool operator==(const LabeledSymbol& comp) const;
|
||||
bool operator==(gtsam::Key comp) const;
|
||||
bool operator!=(const LabeledSymbol& comp) const;
|
||||
bool operator!=(gtsam::Key comp) const;
|
||||
|
||||
/** Return a filter function that returns true when evaluated on a gtsam::Key whose
|
||||
* character (when converted to a LabeledSymbol) matches \c c. Use this with the
|
||||
* Values::filter() function to retrieve all key-value pairs with the
|
||||
* requested character.
|
||||
*/
|
||||
|
||||
// Checks only the type
|
||||
static boost::function<bool(gtsam::Key)> TypeTest(unsigned char c);
|
||||
|
||||
// Checks only the robot ID (label_)
|
||||
static boost::function<bool(gtsam::Key)> LabelTest(unsigned char label);
|
||||
|
||||
// Checks both type and the robot ID
|
||||
static boost::function<bool(gtsam::Key)> TypeLabelTest(unsigned char c, unsigned char label);
|
||||
|
||||
// Converts to upper/lower versions of labels
|
||||
LabeledSymbol upper() const { return LabeledSymbol(c_, toupper(label_), j_); }
|
||||
LabeledSymbol lower() const { return LabeledSymbol(c_, tolower(label_), j_); }
|
||||
|
||||
// Create a new symbol with a different value
|
||||
LabeledSymbol newChr(unsigned char c) const { return LabeledSymbol(c, label_, j_); }
|
||||
LabeledSymbol newLabel(unsigned char label) const { return LabeledSymbol(c_, label, j_); }
|
||||
|
||||
private:
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(c_);
|
||||
ar & BOOST_SERIALIZATION_NVP(label_);
|
||||
ar & BOOST_SERIALIZATION_NVP(j_);
|
||||
}
|
||||
}; // \class LabeledSymbol
|
||||
|
||||
// Helper function for Multi-robot Key Formatter
|
||||
std::string _multirobotKeyFormatter(gtsam::Key key);
|
||||
|
||||
/**
|
||||
* A KeyFormatter that will check for LabeledSymbol keys, as well as Symbol and plain
|
||||
* integer keys. This keyformatter will need to be passed in to override the default
|
||||
* formatter in print functions.
|
||||
*
|
||||
* Checks for LabeledSymbol, Symbol and then plain keys, in order.
|
||||
*/
|
||||
static const gtsam::KeyFormatter MultiRobotKeyFormatter = &_multirobotKeyFormatter;
|
||||
|
||||
/// Version of orderingIndexFormatter using multi-robot formatter
|
||||
struct MultiRobotLinearFormatter : gtsam::OrderingIndexFormatter {
|
||||
MultiRobotLinearFormatter(const gtsam::Ordering& ordering)
|
||||
: gtsam::OrderingIndexFormatter(ordering, MultiRobotKeyFormatter) {}
|
||||
};
|
||||
|
||||
/// Utility function to print sets of keys with optional prefix
|
||||
void printKeySet(const KeySet& keys, const std::string& s = "",
|
||||
const KeyFormatter& keyFormatter = DefaultKeyFormatter);
|
||||
|
||||
/// Computes the intersection between two sets
|
||||
gtsam::KeySet keyIntersection(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB);
|
||||
|
||||
/// Checks if an intersection exists - faster checking size of above
|
||||
bool hasKeyIntersection(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB);
|
||||
|
||||
/// Computes a difference between sets, so result is those that are in A, but not B
|
||||
gtsam::KeySet keyDifference(const gtsam::KeySet& keysA, const gtsam::KeySet& keysB);
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
|
|
@ -0,0 +1,69 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, 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
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* @file testLabeledSymbol.cpp
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <boost/assign/std/list.hpp> // for operator +=
|
||||
using namespace boost::assign;
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
|
||||
#include <gtsam/nonlinear/LabeledSymbol.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testLabeledSymbol, KeyLabeledSymbolConversion ) {
|
||||
LabeledSymbol expected('x', 'A', 4);
|
||||
Key key(expected);
|
||||
LabeledSymbol actual(key);
|
||||
|
||||
EXPECT(assert_equal(expected, actual))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testLabeledSymbol, KeyLabeledSymbolEncoding ) {
|
||||
|
||||
// Test encoding of LabeledSymbol <-> size_t <-> string
|
||||
// Encoding scheme:
|
||||
// Top 8 bits: variable type (255 possible values) - zero will not process
|
||||
// Next 8 high bits: variable type (255 possible values)
|
||||
// TODO: use fewer bits for type - only 4 or 5 should be necessary - will need more decoding
|
||||
|
||||
if(sizeof(Key) == 8) {
|
||||
LabeledSymbol symbol(0x78, 0x41, 5);
|
||||
Key key = 0x7841000000000005;
|
||||
string str = "xA5";
|
||||
|
||||
EXPECT_LONGS_EQUAL(key, (Key)symbol);
|
||||
EXPECT(assert_equal(str, MultiRobotKeyFormatter(symbol)));
|
||||
EXPECT(assert_equal(symbol, LabeledSymbol(key)));
|
||||
} else if(sizeof(Key) == 4) {
|
||||
LabeledSymbol symbol(0x78, 0x41, 5);
|
||||
Key key = 0x78410005;
|
||||
string str = "xA5";
|
||||
|
||||
EXPECT_LONGS_EQUAL(key, (Key)symbol);
|
||||
EXPECT(assert_equal(str, DefaultKeyFormatter(symbol)));
|
||||
EXPECT(assert_equal(symbol, LabeledSymbol(key)));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
||||
Loading…
Reference in New Issue