diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 8337b82e2..f509fe534 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -92,18 +92,28 @@ namespace gtsam { } }; -/* ************************************************************************* */ + /* ************************************************************************* */ + template + size_t Values::count() const { + size_t i = 0; + for (const auto& [_, value] : values_) { + if (dynamic_cast*>(value.get())) ++i; + } + return i; + } + + /* ************************************************************************* */ template std::map Values::extract(const std::function& filterFcn) const { std::map result; - for (const auto& key_value : values_) { + for (const auto& [key,value] : values_) { // Check if key matches - if (filterFcn(key_value.first)) { + if (filterFcn(key)) { // Check if type matches (typically does as symbols matched with types) if (auto t = - dynamic_cast*>(key_value.second)) - result[key_value.first] = t->value(); + dynamic_cast*>(value.get())) + result[key] = t->value(); } } return result; @@ -204,7 +214,7 @@ namespace gtsam { // Check the type and throw exception if incorrect // h() split in two lines to avoid internal compiler error (MSVC2017) auto h = internal::handle(); - return h(j, item->second); + return h(j, item->second.get()); } /* ************************************************************************* */ @@ -214,13 +224,13 @@ namespace gtsam { KeyValueMap::const_iterator item = values_.find(j); if(item != values_.end()) { + const Value* value = item->second.get(); // dynamic cast the type and throw exception if incorrect - auto ptr = dynamic_cast*>(item->second); + auto ptr = dynamic_cast*>(value); if (ptr) { return &ptr->value(); } else { // NOTE(abe): clang warns about potential side effects if done in typeid - const Value* value = item->second; throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType)); } } else { diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp index 7e2295b85..6af9f2b6a 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -50,15 +50,14 @@ namespace gtsam { /* ************************************************************************* */ Values::Values(const Values& other, const VectorValues& delta) { - for (const auto& key_value : other.values_) { - VectorValues::const_iterator it = delta.find(key_value.first); - Key key = key_value.first; // Non-const duplicate to deal with non-const insert argument + for (const auto& [key,value] : other.values_) { + VectorValues::const_iterator it = delta.find(key); if (it != delta.end()) { const Vector& v = it->second; - Value* retractedValue(key_value.second->retract_(v)); // Retract - values_.insert(key, retractedValue); // Add retracted result directly to result values + Value* retractedValue(value->retract_(v)); // Retract + values_.emplace(key, retractedValue); // Add retracted result directly to result values } 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 { cout << str << (str.empty() ? "" : "\n"); cout << "Values with " << size() << " values:\n"; - for (const auto& key_value : values_) { - cout << "Value " << keyFormatter(key_value.first) << ": "; - key_value.second->print(""); + for (const auto& [key,value] : values_) { + cout << "Value " << keyFormatter(key) << ": "; + value->print(""); cout << "\n"; } } @@ -80,8 +79,8 @@ namespace gtsam { return false; for (auto it1 = values_.begin(), it2 = other.values_.begin(); it1 != values_.end(); ++it1, ++it2) { - const Value* value1 = it1->second; - const Value* value2 = it2->second; + const Value* value1 = it1->second.get(); + const Value* value2 = it2->second.get(); if (typeid(*value1) != typeid(*value2) || it1->first != it2->first || !value1->equals_(*value2, tol)) { return false; @@ -144,50 +143,48 @@ namespace gtsam { /* ************************************************************************* */ const Value& Values::at(Key j) const { - // Find the item - KeyValueMap::const_iterator item = values_.find(j); + KeyValueMap::const_iterator it = values_.find(j); // Throw exception if it does not exist - if(item == values_.end()) + if(it == values_.end()) throw ValuesKeyDoesNotExist("retrieve", j); - return *item->second; + return *it->second; } /* ************************************************************************* */ 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) throw ValuesKeyAlreadyExists(j); } /* ************************************************************************* */ void Values::insert(const Values& other) { - for (auto key_value = other.values_.begin(); - key_value != other.values_.end(); ++key_value) { - insert(key_value->first, *(key_value->second)); + for (const auto& [key, value] : other.values_) { + insert(key, *(value)); } } /* ************************************************************************* */ void Values::update(Key j, const Value& val) { // Find the value to update - KeyValueMap::iterator item = values_.find(j); - if (item == values_.end()) + KeyValueMap::iterator it = values_.find(j); + if (it == values_.end()) throw ValuesKeyDoesNotExist("update", j); // Cast to the derived type - const Value& old_value = *item->second; + const Value& old_value = *it->second; if (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) { - for (auto key_value = other.values_.begin(); - key_value != other.values_.end(); ++key_value) { - this->update(key_value->first, *(key_value->second)); + for (auto& [key, value] : other.values_) { + this->update(key, *(value)); } } @@ -204,34 +201,33 @@ namespace gtsam { /* ************************************************************************ */ void Values::insert_or_assign(const Values& other) { - for (auto key_value = other.values_.begin(); - key_value != other.values_.end(); ++key_value) { - this->insert_or_assign(key_value->first, *(key_value->second)); + for (auto& [key, value] : other.values_) { + this->insert_or_assign(key, *(value)); } } /* ************************************************************************* */ void Values::erase(Key j) { - KeyValueMap::iterator item = values_.find(j); - if(item == values_.end()) + KeyValueMap::iterator it = values_.find(j); + if(it == values_.end()) throw ValuesKeyDoesNotExist("erase", j); - values_.erase(item); + values_.erase(it); } /* ************************************************************************* */ KeyVector Values::keys() const { KeyVector result; result.reserve(size()); - for(const auto& key_value: values_) - result.push_back(key_value.first); + for(const auto& [key,value]: values_) + result.push_back(key); return result; } /* ************************************************************************* */ KeySet Values::keySet() const { KeySet result; - for(const auto& key_value: values_) - result.insert(key_value.first); + for(const auto& [key,value]: values_) + result.insert(key); return result; } @@ -245,8 +241,8 @@ namespace gtsam { /* ************************************************************************* */ size_t Values::dim() const { size_t result = 0; - for (const auto key_value : values_) { - result += key_value->second->dim(); + for (const auto& [key,value] : values_) { + result += value->dim(); } return result; } @@ -254,8 +250,8 @@ namespace gtsam { /* ************************************************************************* */ std::map Values::dims() const { std::map result; - for (const auto key_value : values_) { - result.emplace(key_value->first, key_value->second->dim()); + for (const auto& [key,value] : values_) { + result.emplace(key, value->dim()); } return result; } @@ -263,8 +259,8 @@ namespace gtsam { /* ************************************************************************* */ VectorValues Values::zeroVectors() const { VectorValues result; - for (const auto key_value : values_) - result.insert(key_value->first, Vector::Zero(key_value->second->dim())); + for (const auto& [key,value] : values_) + result.insert(key, Vector::Zero(value->dim())); return result; } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 172bfba89..ba0252c98 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -24,13 +24,17 @@ #pragma once +#include #include #include #include -#include -#include -#include +#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +#include +#endif + + +#include #include #include @@ -67,12 +71,9 @@ namespace gtsam { // user defines the allocation details (i.e. optimize for memory pool/arenas // concurrency). typedef internal::FastDefaultAllocator>::type KeyValuePtrPairAllocator; - typedef boost::ptr_map< - Key, - Value, - std::less, - ValueCloneAllocator, - KeyValuePtrPairAllocator > KeyValueMap; + using KeyValueMap = + std::map, std::less, + std::allocator>>>; // The member to store the values, see just above KeyValueMap values_; @@ -264,15 +265,8 @@ namespace gtsam { VectorValues zeroVectors() const; // Count values of given type \c ValueType - template - size_t count() const { - size_t i = 0; - for (const auto key_value : values_) { - if (dynamic_cast*>(key_value.second)) - ++i; - } - return i; - } + template + size_t count() const; /** * Extract a subset of values of the given type \c ValueType. diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 42e2d563c..02a3374e4 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -474,13 +474,13 @@ TEST(Values, std_move) { EXPECT_LONGS_EQUAL(2, (long)TestValueData::DestructorCount); EXPECT_LONGS_EQUAL(2, values.size()); 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(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(8, (long)TestValueData::ConstructorCount); - EXPECT_LONGS_EQUAL(8, (long)TestValueData::DestructorCount); + EXPECT_LONGS_EQUAL(6, (long)TestValueData::ConstructorCount); + EXPECT_LONGS_EQUAL(6, (long)TestValueData::DestructorCount); } }