Values.cpp

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/VectorValues.h>

#include <list>
#include <memory>
#include <sstream>

using namespace std;

namespace gtsam {

  /* ************************************************************************* */
  Values::Values(const Values& other) {
    this->insert(other);
  }

  /* ************************************************************************* */
  Values::Values(Values&& other) : values_(std::move(other.values_)) {
  }

  /* ************************************************************************* */
  Values::Values(std::initializer_list<ConstKeyValuePair> init) {
    for (const auto &kv : init)
      insert(kv.key, kv.value);
  }

  /* ************************************************************************* */
  Values::Values(const Values& other, const VectorValues& delta) {
    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(value->retract_(v));  // Retract
        values_.emplace(key, retractedValue);  // Add retracted result directly to result values
      } else {
        values_.emplace(key, value->clone_());  // Add original version to result values
      }
    }
  }

  /* ************************************************************************* */
  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->print("");
      cout << "\n";
    }
  }

  /* ************************************************************************* */
  bool Values::equals(const Values& other, double tol) const {
    if (this->size() != other.size())
      return false;
    for (auto it1 = values_.begin(), it2 = other.values_.begin();
         it1 != values_.end(); ++it1, ++it2) {
      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;
      }
    }
    return true; // We return false earlier if we find anything that does not match
}

  /* ************************************************************************* */
  bool Values::exists(Key j) const {
    return values_.find(j) != values_.end();
  }

  /* ************************************************************************* */
  Values Values::retract(const VectorValues& delta) const {
    return Values(*this, delta);
  }

  /* ************************************************************************* */
  void Values::retractMasked(const VectorValues& delta, const KeySet& mask) {
    gttic(retractMasked);
    assert(this->size() == delta.size());
    auto key_value = values_.begin();
    VectorValues::const_iterator key_delta;
#ifdef GTSAM_USE_TBB
    for (; key_value != values_.end(); ++key_value) {
      key_delta = delta.find(key_value->first);
#else
    for (key_delta = delta.begin(); key_value != values_.end();
         ++key_value, ++key_delta) {
      assert(key_value->first == key_delta->first);
#endif
      Key var = key_value->first;
      assert(static_cast<size_t>(delta[var].size()) == key_value->second->dim());
      assert(delta[var].allFinite());
      if (mask.exists(var)) {
        Value* retracted = key_value->second->retract_(delta[var]);
        // TODO(dellaert): can we use std::move here?
        *(key_value->second) = *retracted;
        retracted->deallocate_();
      }
    }
  }

  /* ************************************************************************* */
  VectorValues Values::localCoordinates(const Values& cp) const {
    if(this->size() != cp.size())
      throw DynamicValuesMismatched();
    VectorValues result;
    for (auto it1 = values_.begin(), it2 = cp.values_.begin();
         it1 != values_.end(); ++it1, ++it2) {
      if(it1->first != it2->first)
        throw DynamicValuesMismatched(); // If keys do not match
      // Will throw a dynamic_cast exception if types do not match
      // NOTE: this is separate from localCoordinates(cp, ordering, result) due to at() vs. insert
      result.insert(it1->first, it1->second->localCoordinates_(*it2->second));
    }
    return result;
  }

  /* ************************************************************************* */
  const Value& Values::at(Key j) const {
    KeyValueMap::const_iterator it = values_.find(j);

    // Throw exception if it does not exist
    if(it == values_.end())
      throw ValuesKeyDoesNotExist("retrieve", j);
    return *it->second;
  }

  /* ************************************************************************* */
  void Values::insert(Key j, const Value& val) {
    auto insertResult = values_.emplace(j, val.clone_());
    if(!insertResult.second)
      throw ValuesKeyAlreadyExists(j);
  }

  /* ************************************************************************* */
  void Values::insert(const Values& other) {
    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 it = values_.find(j);
    if (it == values_.end())
      throw ValuesKeyDoesNotExist("update", j);

    // Cast to the derived type
    const Value& old_value = *it->second;
    if (typeid(old_value) != typeid(val))
      throw ValuesIncorrectType(j, typeid(old_value), typeid(val));

    values_.erase(j);
    values_.emplace(j, val.clone_());
  }

  /* ************************************************************************* */
  void Values::update(const Values& other) {
    for (auto& [key, value] : other.values_) {
      this->update(key, *(value));
    }
  }

  /* ************************************************************************ */
  void Values::insert_or_assign(Key j, const Value& val) {
    if (this->exists(j)) {
      // If key already exists, perform an update.
      this->update(j, val);
    } else {
      // If key does not exist, perform an insert.
      this->insert(j, val);
    }
  }

  /* ************************************************************************ */
  void Values::insert_or_assign(const Values& other) {
    for (auto& [key, value] : other.values_) {
      this->insert_or_assign(key, *(value));
    }
  }

  /* ************************************************************************* */
  void Values::erase(Key j) {
    KeyValueMap::iterator it = values_.find(j);
    if(it == values_.end())
      throw ValuesKeyDoesNotExist("erase", j);
    values_.erase(it);
  }

  /* ************************************************************************* */
  KeyVector Values::keys() const {
    KeyVector result;
    result.reserve(size());
    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);
    return result;
  }

  /* ************************************************************************* */
  Values& Values::operator=(const Values& rhs) {
    this->clear();
    this->insert(rhs);
    return *this;
  }

  /* ************************************************************************* */
  size_t Values::dim() const {
    size_t result = 0;
    for (const auto& [key,value] : values_) {
      result += value->dim();
    }
    return result;
  }

  /* ************************************************************************* */
  std::map<Key,size_t> Values::dims() const {
    std::map<Key,size_t> result;
    for (const auto& [key,value] : values_) {
      result.emplace(key, value->dim());
    }
    return result;
  }

  /* ************************************************************************* */
  VectorValues Values::zeroVectors() const {
    VectorValues result;
    for (const auto& [key,value] : values_)
      result.insert(key, Vector::Zero(value->dim()));
    return result;
  }

  /* ************************************************************************* */
  const char* ValuesKeyAlreadyExists::what() const noexcept {
    if(message_.empty())
      message_ =
          "Attempting to add a key-value pair with key \"" + DefaultKeyFormatter(key_) + "\", key already exists.";
    return message_.c_str();
  }

  /* ************************************************************************* */
  const char* ValuesKeyDoesNotExist::what() const noexcept {
    if(message_.empty())
      message_ =
          "Attempting to " + std::string(operation_) + " the key \"" +
          DefaultKeyFormatter(key_) + "\", which does not exist in the Values.";
    return message_.c_str();
  }

  /* ************************************************************************* */
  const char* ValuesIncorrectType::what() const noexcept {
    if(message_.empty()) {
      std::string storedTypeName = demangle(storedTypeId_.name());
      std::string requestedTypeName = demangle(requestedTypeId_.name());

      if (storedTypeName == requestedTypeName) {
        message_ = "WARNING: Detected types with same name but different `typeid`. \
          This is usually caused by incorrect linking/inlining settings when compiling libraries using GTSAM. \
          If you are a user, please report to the author of the library using GTSAM. \
          If you are a package maintainer, please consult `cmake/GtsamPybindWrap.cmake`, line 74 for details.";
      } else {
        message_ =
        "Attempting to retrieve value with key \"" + DefaultKeyFormatter(key_) + "\", type stored in Values is " +
         storedTypeName + " but requested type was " + requestedTypeName;
      }
    }
    return message_.c_str();
  }

  /* ************************************************************************* */
  const char* NoMatchFoundForFixed::what() const noexcept {
    if(message_.empty()) {
      ostringstream oss;
    oss
        << "Attempting to retrieve fixed-size matrix with dimensions " //
        << M1_ << "x" << N1_
        << ", but found dynamic Matrix with mismatched dimensions " //
        << M2_ << "x" << N2_;
      message_ = oss.str();
    }
    return message_.c_str();
  }

}