Values.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
25 #include <gtsam/nonlinear/Values.h>
27 
28 #include <list>
29 #include <memory>
30 #include <sstream>
31 
32 using namespace std;
33 
34 namespace gtsam {
35 
36  /* ************************************************************************* */
38  this->insert(other);
39  }
40 
41  /* ************************************************************************* */
42  Values::Values(Values&& other) : values_(std::move(other.values_)) {
43  }
44 
45  /* ************************************************************************* */
46  Values::Values(std::initializer_list<ConstKeyValuePair> init) {
47  for (const auto &kv : init)
48  insert(kv.key, kv.value);
49  }
50 
51  /* ************************************************************************* */
53  for (const auto& [key,value] : other.values_) {
55  if (it != delta.end()) {
56  const Vector& v = it->second;
57  Value* retractedValue(value->retract_(v)); // Retract
58  values_.emplace(key, retractedValue); // Add retracted result directly to result values
59  } else {
60  values_.emplace(key, value->clone_()); // Add original version to result values
61  }
62  }
63  }
64 
65  /* ************************************************************************* */
66  void Values::print(const string& str, const KeyFormatter& keyFormatter) const {
67  cout << str << (str.empty() ? "" : "\n");
68  cout << "Values with " << size() << " values:\n";
69  for (const auto& [key,value] : values_) {
70  cout << "Value " << keyFormatter(key) << ": ";
71  value->print("");
72  cout << "\n";
73  }
74  }
75 
76  /* ************************************************************************* */
77  bool Values::equals(const Values& other, double tol) const {
78  if (this->size() != other.size())
79  return false;
80  for (auto it1 = values_.begin(), it2 = other.values_.begin();
81  it1 != values_.end(); ++it1, ++it2) {
82  const Value* value1 = it1->second.get();
83  const Value* value2 = it2->second.get();
84  if (typeid(*value1) != typeid(*value2) || it1->first != it2->first
85  || !value1->equals_(*value2, tol)) {
86  return false;
87  }
88  }
89  return true; // We return false earlier if we find anything that does not match
90 }
91 
92  /* ************************************************************************* */
93  bool Values::exists(Key j) const {
94  return values_.find(j) != values_.end();
95  }
96 
97  /* ************************************************************************* */
99  return Values(*this, delta);
100  }
101 
102  /* ************************************************************************* */
103  void Values::retractMasked(const VectorValues& delta, const KeySet& mask) {
105  assert(this->size() == delta.size());
106  auto key_value = values_.begin();
108 #ifdef GTSAM_USE_TBB
109  for (; key_value != values_.end(); ++key_value) {
110  key_delta = delta.find(key_value->first);
111 #else
112  for (key_delta = delta.begin(); key_value != values_.end();
113  ++key_value, ++key_delta) {
114  assert(key_value->first == key_delta->first);
115 #endif
116  Key var = key_value->first;
117  assert(static_cast<size_t>(delta[var].size()) == key_value->second->dim());
118  assert(delta[var].allFinite());
119  if (mask.exists(var)) {
120  Value* retracted = key_value->second->retract_(delta[var]);
121  // TODO(dellaert): can we use std::move here?
122  *(key_value->second) = *retracted;
123  retracted->deallocate_();
124  }
125  }
126  }
127 
128  /* ************************************************************************* */
130  if(this->size() != cp.size())
131  throw DynamicValuesMismatched();
133  for (auto it1 = values_.begin(), it2 = cp.values_.begin();
134  it1 != values_.end(); ++it1, ++it2) {
135  if(it1->first != it2->first)
136  throw DynamicValuesMismatched(); // If keys do not match
137  // Will throw a dynamic_cast exception if types do not match
138  // NOTE: this is separate from localCoordinates(cp, ordering, result) due to at() vs. insert
139  result.insert(it1->first, it1->second->localCoordinates_(*it2->second));
140  }
141  return result;
142  }
143 
144  /* ************************************************************************* */
145  const Value& Values::at(Key j) const {
146  KeyValueMap::const_iterator it = values_.find(j);
147 
148  // Throw exception if it does not exist
149  if(it == values_.end())
150  throw ValuesKeyDoesNotExist("retrieve", j);
151  return *it->second;
152  }
153 
154  /* ************************************************************************* */
155  void Values::insert(Key j, const Value& val) {
156  auto insertResult = values_.emplace(j, val.clone_());
157  if(!insertResult.second)
158  throw ValuesKeyAlreadyExists(j);
159  }
160 
161  /* ************************************************************************* */
162  void Values::insert(const Values& other) {
163  for (const auto& [key, value] : other.values_) {
164  insert(key, *(value));
165  }
166  }
167 
168  /* ************************************************************************* */
169  void Values::update(Key j, const Value& val) {
170  // Find the value to update
171  KeyValueMap::iterator it = values_.find(j);
172  if (it == values_.end())
173  throw ValuesKeyDoesNotExist("update", j);
174 
175  // Cast to the derived type
176  const Value& old_value = *it->second;
177  if (typeid(old_value) != typeid(val))
178  throw ValuesIncorrectType(j, typeid(old_value), typeid(val));
179 
180  values_.erase(j);
181  values_.emplace(j, val.clone_());
182  }
183 
184  /* ************************************************************************* */
185  void Values::update(const Values& other) {
186  for (auto& [key, value] : other.values_) {
187  this->update(key, *(value));
188  }
189  }
190 
191  /* ************************************************************************ */
192  void Values::insert_or_assign(Key j, const Value& val) {
193  if (this->exists(j)) {
194  // If key already exists, perform an update.
195  this->update(j, val);
196  } else {
197  // If key does not exist, perform an insert.
198  this->insert(j, val);
199  }
200  }
201 
202  /* ************************************************************************ */
204  for (auto& [key, value] : other.values_) {
205  this->insert_or_assign(key, *(value));
206  }
207  }
208 
209  /* ************************************************************************* */
211  KeyValueMap::iterator it = values_.find(j);
212  if(it == values_.end())
213  throw ValuesKeyDoesNotExist("erase", j);
214  values_.erase(it);
215  }
216 
217  /* ************************************************************************* */
220  result.reserve(size());
221  for(const auto& [key,value]: values_)
222  result.push_back(key);
223  return result;
224  }
225 
226  /* ************************************************************************* */
228  KeySet result;
229  for(const auto& [key,value]: values_)
230  result.insert(key);
231  return result;
232  }
233 
234  /* ************************************************************************* */
236  this->clear();
237  this->insert(rhs);
238  return *this;
239  }
240 
241  /* ************************************************************************* */
242  size_t Values::dim() const {
243  size_t result = 0;
244  for (const auto& [key,value] : values_) {
245  result += value->dim();
246  }
247  return result;
248  }
249 
250  /* ************************************************************************* */
251  std::map<Key,size_t> Values::dims() const {
252  std::map<Key,size_t> result;
253  for (const auto& [key,value] : values_) {
254  result.emplace(key, value->dim());
255  }
256  return result;
257  }
258 
259  /* ************************************************************************* */
262  for (const auto& [key,value] : values_)
263  result.insert(key, Vector::Zero(value->dim()));
264  return result;
265  }
266 
267  /* ************************************************************************* */
268  const char* ValuesKeyAlreadyExists::what() const noexcept {
269  if(message_.empty())
270  message_ =
271  "Attempting to add a key-value pair with key \"" + DefaultKeyFormatter(key_) + "\", key already exists.";
272  return message_.c_str();
273  }
274 
275  /* ************************************************************************* */
276  const char* ValuesKeyDoesNotExist::what() const noexcept {
277  if(message_.empty())
278  message_ =
279  "Attempting to " + std::string(operation_) + " the key \"" +
280  DefaultKeyFormatter(key_) + "\", which does not exist in the Values.";
281  return message_.c_str();
282  }
283 
284  /* ************************************************************************* */
285  const char* ValuesIncorrectType::what() const noexcept {
286  if(message_.empty()) {
287  std::string storedTypeName = demangle(storedTypeId_.name());
288  std::string requestedTypeName = demangle(requestedTypeId_.name());
289 
290  if (storedTypeName == requestedTypeName) {
291  message_ = "WARNING: Detected types with same name but different `typeid`. \
292  This is usually caused by incorrect linking/inlining settings when compiling libraries using GTSAM. \
293  If you are a user, please report to the author of the library using GTSAM. \
294  If you are a package maintainer, please consult `cmake/GtsamPybindWrap.cmake`, line 74 for details.";
295  } else {
296  message_ =
297  "Attempting to retrieve value with key \"" + DefaultKeyFormatter(key_) + "\", type stored in Values is " +
298  storedTypeName + " but requested type was " + requestedTypeName;
299  }
300  }
301  return message_.c_str();
302  }
303 
304  /* ************************************************************************* */
305  const char* NoMatchFoundForFixed::what() const noexcept {
306  if(message_.empty()) {
307  ostringstream oss;
308  oss
309  << "Attempting to retrieve fixed-size matrix with dimensions " //
310  << M1_ << "x" << N1_
311  << ", but found dynamic Matrix with mismatched dimensions " //
312  << M2_ << "x" << N2_;
313  message_ = oss.str();
314  }
315  return message_.c_str();
316  }
317 
318 }
gtsam::Value::equals_
virtual bool equals_(const Value &other, double tol=1e-9) const =0
gtsam::Values::exists
bool exists(Key j) const
Definition: Values.cpp:93
gtsam::Values::dims
std::map< Key, size_t > dims() const
Definition: Values.cpp:251
gtsam::Values::keys
KeyVector keys() const
Definition: Values.cpp:218
gtsam::DynamicValuesMismatched
Definition: Values.h:484
gtsam::Values::size
size_t size() const
Definition: Values.h:178
gtsam::ValuesIncorrectType
Definition: Values.h:453
gtsam::FastSet::exists
bool exists(const VALUE &e) const
Definition: FastSet.h:98
gtsam::Values::retractMasked
void retractMasked(const VectorValues &delta, const KeySet &mask)
Definition: Values.cpp:103
gtsam::Value::deallocate_
virtual void deallocate_() const =0
gtsam::FastSet< Key >
gtsam::Values::update
void update(Key j, const Value &val)
Definition: Values.cpp:169
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::Values::values_
KeyValueMap values_
Definition: Values.h:79
gtsam::Values::Values
Values()=default
gtsam::Values::localCoordinates
VectorValues localCoordinates(const Values &cp) const
Definition: Values.cpp:129
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::VectorValues::const_iterator
Values::const_iterator const_iterator
Const iterator over vector values.
Definition: VectorValues.h:85
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
gtsam::Values::retract
Values retract(const VectorValues &delta) const
Definition: Values.cpp:98
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
gtsam::Value::clone_
virtual Value * clone_() const =0
gtsam::VectorValues
Definition: VectorValues.h:74
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
gtsam::ValuesKeyDoesNotExist::what
const GTSAM_EXPORT char * what() const noexcept override
The message to be displayed to the user.
Definition: Values.cpp:276
gtsam::Value
Definition: Value.h:39
gtsam::Values::dim
size_t dim() const
Definition: Values.cpp:242
gtsam::Values::equals
bool equals(const Values &other, double tol=1e-9) const
Definition: Values.cpp:77
gtsam::NoMatchFoundForFixed::what
const GTSAM_EXPORT char * what() const noexcept override
Definition: Values.cpp:305
VectorValues.h
Factor Graph Values.
gtsam::ValuesKeyAlreadyExists
Definition: Values.h:408
str
Definition: pytypes.h:1558
key
const gtsam::Symbol key('X', 0)
gtsam::Values::zeroVectors
VectorValues zeroVectors() const
Definition: Values.cpp:260
move
detail::enable_if_t<!detail::move_never< T >::value, T > move(object &&obj)
Definition: cast.h:1243
gtsam::Values::keySet
KeySet keySet() const
Definition: Values.cpp:227
Values
std::vector< float > Values
Definition: sparse_setter.cpp:45
gtsam::Values::print
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Values.cpp:66
gtsam
traits
Definition: SFMdata.h:40
gtsam::Values
Definition: Values.h:65
gtsam::ValuesKeyAlreadyExists::what
const GTSAM_EXPORT char * what() const noexcept override
The message to be displayed to the user.
Definition: Values.cpp:268
gtsam::demangle
std::string demangle(const char *name)
Pretty print Value type name.
Definition: types.cpp:37
std
Definition: BFloat16.h:88
gtsam::Values::clear
void clear()
Definition: Values.h:347
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::VectorValues::size
size_t size() const
Definition: VectorValues.h:130
gtsam::ValuesKeyDoesNotExist
Definition: Values.h:430
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Values::erase
void erase(Key j)
Definition: Values.cpp:210
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::ValuesIncorrectType::what
const GTSAM_EXPORT char * what() const noexcept override
The message to be displayed to the user.
Definition: Values.cpp:285
insert
A insert(1, 2)=0
init
Definition: TutorialInplaceLU.cpp:2
gtsam::Values::operator=
Values & operator=(const Values &rhs)
Definition: Values.cpp:235
gtsam::Value::retract_
virtual Value * retract_(const Vector &delta) const =0
test_callbacks.value
value
Definition: test_callbacks.py:160
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
Values.h
A non-templated config holding any types of Manifold-group elements.
gttic
#define gttic(label)
Definition: timing.h:295
gtsam::Values::insert_or_assign
void insert_or_assign(Key j, const Value &val)
If key j exists, update value, else perform an insert.
Definition: Values.cpp:192


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:43:08