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

Fixed compile problem with gcc and bug fix in int key <-> Symbol encoding

release/4.3a0
Richard Roberts 2012-02-24 23:38:51 +00:00
parent eae3ee0ecd
commit 12775256c1
9 changed files with 183 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/base/FastList.h
View File

@ -26,7 +26,7 @@
namespace gtsam {
/**
* FastSet is a thin wrapper around std::set that uses the boost
* FastList is a thin wrapper around std::list that uses the boost
* fast_pool_allocator instead of the default STL allocator. This is just a
* convenience to avoid having lengthy types in the code. Through timing,
* we've seen that the fast_pool_allocator can lead to speedups of several

2
gtsam/geometry/Pose3.h
View File

@ -62,7 +62,7 @@ namespace gtsam {
Pose3(const Rot3& R, const Point3& t) : R_(R), t_(t) {}
/** Construct from Pose2 */
Pose3(const Pose2& pose2);
explicit Pose3(const Pose2& pose2);
/** Constructor from 4*4 matrix */
Pose3(const Matrix &T) :

16
gtsam/nonlinear/ISAM2-impl-inl.h
View File

@ -144,8 +144,8 @@ void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
Index nextVar = originalnVars;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
delta.permutation()[nextVar] = nextVar;
ordering.insert(key_value.first, nextVar);
if(debug) cout << "Adding variable " << keyFormatter(key_value.first) << " with order " << nextVar << endl;
ordering.insert(key_value.key, nextVar);
if(debug) cout << "Adding variable " << keyFormatter(key_value.key) << " with order " << nextVar << endl;
++ nextVar;
}
assert(delta.permutation().size() == delta.container().size());
@ -234,19 +234,19 @@ void ISAM2<CONDITIONAL, GRAPH>::Impl::ExpmapMasked(Values& values, const Permute
Ordering::const_iterator key_index;
for(key_value = values.begin(), key_index = ordering.begin();
key_value != values.end() && key_index != ordering.end(); ++key_value, ++key_index) {
assert(key_value->first == key_index->first);
assert(key_value->key == key_index->first);
const Index var = key_index->second;
if(ISDEBUG("ISAM2 update verbose")) {
if(mask[var])
cout << "expmap " << keyFormatter(key_value->first) << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
cout << "expmap " << keyFormatter(key_value->key) << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
else
cout << " " << keyFormatter(key_value->first) << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
cout << " " << keyFormatter(key_value->key) << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
}
assert(delta[var].size() == (int)key_value->second.dim());
assert(delta[var].size() == (int)key_value->value.dim());
assert(delta[var].unaryExpr(&isfinite<double>).all());
if(mask[var]) {
Value* retracted = key_value->second.retract_(delta[var]);
key_value->second = *retracted;
Value* retracted = key_value->value.retract_(delta[var]);
key_value->value = *retracted;
retracted->deallocate_();
if(invalidateIfDebug)
(*invalidateIfDebug)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits<double>::infinity())); // Strange syntax to work with clang++ (bug in clang?)

34
gtsam/nonlinear/Symbol.h
View File

@ -23,6 +23,10 @@
#include <boost/mpl/char.hpp>
#include <boost/format.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/function.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/lambda/construct.hpp>
#include <boost/lambda/lambda.hpp>
#include <gtsam/nonlinear/Key.h>
@ -57,25 +61,25 @@ public:
/** Constructor that decodes an integer Key */
Symbol(Key key) {
const size_t keyBytes = sizeof(Key);
const size_t chrBytes = sizeof(unsigned char);
const size_t indexBytes = keyBytes - chrBytes;
const Key chrMask = std::numeric_limits<unsigned char>::max() << indexBytes;
const size_t keyBits = sizeof(Key) * 8;
const size_t chrBits = sizeof(unsigned char) * 8;
const size_t indexBits = keyBits - chrBits;
const Key chrMask = Key(std::numeric_limits<unsigned char>::max()) << indexBits;
const Key indexMask = ~chrMask;
c_ = (key & chrMask) >> indexBytes;
c_ = (unsigned char)((key & chrMask) >> indexBits);
j_ = key & indexMask;
}
/** Cast to integer */
operator Key() const {
const size_t keyBytes = sizeof(Key);
const size_t chrBytes = sizeof(unsigned char);
const size_t indexBytes = keyBytes - chrBytes;
const Key chrMask = std::numeric_limits<unsigned char>::max() << indexBytes;
const size_t keyBits = sizeof(Key) * 8;
const size_t chrBits = sizeof(unsigned char) * 8;
const size_t indexBits = keyBits - chrBits;
const Key chrMask = Key(std::numeric_limits<unsigned char>::max()) << indexBits;
const Key indexMask = ~chrMask;
if(j_ > indexMask)
throw std::invalid_argument("Symbol index is too large");
Key key = (c_ << indexBytes) | j_;
Key key = (Key(c_) << indexBits) | j_;
return key;
}
@ -113,6 +117,16 @@ public:
return comp.c_ != c_ || comp.j_ != j_;
}
/** Return a filter function that returns true when evaluated on a Key whose
* character (when converted to a Symbol) matches \c c. Use this with the
* Values::filter() function to retrieve all key-value pairs with the
* requested character.
*/
static boost::function<bool(Key)> ChrTest(unsigned char c) {
namespace bl = boost::lambda;
return bl::bind(&Symbol::chr, bl::bind(bl::constructor<Symbol>(), bl::_1)) == c;
}
private:
/** Serialization function */

42
gtsam/nonlinear/Values.cpp
View File

@ -43,8 +43,8 @@ namespace gtsam {
void Values::print(const string& str, const KeyFormatter& keyFormatter) const {
cout << str << "Values with " << size() << " values:\n" << endl;
for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
cout << " " << keyFormatter(key_value->first) << ": ";
key_value->second.print("");
cout << " " << keyFormatter(key_value->key) << ": ";
key_value->value.print("");
}
}
@ -53,11 +53,11 @@ namespace gtsam {
if(this->size() != other.size())
return false;
for(const_iterator it1=this->begin(), it2=other.begin(); it1!=this->end(); ++it1, ++it2) {
if(typeid(it1->second) != typeid(it2->second))
if(typeid(it1->value) != typeid(it2->value))
return false;
if(it1->first != it2->first)
if(it1->key != it2->key)
return false;
if(!it1->second.equals_(it2->second, tol))
if(!it1->value.equals_(it2->value, tol))
return false;
}
return true; // We return false earlier if we find anything that does not match
@ -78,9 +78,9 @@ namespace gtsam {
Values result;
for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
const SubVector& singleDelta = delta[ordering[key_value->first]]; // Delta for this value
Key key = key_value->first; // Non-const duplicate to deal with non-const insert argument
Value* retractedValue(key_value->second.retract_(singleDelta)); // Retract
const SubVector& singleDelta = delta[ordering[key_value->key]]; // Delta for this value
Key key = key_value->key; // Non-const duplicate to deal with non-const insert argument
Value* retractedValue(key_value->value.retract_(singleDelta)); // Retract
result.values_.insert(key, retractedValue); // Add retracted result directly to result values
}
@ -99,10 +99,10 @@ namespace gtsam {
if(this->size() != cp.size())
throw DynamicValuesMismatched();
for(const_iterator it1=this->begin(), it2=cp.begin(); it1!=this->end(); ++it1, ++it2) {
if(it1->first != it2->first)
if(it1->key != it2->key)
throw DynamicValuesMismatched(); // If keys do not match
// Will throw a dynamic_cast exception if types do not match
result.insert(ordering[it1->first], it1->second.localCoordinates_(it2->second));
result.insert(ordering[it1->key], it1->value.localCoordinates_(it2->value));
}
}
@ -117,8 +117,8 @@ namespace gtsam {
/* ************************************************************************* */
void Values::insert(const Values& values) {
for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
Key key = key_value->first; // Non-const duplicate to deal with non-const insert argument
insert(key, key_value->second);
Key key = key_value->key; // Non-const duplicate to deal with non-const insert argument
insert(key, key_value->value);
}
}
@ -139,7 +139,7 @@ namespace gtsam {
/* ************************************************************************* */
void Values::update(const Values& values) {
for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
this->update(key_value->first, key_value->second);
this->update(key_value->key, key_value->value);
}
}
@ -152,10 +152,12 @@ namespace gtsam {
}
/* ************************************************************************* */
FastList<Key> Values::keys() const {
FastList<Key> result;
for(const_iterator key_value = begin(); key_value != end(); ++key_value)
result.push_back(key_value->first);
std::list<Key> Values::keys() const {
std::list<Key> result;
for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
result.push_back(key_value->key);
cout << result.back() << endl;
}
return result;
}
@ -170,7 +172,7 @@ namespace gtsam {
vector<size_t> Values::dims(const Ordering& ordering) const {
vector<size_t> result(values_.size());
BOOST_FOREACH(const ConstKeyValuePair& key_value, *this) {
result[ordering[key_value.first]] = key_value.second.dim();
result[ordering[key_value.key]] = key_value.value.dim();
}
return result;
}
@ -179,7 +181,7 @@ namespace gtsam {
size_t Values::dim() const {
size_t result = 0;
BOOST_FOREACH(const ConstKeyValuePair& key_value, *this) {
result += key_value.second.dim();
result += key_value.value.dim();
}
return result;
}
@ -188,7 +190,7 @@ namespace gtsam {
Ordering::shared_ptr Values::orderingArbitrary(Index firstVar) const {
Ordering::shared_ptr ordering(new Ordering);
for(const_iterator key_value = begin(); key_value != end(); ++key_value) {
ordering->insert(key_value->first, firstVar++);
ordering->insert(key_value->key, firstVar++);
}
return ordering;
}

71
gtsam/nonlinear/Values.h
View File

@ -87,10 +87,21 @@ namespace gtsam {
typedef boost::shared_ptr<Values> shared_ptr;
/// A key-value pair, which you get by dereferencing iterators
typedef std::pair<const Key, Value&> KeyValuePair;
struct KeyValuePair {
const Key key; ///< The key
Value& value; ///< The value
KeyValuePair(Key _key, Value& _value) : key(_key), value(_value) {}
};
/// A key-value pair, which you get by dereferencing iterators
typedef std::pair<const Key, const Value&> ConstKeyValuePair;
struct ConstKeyValuePair {
const Key key; ///< The key
const Value& value; ///< The value
ConstKeyValuePair(Key _key, const Value& _value) : key(_key), value(_value) {}
ConstKeyValuePair(const KeyValuePair& kv) : key(kv.key), value(kv.value) {}
};
/// Mutable forward iterator, with value type KeyValuePair
typedef boost::transform_iterator<
@ -108,17 +119,42 @@ namespace gtsam {
typedef boost::transform_iterator<
boost::function1<ConstKeyValuePair, const ConstKeyValuePtrPair&>, KeyValueMap::const_reverse_iterator> const_reverse_iterator;
private:
template<class ValueType>
struct _KeyValuePair {
const Key key; ///< The key
ValueType& value; ///< The value
const Key& first; ///< For std::pair compatibility, the key
ValueType& second; ///< For std::pair compatibility, the value
_KeyValuePair(Key _key, ValueType& _value) : key(_key), value(_value), first(key), second(value) {}
};
template<class ValueType>
struct _ConstKeyValuePair {
const Key key; ///< The key
const ValueType& value; ///< The value
const Key& first; ///< For std::pair compatibility, the key
const ValueType& second; ///< For std::pair compatibility, the value
_ConstKeyValuePair(Key _key, const Value& _value) : key(_key), value(_value), first(key), second(value) {}
};
public:
template<class ValueType = Value>
class Filtered : public boost::transformed_range<
std::pair<const Key, ValueType&>(*)(KeyValuePair key_value),
_KeyValuePair<ValueType>(*)(Values::KeyValuePair key_value),
const boost::filtered_range<
boost::function<bool(const ConstKeyValuePair&)>,
const boost::iterator_range<iterator> > > {
public:
typedef _KeyValuePair<ValueType> KeyValuePair;
private:
typedef boost::transformed_range<
std::pair<const Key, ValueType&>(*)(KeyValuePair key_value),
KeyValuePair(*)(Values::KeyValuePair key_value),
const boost::filtered_range<
boost::function<bool(const ConstKeyValuePair&)>,
boost::function<bool(const Values::ConstKeyValuePair&)>,
const boost::iterator_range<iterator> > > Base;
Filtered(const Base& base) : Base(base) {}
@ -126,17 +162,20 @@ namespace gtsam {
friend class Values;
};
template<class ValueType>
template<class ValueType = Value>
class ConstFiltered : public boost::transformed_range<
std::pair<const Key, const ValueType&>(*)(ConstKeyValuePair key_value),
_ConstKeyValuePair<ValueType>(*)(Values::ConstKeyValuePair key_value),
const boost::filtered_range<
boost::function<bool(const ConstKeyValuePair&)>,
const boost::iterator_range<const_iterator> > > {
public:
typedef _ConstKeyValuePair<ValueType> KeyValuePair;
private:
typedef boost::transformed_range<
std::pair<const Key, const ValueType&>(*)(ConstKeyValuePair key_value),
KeyValuePair(*)(Values::ConstKeyValuePair key_value),
const boost::filtered_range<
boost::function<bool(const ConstKeyValuePair&)>,
boost::function<bool(const Values::ConstKeyValuePair&)>,
const boost::iterator_range<const_iterator> > > Base;
ConstFiltered(const Base& base) : Base(base) {}
@ -257,7 +296,7 @@ namespace gtsam {
* Returns a set of keys in the config
* Note: by construction, the list is ordered
*/
FastList<Key> keys() const;
std::list<Key> keys() const;
/** Replace all keys and variables */
Values& operator=(const Values& rhs);
@ -324,7 +363,7 @@ namespace gtsam {
boost::make_iterator_range(begin(), end()),
boost::function<bool(const ConstKeyValuePair&)>(
boost::bind(&filterHelper<ValueType>, filterFcn, _1))),
&castHelper<ValueType, KeyValuePair>);
&castHelper<ValueType, _KeyValuePair<ValueType>, KeyValuePair>);
}
/**
@ -371,7 +410,7 @@ namespace gtsam {
boost::make_iterator_range(begin(), end()),
boost::function<bool(const ConstKeyValuePair&)>(
boost::bind(&filterHelper<ValueType>, filterFcn, _1))),
&castHelper<const ValueType, ConstKeyValuePair>);
&castHelper<const ValueType, _ConstKeyValuePair<ValueType>, ConstKeyValuePair>);
}
private:
@ -380,14 +419,14 @@ namespace gtsam {
template<class ValueType>
static bool filterHelper(const boost::function<bool(Key)> filter, const ConstKeyValuePair& key_value) {
// Filter and check the type
return filter(key_value.first) && (typeid(ValueType) == typeid(key_value.second) || typeid(ValueType) == typeid(Value));
return filter(key_value.key) && (typeid(ValueType) == typeid(key_value.value) || typeid(ValueType) == typeid(Value));
}
// Cast to the derived ValueType
template<class ValueType, class KeyValuePairType>
static std::pair<const Key, ValueType&> castHelper(KeyValuePairType key_value) {
template<class ValueType, class CastedKeyValuePairType, class KeyValuePairType>
static CastedKeyValuePairType castHelper(KeyValuePairType key_value) {
// Static cast because we already checked the type during filtering
return std::pair<const Key, ValueType&>(key_value.first, static_cast<ValueType&>(key_value.second));
return CastedKeyValuePairType(key_value.key, static_cast<ValueType&>(key_value.value));
}
/** Serialization function */

25
gtsam/nonlinear/tests/testKey.cpp
View File

@ -19,6 +19,7 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/nonlinear/Symbol.h>
using namespace std;
@ -33,6 +34,30 @@ TEST(Key, KeySymbolConversion) {
EXPECT(assert_equal(expected, actual))
}
/* ************************************************************************* */
TEST(Key, KeySymbolEncoding) {
// Test encoding of Symbol <-> size_t <-> string
if(sizeof(Key) == 8) {
Symbol symbol(0x61, 5);
Key key = 0x6100000000000005;
string str = "a5";
LONGS_EQUAL(key, (Key)symbol);
assert_equal(str, DefaultKeyFormatter(symbol));
assert_equal(symbol, Symbol(key));
} else if(sizeof(Key) == 4) {
Symbol symbol(0x61, 5);
Key key = 0x61000005;
string str = "a5";
LONGS_EQUAL(key, (Key)symbol);
assert_equal(str, DefaultKeyFormatter(symbol));
assert_equal(symbol, Symbol(key));
}
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

61
gtsam/nonlinear/tests/testValues.cpp
View File

@ -31,7 +31,7 @@ using namespace gtsam;
using namespace std;
static double inf = std::numeric_limits<double>::infinity();
Key key1(Symbol('v',1)), key2(Symbol('v',2)), key3(Symbol('v',3)), key4(Symbol('v',4));
const Key key1(Symbol('v',1)), key2(Symbol('v',2)), key3(Symbol('v',3)), key4(Symbol('v',4));
/* ************************************************************************* */
TEST( Values, equals1 )
@ -218,14 +218,14 @@ TEST(Values, extract_keys)
config.insert(key3, Pose2());
config.insert(key4, Pose2());
FastList<Key> expected, actual;
std::list<Key> expected, actual;
expected += key1, key2, key3, key4;
actual = config.keys();
CHECK(actual.size() == expected.size());
FastList<Key>::const_iterator itAct = actual.begin(), itExp = expected.begin();
std::list<Key>::const_iterator itAct = actual.begin(), itExp = expected.begin();
for (; itAct != actual.end() && itExp != expected.end(); ++itAct, ++itExp) {
LONGS_EQUAL(*itExp, *itAct);
EXPECT(*itExp == *itAct);
}
}
@ -274,15 +274,15 @@ TEST(Values, filter) {
// Filter by key
int i = 0;
Values::Filtered<Value> filtered = values.filter(boost::bind(std::greater_equal<Key>(), _1, 2));
BOOST_FOREACH(const Values::Filtered<Value>::value_type& key_value, filtered) {
BOOST_FOREACH(const Values::Filtered<>::KeyValuePair& key_value, filtered) {
if(i == 0) {
LONGS_EQUAL(2, key_value.first);
EXPECT(typeid(Pose2) == typeid(key_value.second));
EXPECT(assert_equal(pose2, dynamic_cast<const Pose2&>(key_value.second)));
LONGS_EQUAL(2, key_value.key);
EXPECT(typeid(Pose2) == typeid(key_value.value));
EXPECT(assert_equal(pose2, dynamic_cast<const Pose2&>(key_value.value)));
} else if(i == 1) {
LONGS_EQUAL(3, key_value.first);
EXPECT(typeid(Pose3) == typeid(key_value.second));
EXPECT(assert_equal(pose3, dynamic_cast<const Pose3&>(key_value.second)));
LONGS_EQUAL(3, key_value.key);
EXPECT(typeid(Pose3) == typeid(key_value.value));
EXPECT(assert_equal(pose3, dynamic_cast<const Pose3&>(key_value.value)));
} else {
EXPECT(false);
}
@ -292,13 +292,42 @@ TEST(Values, filter) {
// Filter by type
i = 0;
BOOST_FOREACH(const Values::Filtered<Pose3>::value_type& key_value, values.filter<Pose3>()) {
BOOST_FOREACH(const Values::Filtered<Pose3>::KeyValuePair& key_value, values.filter<Pose3>()) {
if(i == 0) {
LONGS_EQUAL(1, key_value.first);
EXPECT(assert_equal(pose1, key_value.second));
LONGS_EQUAL(1, key_value.key);
EXPECT(assert_equal(pose1, key_value.value));
} else if(i == 1) {
LONGS_EQUAL(3, key_value.first);
EXPECT(assert_equal(pose3, key_value.second));
LONGS_EQUAL(3, key_value.key);
EXPECT(assert_equal(pose3, key_value.value));
} else {
EXPECT(false);
}
++ i;
}
LONGS_EQUAL(2, i);
}
/* ************************************************************************* */
TEST(Values, Symbol_filter) {
Pose2 pose0(1.0, 2.0, 0.3);
Pose3 pose1(Pose2(0.1, 0.2, 0.3));
Pose2 pose2(4.0, 5.0, 0.6);
Pose3 pose3(Pose2(0.3, 0.7, 0.9));
Values values;
values.insert(Symbol('x',0), pose0);
values.insert(Symbol('y',1), pose1);
values.insert(Symbol('x',2), pose2);
values.insert(Symbol('y',3), pose3);
int i = 0;
BOOST_FOREACH(const Values::Filtered<Value>::KeyValuePair& key_value, values.filter(Symbol::ChrTest('y'))) {
if(i == 0) {
LONGS_EQUAL(Symbol('y',1), key_value.key);
EXPECT(assert_equal(pose1, dynamic_cast<const Pose3&>(key_value.value)));
} else if(i == 1) {
LONGS_EQUAL(Symbol('y',3), key_value.key);
EXPECT(assert_equal(pose3, dynamic_cast<const Pose3&>(key_value.value)));
} else {
EXPECT(false);
}

4
gtsam/slam/dataset.cpp
View File

@ -155,8 +155,8 @@ void save2D(const pose2SLAM::Graph& graph, const Values& config, const SharedDia
// save poses
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) {
const Pose2& pose = dynamic_cast<const Pose2&>(key_value.second);
stream << "VERTEX2 " << key_value.first << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl;
const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value);
stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl;
}
// save edges