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

Moved utility functions and formatter from LabeledSymbol to Key. Added namespace functions

release/4.3a0
Alex Cunningham 2013-05-03 16:21:47 +00:00
parent 36e66b2f0b
commit 31b0f70530
6 changed files with 117 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam.h
View File

@ -1476,6 +1476,11 @@ class LabeledSymbol {
void print(string s) const;
};
size_t mrsymbol(unsigned char c, unsigned char label, size_t j);
unsigned char mrsymbolChr(size_t key);
unsigned char mrsymbolLabel(size_t key);
size_t mrsymbolIndex(size_t key);
#include <gtsam/nonlinear/Ordering.h>
class Ordering {
// Standard Constructors and Named Constructors

68
gtsam/nonlinear/Key.cpp
View File

@ -13,16 +13,21 @@
* @file Key.h
* @brief
* @author Richard Roberts
* @author Alex Cunningham
* @date Feb 20, 2012
*/
#include <gtsam/nonlinear/Key.h>
#include <iostream>
#include <boost/lexical_cast.hpp>
#include <gtsam/nonlinear/Symbol.h>
#include <boost/foreach.hpp>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/LabeledSymbol.h>
namespace gtsam {
/* ************************************************************************* */
std::string _defaultKeyFormatter(Key key) {
const Symbol asSymbol(key);
if(asSymbol.chr() > 0)
@ -31,4 +36,63 @@ std::string _defaultKeyFormatter(Key key) {
return boost::lexical_cast<std::string>(key);
}
/* ************************************************************************* */
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) {
std::cout << s << " ";
if (keys.empty())
std::cout << "(none)" << std::endl;
else {
BOOST_FOREACH(const gtsam::Key& key, keys)
std::cout << keyFormatter(key) << " ";
std::cout << std::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

23
gtsam/nonlinear/Key.h
View File

@ -41,10 +41,33 @@ namespace gtsam {
/// and Symbol keys.
static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter;
// Helper function for Multi-robot Key Formatter
GTSAM_EXPORT 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;
/// Useful typedefs for operations with Values - allow for matlab interfaces
typedef FastList<Key> KeyList;
typedef FastVector<Key> KeyVector;
typedef FastSet<Key> KeySet;
/// 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);
}

58
gtsam/nonlinear/LabeledSymbol.cpp
View File

@ -129,63 +129,5 @@ boost::function<bool(gtsam::Key)> LabeledSymbol::TypeLabelTest(unsigned char 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

47
gtsam/nonlinear/LabeledSymbol.h
View File

@ -20,14 +20,18 @@
#pragma once
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/Ordering.h>
namespace gtsam {
/**
* Customized version of gtsam::Symbol for multi-robot use
* Customized version of gtsam::Symbol for multi-robot use.
*
* This variation of Symbol stores two char values in addition to
* an integer key, which is useful for encoding a group for a
* variable. This was originally designed for multi-robot systems,
* which allows expressing "Pose 7 from robot B" as "xB7".
*/
class LabeledSymbol {
class GTSAM_EXPORT LabeledSymbol {
protected:
unsigned char c_, label_;
size_t j_;
@ -112,36 +116,19 @@ private:
}
}; // \class LabeledSymbol
// Helper function for Multi-robot Key Formatter
std::string _multirobotKeyFormatter(gtsam::Key key);
/** Create a symbol key from a character, label and index, i.e. xA5. */
inline Key mrsymbol(unsigned char c, unsigned char label, size_t j) {
return (Key)LabeledSymbol(c,label,j);
}
/**
* 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;
/** Return the character portion of a symbol key. */
inline unsigned char mrsymbolChr(Key key) { return LabeledSymbol(key).chr(); }
/// Version of orderingIndexFormatter using multi-robot formatter
struct MultiRobotLinearFormatter : gtsam::OrderingIndexFormatter {
MultiRobotLinearFormatter(const gtsam::Ordering& ordering)
: gtsam::OrderingIndexFormatter(ordering, MultiRobotKeyFormatter) {}
};
/** Return the label portion of a symbol key. */
inline unsigned char mrsymbolLabel(Key key) { return LabeledSymbol(key).label(); }
/// 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);
/** Return the index portion of a symbol key. */
inline size_t mrsymbolIndex(Key key) { return LabeledSymbol(key).index(); }
} // \namespace gtsam

6
gtsam/nonlinear/Ordering.h
View File

@ -267,5 +267,11 @@ public:
return keyFormatter_(ordering_.key(index)); }
};
/// Version of orderingIndexFormatter using multi-robot formatter
struct GTSAM_EXPORT MultiRobotLinearFormatter : gtsam::OrderingIndexFormatter {
MultiRobotLinearFormatter(const gtsam::Ordering& ordering)
: gtsam::OrderingIndexFormatter(ordering, MultiRobotKeyFormatter) {}
};
} // \namespace gtsam