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

Merge pull request #1411 from borglab/feature/no_boost_in_values

release/4.3a0
Frank Dellaert 2023-01-24 09:37:17 -08:00 committed by GitHub
parent dd95757229 48af607e7e
commit ce7fd30da4
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 72 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
gtsam/nonlinear/Values-inl.h
View File

@ -92,18 +92,28 @@ namespace gtsam {
} }
}; };
/* ************************************************************************* */ /* ************************************************************************* */
template <class ValueType>
size_t Values::count() const {
size_t i = 0;
for (const auto& [_, value] : values_) {
if (dynamic_cast<const GenericValue<ValueType>*>(value.get())) ++i;
}
return i;
}
/* ************************************************************************* */
template <class ValueType> template <class ValueType>
std::map<Key, ValueType> std::map<Key, ValueType>
Values::extract(const std::function<bool(Key)>& filterFcn) const { Values::extract(const std::function<bool(Key)>& filterFcn) const {
std::map<Key, ValueType> result; std::map<Key, ValueType> result;
for (const auto& key_value : values_) { for (const auto& [key,value] : values_) {
// Check if key matches // Check if key matches
if (filterFcn(key_value.first)) { if (filterFcn(key)) {
// Check if type matches (typically does as symbols matched with types) // Check if type matches (typically does as symbols matched with types)
if (auto t = if (auto t =
dynamic_cast<const GenericValue<ValueType>*>(key_value.second)) dynamic_cast<const GenericValue<ValueType>*>(value.get()))
result[key_value.first] = t->value(); result[key] = t->value();
} }
} }
return result; return result;
@ -204,7 +214,7 @@ namespace gtsam {
// Check the type and throw exception if incorrect // Check the type and throw exception if incorrect
// h() split in two lines to avoid internal compiler error (MSVC2017) // h() split in two lines to avoid internal compiler error (MSVC2017)
auto h = internal::handle<ValueType>(); auto h = internal::handle<ValueType>();
return h(j, item->second); return h(j, item->second.get());
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -214,13 +224,13 @@ namespace gtsam {
KeyValueMap::const_iterator item = values_.find(j); KeyValueMap::const_iterator item = values_.find(j);
if(item != values_.end()) { if(item != values_.end()) {
const Value* value = item->second.get();
// dynamic cast the type and throw exception if incorrect // dynamic cast the type and throw exception if incorrect
auto ptr = dynamic_cast<const GenericValue<ValueType>*>(item->second); auto ptr = dynamic_cast<const GenericValue<ValueType>*>(value);
if (ptr) { if (ptr) {
return &ptr->value(); return &ptr->value();
} else { } else {
// NOTE(abe): clang warns about potential side effects if done in typeid // NOTE(abe): clang warns about potential side effects if done in typeid
const Value* value = item->second;
throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType)); throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType));
} }
} else { } else {

80
gtsam/nonlinear/Values.cpp
View File

@ -50,15 +50,14 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Values::Values(const Values& other, const VectorValues& delta) { Values::Values(const Values& other, const VectorValues& delta) {
for (const auto& key_value : other.values_) { for (const auto& [key,value] : other.values_) {
VectorValues::const_iterator it = delta.find(key_value.first); VectorValues::const_iterator it = delta.find(key);
Key key = key_value.first; // Non-const duplicate to deal with non-const insert argument
if (it != delta.end()) { if (it != delta.end()) {
const Vector& v = it->second; const Vector& v = it->second;
Value* retractedValue(key_value.second->retract_(v)); // Retract Value* retractedValue(value->retract_(v)); // Retract
values_.insert(key, retractedValue); // Add retracted result directly to result values values_.emplace(key, retractedValue); // Add retracted result directly to result values
} else { } else {
values_.insert(key, key_value.second->clone_()); // Add original version to result values values_.emplace(key, value->clone_()); // Add original version to result values
} }
} }
} }
@ -67,9 +66,9 @@ namespace gtsam {
void Values::print(const string& str, const KeyFormatter& keyFormatter) const { void Values::print(const string& str, const KeyFormatter& keyFormatter) const {
cout << str << (str.empty() ? "" : "\n"); cout << str << (str.empty() ? "" : "\n");
cout << "Values with " << size() << " values:\n"; cout << "Values with " << size() << " values:\n";
for (const auto& key_value : values_) { for (const auto& [key,value] : values_) {
cout << "Value " << keyFormatter(key_value.first) << ": "; cout << "Value " << keyFormatter(key) << ": ";
key_value.second->print(""); value->print("");
cout << "\n"; cout << "\n";
} }
} }
@ -80,8 +79,8 @@ namespace gtsam {
return false; return false;
for (auto it1 = values_.begin(), it2 = other.values_.begin(); for (auto it1 = values_.begin(), it2 = other.values_.begin();
it1 != values_.end(); ++it1, ++it2) { it1 != values_.end(); ++it1, ++it2) {
const Value* value1 = it1->second; const Value* value1 = it1->second.get();
const Value* value2 = it2->second; const Value* value2 = it2->second.get();
if (typeid(*value1) != typeid(*value2) || it1->first != it2->first if (typeid(*value1) != typeid(*value2) || it1->first != it2->first
|| !value1->equals_(*value2, tol)) { || !value1->equals_(*value2, tol)) {
return false; return false;
@ -144,50 +143,48 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
const Value& Values::at(Key j) const { const Value& Values::at(Key j) const {
// Find the item KeyValueMap::const_iterator it = values_.find(j);
KeyValueMap::const_iterator item = values_.find(j);
// Throw exception if it does not exist // Throw exception if it does not exist
if(item == values_.end()) if(it == values_.end())
throw ValuesKeyDoesNotExist("retrieve", j); throw ValuesKeyDoesNotExist("retrieve", j);
return *item->second; return *it->second;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Values::insert(Key j, const Value& val) { void Values::insert(Key j, const Value& val) {
auto insertResult = values_.insert(j, val.clone_()); auto insertResult = values_.emplace(j, val.clone_());
if(!insertResult.second) if(!insertResult.second)
throw ValuesKeyAlreadyExists(j); throw ValuesKeyAlreadyExists(j);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Values::insert(const Values& other) { void Values::insert(const Values& other) {
for (auto key_value = other.values_.begin(); for (const auto& [key, value] : other.values_) {
key_value != other.values_.end(); ++key_value) { insert(key, *(value));
insert(key_value->first, *(key_value->second));
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Values::update(Key j, const Value& val) { void Values::update(Key j, const Value& val) {
// Find the value to update // Find the value to update
KeyValueMap::iterator item = values_.find(j); KeyValueMap::iterator it = values_.find(j);
if (item == values_.end()) if (it == values_.end())
throw ValuesKeyDoesNotExist("update", j); throw ValuesKeyDoesNotExist("update", j);
// Cast to the derived type // Cast to the derived type
const Value& old_value = *item->second; const Value& old_value = *it->second;
if (typeid(old_value) != typeid(val)) if (typeid(old_value) != typeid(val))
throw ValuesIncorrectType(j, typeid(old_value), typeid(val)); throw ValuesIncorrectType(j, typeid(old_value), typeid(val));
values_.replace(item, val.clone_()); values_.erase(j);
values_.emplace(j, val.clone_());
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Values::update(const Values& other) { void Values::update(const Values& other) {
for (auto key_value = other.values_.begin(); for (auto& [key, value] : other.values_) {
key_value != other.values_.end(); ++key_value) { this->update(key, *(value));
this->update(key_value->first, *(key_value->second));
} }
} }
@ -204,34 +201,33 @@ namespace gtsam {
/* ************************************************************************ */ /* ************************************************************************ */
void Values::insert_or_assign(const Values& other) { void Values::insert_or_assign(const Values& other) {
for (auto key_value = other.values_.begin(); for (auto& [key, value] : other.values_) {
key_value != other.values_.end(); ++key_value) { this->insert_or_assign(key, *(value));
this->insert_or_assign(key_value->first, *(key_value->second));
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
void Values::erase(Key j) { void Values::erase(Key j) {
KeyValueMap::iterator item = values_.find(j); KeyValueMap::iterator it = values_.find(j);
if(item == values_.end()) if(it == values_.end())
throw ValuesKeyDoesNotExist("erase", j); throw ValuesKeyDoesNotExist("erase", j);
values_.erase(item); values_.erase(it);
} }
/* ************************************************************************* */ /* ************************************************************************* */
KeyVector Values::keys() const { KeyVector Values::keys() const {
KeyVector result; KeyVector result;
result.reserve(size()); result.reserve(size());
for(const auto& key_value: values_) for(const auto& [key,value]: values_)
result.push_back(key_value.first); result.push_back(key);
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
KeySet Values::keySet() const { KeySet Values::keySet() const {
KeySet result; KeySet result;
for(const auto& key_value: values_) for(const auto& [key,value]: values_)
result.insert(key_value.first); result.insert(key);
return result; return result;
} }
@ -245,8 +241,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
size_t Values::dim() const { size_t Values::dim() const {
size_t result = 0; size_t result = 0;
for (const auto key_value : values_) { for (const auto& [key,value] : values_) {
result += key_value->second->dim(); result += value->dim();
} }
return result; return result;
} }
@ -254,8 +250,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
std::map<Key,size_t> Values::dims() const { std::map<Key,size_t> Values::dims() const {
std::map<Key,size_t> result; std::map<Key,size_t> result;
for (const auto key_value : values_) { for (const auto& [key,value] : values_) {
result.emplace(key_value->first, key_value->second->dim()); result.emplace(key, value->dim());
} }
return result; return result;
} }
@ -263,8 +259,8 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues Values::zeroVectors() const { VectorValues Values::zeroVectors() const {
VectorValues result; VectorValues result;
for (const auto key_value : values_) for (const auto& [key,value] : values_)
result.insert(key_value->first, Vector::Zero(key_value->second->dim())); result.insert(key, Vector::Zero(value->dim()));
return result; return result;
} }

30
gtsam/nonlinear/Values.h
View File

@ -24,13 +24,17 @@
#pragma once #pragma once
#include <gtsam/inference/Key.h>
#include <gtsam/base/FastDefaultAllocator.h> #include <gtsam/base/FastDefaultAllocator.h>
#include <gtsam/base/GenericValue.h> #include <gtsam/base/GenericValue.h>
#include <gtsam/base/VectorSpace.h> #include <gtsam/base/VectorSpace.h>
#include <gtsam/inference/Key.h>
#include <boost/ptr_container/serialize_ptr_map.hpp>
#include <memory>
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/unique_ptr.hpp>
#endif
#include <memory>
#include <string> #include <string>
#include <utility> #include <utility>
@ -67,12 +71,9 @@ namespace gtsam {
// user defines the allocation details (i.e. optimize for memory pool/arenas // user defines the allocation details (i.e. optimize for memory pool/arenas
// concurrency). // concurrency).
typedef internal::FastDefaultAllocator<typename std::pair<const Key, void*>>::type KeyValuePtrPairAllocator; typedef internal::FastDefaultAllocator<typename std::pair<const Key, void*>>::type KeyValuePtrPairAllocator;
typedef boost::ptr_map< using KeyValueMap =
Key, std::map<Key, std::unique_ptr<Value>, std::less<Key>,
Value, std::allocator<std::pair<const Key, std::unique_ptr<Value>>>>;
std::less<Key>,
ValueCloneAllocator,
KeyValuePtrPairAllocator > KeyValueMap;
// The member to store the values, see just above // The member to store the values, see just above
KeyValueMap values_; KeyValueMap values_;
@ -264,15 +265,8 @@ namespace gtsam {
VectorValues zeroVectors() const; VectorValues zeroVectors() const;
// Count values of given type \c ValueType // Count values of given type \c ValueType
template<class ValueType> template <class ValueType>
size_t count() const { size_t count() const;
size_t i = 0;
for (const auto key_value : values_) {
if (dynamic_cast<const GenericValue<ValueType>*>(key_value.second))
++i;
}
return i;
}
/** /**
* Extract a subset of values of the given type \c ValueType. * Extract a subset of values of the given type \c ValueType.

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

@ -474,13 +474,13 @@ TEST(Values, std_move) {
EXPECT_LONGS_EQUAL(2, (long)TestValueData::DestructorCount); EXPECT_LONGS_EQUAL(2, (long)TestValueData::DestructorCount);
EXPECT_LONGS_EQUAL(2, values.size()); EXPECT_LONGS_EQUAL(2, values.size());
TestValues moved(std::move(values)); // Move happens here ! TestValues moved(std::move(values)); // Move happens here !
EXPECT_LONGS_EQUAL(2, values.size()); // Uh oh! Should be 0 ! EXPECT_LONGS_EQUAL(0, values.size()); // Should be 0 !
EXPECT_LONGS_EQUAL(2, moved.size()); EXPECT_LONGS_EQUAL(2, moved.size());
EXPECT_LONGS_EQUAL(8, (long)TestValueData::ConstructorCount); // Uh oh! Should be 6 :-( EXPECT_LONGS_EQUAL(6, (long)TestValueData::ConstructorCount); // Should be 6 :-)
EXPECT_LONGS_EQUAL(2, (long)TestValueData::DestructorCount); // extra insert copies EXPECT_LONGS_EQUAL(2, (long)TestValueData::DestructorCount); // extra insert copies
} }
EXPECT_LONGS_EQUAL(8, (long)TestValueData::ConstructorCount); EXPECT_LONGS_EQUAL(6, (long)TestValueData::ConstructorCount);
EXPECT_LONGS_EQUAL(8, (long)TestValueData::DestructorCount); EXPECT_LONGS_EQUAL(6, (long)TestValueData::DestructorCount);
} }
} }