From b54ed7209eccca587bf8b77c33925cd5ecae0081 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 25 Aug 2024 13:51:14 -0400 Subject: [PATCH] split HybridValues into .h and .cpp --- gtsam/hybrid/HybridValues.cpp | 167 ++++++++++++++++++++++++++++++++++ gtsam/hybrid/HybridValues.h | 106 ++++++--------------- 2 files changed, 196 insertions(+), 77 deletions(-) create mode 100644 gtsam/hybrid/HybridValues.cpp diff --git a/gtsam/hybrid/HybridValues.cpp b/gtsam/hybrid/HybridValues.cpp new file mode 100644 index 000000000..1fb2a2adb --- /dev/null +++ b/gtsam/hybrid/HybridValues.cpp @@ -0,0 +1,167 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * @file HybridValues.cpp + * @author Varun Agrawal + * @date August 2024 + */ + +#include +#include +#include +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +HybridValues::HybridValues(const VectorValues& cv, const DiscreteValues& dv) + : continuous_(cv), discrete_(dv) {} + +/* ************************************************************************* */ +HybridValues::HybridValues(const VectorValues& cv, const DiscreteValues& dv, + const Values& v) + : continuous_(cv), discrete_(dv), nonlinear_(v) {} + +/* ************************************************************************* */ +void HybridValues::print(const std::string& s, + const KeyFormatter& keyFormatter) const { + std::cout << s << ": \n"; + continuous_.print(" Continuous", + keyFormatter); // print continuous components + discrete_.print(" Discrete", keyFormatter); // print discrete components +} + +/* ************************************************************************* */ +bool HybridValues::equals(const HybridValues& other, double tol) const { + return continuous_.equals(other.continuous_, tol) && + discrete_.equals(other.discrete_, tol); +} + +/* ************************************************************************* */ +const VectorValues& HybridValues::continuous() const { return continuous_; } + +/* ************************************************************************* */ +const DiscreteValues& HybridValues::discrete() const { return discrete_; } + +/* ************************************************************************* */ +const Values& HybridValues::nonlinear() const { return nonlinear_; } + +/* ************************************************************************* */ +bool HybridValues::existsVector(Key j) { return continuous_.exists(j); } + +/* ************************************************************************* */ +bool HybridValues::existsDiscrete(Key j) { + return (discrete_.find(j) != discrete_.end()); +} + +/* ************************************************************************* */ +bool HybridValues::existsNonlinear(Key j) { return nonlinear_.exists(j); } + +/* ************************************************************************* */ +bool HybridValues::exists(Key j) { + return existsVector(j) || existsDiscrete(j) || existsNonlinear(j); +} + +/* ************************************************************************* */ +HybridValues HybridValues::retract(const VectorValues& delta) const { + HybridValues updated(continuous_, discrete_, nonlinear_.retract(delta)); + return updated; +} + +/* ************************************************************************* */ +void HybridValues::insert(Key j, const Vector& value) { + continuous_.insert(j, value); +} + +/* ************************************************************************* */ +void HybridValues::insert(Key j, size_t value) { discrete_[j] = value; } + +/* ************************************************************************* */ +void HybridValues::insert_or_assign(Key j, const Vector& value) { + continuous_.insert_or_assign(j, value); +} + +/* ************************************************************************* */ +void HybridValues::insert_or_assign(Key j, size_t value) { + discrete_[j] = value; +} + +/* ************************************************************************* */ +HybridValues& HybridValues::insert(const VectorValues& values) { + continuous_.insert(values); + return *this; +} + +/* ************************************************************************* */ +HybridValues& HybridValues::insert(const DiscreteValues& values) { + discrete_.insert(values); + return *this; +} + +/* ************************************************************************* */ +HybridValues& HybridValues::insert(const Values& values) { + nonlinear_.insert(values); + return *this; +} + +/* ************************************************************************* */ +HybridValues& HybridValues::insert(const HybridValues& values) { + continuous_.insert(values.continuous()); + discrete_.insert(values.discrete()); + nonlinear_.insert(values.nonlinear()); + return *this; +} + +/* ************************************************************************* */ +Vector& HybridValues::at(Key j) { return continuous_.at(j); } + +/* ************************************************************************* */ +size_t& HybridValues::atDiscrete(Key j) { return discrete_.at(j); } + +/* ************************************************************************* */ +HybridValues& HybridValues::update(const VectorValues& values) { + continuous_.update(values); + return *this; +} + +/* ************************************************************************* */ +HybridValues& HybridValues::update(const DiscreteValues& values) { + discrete_.update(values); + return *this; +} + +/* ************************************************************************* */ +HybridValues& HybridValues::update(const HybridValues& values) { + continuous_.update(values.continuous()); + discrete_.update(values.discrete()); + return *this; +} + +/* ************************************************************************* */ +VectorValues HybridValues::continuousSubset(const KeyVector& keys) const { + VectorValues measurements; + for (const auto& key : keys) { + measurements.insert(key, continuous_.at(key)); + } + return measurements; +} + +/* ************************************************************************* */ +std::string HybridValues::html(const KeyFormatter& keyFormatter) const { + std::stringstream ss; + ss << this->continuous_.html(keyFormatter); + ss << this->discrete_.html(keyFormatter); + return ss.str(); +} + +} // namespace gtsam diff --git a/gtsam/hybrid/HybridValues.h b/gtsam/hybrid/HybridValues.h index 09e7d773c..07ff3e9e3 100644 --- a/gtsam/hybrid/HybridValues.h +++ b/gtsam/hybrid/HybridValues.h @@ -18,8 +18,6 @@ #pragma once -#include -#include #include #include #include @@ -55,13 +53,11 @@ class GTSAM_EXPORT HybridValues { HybridValues() = default; /// Construct from DiscreteValues and VectorValues. - HybridValues(const VectorValues& cv, const DiscreteValues& dv) - : continuous_(cv), discrete_(dv) {} + HybridValues(const VectorValues& cv, const DiscreteValues& dv); /// Construct from all values types. HybridValues(const VectorValues& cv, const DiscreteValues& dv, - const Values& v) - : continuous_(cv), discrete_(dv), nonlinear_(v) {} + const Values& v); /// @} /// @name Testable @@ -69,144 +65,105 @@ class GTSAM_EXPORT HybridValues { /// print required by Testable for unit testing void print(const std::string& s = "HybridValues", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << ": \n"; - continuous_.print(" Continuous", - keyFormatter); // print continuous components - discrete_.print(" Discrete", keyFormatter); // print discrete components - } + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /// equals required by Testable for unit testing - bool equals(const HybridValues& other, double tol = 1e-9) const { - return continuous_.equals(other.continuous_, tol) && - discrete_.equals(other.discrete_, tol); - } + bool equals(const HybridValues& other, double tol = 1e-9) const; /// @} /// @name Interface /// @{ /// Return the multi-dimensional vector values. - const VectorValues& continuous() const { return continuous_; } + const VectorValues& continuous() const; /// Return the discrete values. - const DiscreteValues& discrete() const { return discrete_; } + const DiscreteValues& discrete() const; /// Return the nonlinear values. - const Values& nonlinear() const { return nonlinear_; } + const Values& nonlinear() const; /// Check whether a variable with key \c j exists in VectorValues. - bool existsVector(Key j) { return continuous_.exists(j); } + bool existsVector(Key j); /// Check whether a variable with key \c j exists in DiscreteValues. - bool existsDiscrete(Key j) { return (discrete_.find(j) != discrete_.end()); } + bool existsDiscrete(Key j); /// Check whether a variable with key \c j exists in values. - bool existsNonlinear(Key j) { return nonlinear_.exists(j); } + bool existsNonlinear(Key j); /// Check whether a variable with key \c j exists. - bool exists(Key j) { - return existsVector(j) || existsDiscrete(j) || existsNonlinear(j); - } + bool exists(Key j); + + /** Add a delta config to current config and returns a new config */ + HybridValues retract(const VectorValues& delta) const; /** Insert a vector \c value with key \c j. Throws an invalid_argument * exception if the key \c j is already used. * @param value The vector to be inserted. * @param j The index with which the value will be associated. */ - void insert(Key j, const Vector& value) { continuous_.insert(j, value); } + void insert(Key j, const Vector& value); /** Insert a discrete \c value with key \c j. Replaces the existing value if * the key \c j is already used. * @param value The vector to be inserted. * @param j The index with which the value will be associated. */ - void insert(Key j, size_t value) { discrete_[j] = value; } + void insert(Key j, size_t value); /// insert_or_assign() , similar to Values.h - void insert_or_assign(Key j, const Vector& value) { - continuous_.insert_or_assign(j, value); - } + void insert_or_assign(Key j, const Vector& value); /// insert_or_assign() , similar to Values.h - void insert_or_assign(Key j, size_t value) { discrete_[j] = value; } + void insert_or_assign(Key j, size_t value); /** Insert all continuous values from \c values. Throws an invalid_argument * exception if any keys to be inserted are already used. */ - HybridValues& insert(const VectorValues& values) { - continuous_.insert(values); - return *this; - } + HybridValues& insert(const VectorValues& values); /** Insert all discrete values from \c values. Throws an invalid_argument * exception if any keys to be inserted are already used. */ - HybridValues& insert(const DiscreteValues& values) { - discrete_.insert(values); - return *this; - } + HybridValues& insert(const DiscreteValues& values); /** Insert all values from \c values. Throws an invalid_argument * exception if any keys to be inserted are already used. */ - HybridValues& insert(const Values& values) { - nonlinear_.insert(values); - return *this; - } + HybridValues& insert(const Values& values); /** Insert all values from \c values. Throws an invalid_argument exception if * any keys to be inserted are already used. */ - HybridValues& insert(const HybridValues& values) { - continuous_.insert(values.continuous()); - discrete_.insert(values.discrete()); - nonlinear_.insert(values.nonlinear()); - return *this; - } + HybridValues& insert(const HybridValues& values); /** * Read/write access to the vector value with key \c j, throws * std::out_of_range if \c j does not exist. */ - Vector& at(Key j) { return continuous_.at(j); } + Vector& at(Key j); /** * Read/write access to the discrete value with key \c j, throws * std::out_of_range if \c j does not exist. */ - size_t& atDiscrete(Key j) { return discrete_.at(j); } + size_t& atDiscrete(Key j); /** For all key/value pairs in \c values, replace continuous values with * corresponding keys in this object with those in \c values. Throws * std::out_of_range if any keys in \c values are not present in this object. */ - HybridValues& update(const VectorValues& values) { - continuous_.update(values); - return *this; - } + HybridValues& update(const VectorValues& values); /** For all key/value pairs in \c values, replace discrete values with * corresponding keys in this object with those in \c values. Throws * std::out_of_range if any keys in \c values are not present in this object. */ - HybridValues& update(const DiscreteValues& values) { - discrete_.update(values); - return *this; - } + HybridValues& update(const DiscreteValues& values); /** For all key/value pairs in \c values, replace all values with * corresponding keys in this object with those in \c values. Throws * std::out_of_range if any keys in \c values are not present in this object. */ - HybridValues& update(const HybridValues& values) { - continuous_.update(values.continuous()); - discrete_.update(values.discrete()); - return *this; - } + HybridValues& update(const HybridValues& values); /// Extract continuous values with given keys. - VectorValues continuousSubset(const KeyVector& keys) const { - VectorValues measurements; - for (const auto& key : keys) { - measurements.insert(key, continuous_.at(key)); - } - return measurements; - } + VectorValues continuousSubset(const KeyVector& keys) const; /// @} /// @name Wrapper support @@ -219,12 +176,7 @@ class GTSAM_EXPORT HybridValues { * @return string html output. */ std::string html( - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::stringstream ss; - ss << this->continuous_.html(keyFormatter); - ss << this->discrete_.html(keyFormatter); - return ss.str(); - } + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /// @} };