diff --git a/gtsam/hybrid/HybridValues.h b/gtsam/hybrid/HybridValues.h index 4c4f5fa1e..3a0ad516a 100644 --- a/gtsam/hybrid/HybridValues.h +++ b/gtsam/hybrid/HybridValues.h @@ -37,12 +37,15 @@ namespace gtsam { */ class GTSAM_EXPORT HybridValues { private: - // VectorValue stored the continuous components of the HybridValues. + /// Continuous multi-dimensional vectors for \class GaussianFactor. VectorValues continuous_; - // DiscreteValue stored the discrete components of the HybridValues. + /// Discrete values for \class DiscreteFactor. DiscreteValues discrete_; + /// Continuous, differentiable manifold values for \class NonlinearFactor. + Values nonlinear_; + public: /// @name Standard Constructors /// @{ @@ -54,6 +57,11 @@ class GTSAM_EXPORT HybridValues { HybridValues(const VectorValues& cv, const DiscreteValues& dv) : continuous_(cv), discrete_(dv){}; + /// Construct from all values types. + HybridValues(const VectorValues& cv, const DiscreteValues& dv, + const Values& v) + : continuous_(cv), discrete_(dv), nonlinear_(v){}; + /// @} /// @name Testable /// @{ @@ -77,26 +85,30 @@ class GTSAM_EXPORT HybridValues { /// @name Interface /// @{ - /// Return the discrete MPE assignment - const DiscreteValues& discrete() const { return discrete_; } - - /// Return the delta update for the continuous vectors + /// Return the multi-dimensional vector values. const VectorValues& continuous() const { return continuous_; } - /// Check whether a variable with key \c j exists in DiscreteValue. - bool existsDiscrete(Key j) { return (discrete_.find(j) != discrete_.end()); }; + /// Return the discrete values. + const DiscreteValues& discrete() const { return discrete_; } - /// Check whether a variable with key \c j exists in VectorValue. + /// Return the nonlinear values. + const Values& nonlinear() const { return nonlinear_; } + + /// Check whether a variable with key \c j exists in VectorValues. bool existsVector(Key j) { return continuous_.exists(j); }; - /// Check whether a variable with key \c j exists. - bool exists(Key j) { return existsDiscrete(j) || existsVector(j); }; + /// Check whether a variable with key \c j exists in DiscreteValues. + bool existsDiscrete(Key j) { return (discrete_.find(j) != discrete_.end()); }; - /** 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; }; + /// Check whether a variable with key \c j exists in values. + bool existsNonlinear(Key j) { + return (nonlinear_.find(j) != nonlinear_.end()); + }; + + /// Check whether a variable with key \c j exists. + bool exists(Key j) { + return existsVector(j) || existsDiscrete(j) || existsNonlinear(j); + }; /** Insert a vector \c value with key \c j. Throws an invalid_argument * exception if the key \c j is already used. @@ -104,6 +116,12 @@ class GTSAM_EXPORT HybridValues { * @param j The index with which the value will be associated. */ void insert(Key j, const Vector& value) { continuous_.insert(j, 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; }; + /** 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) { @@ -118,28 +136,36 @@ class GTSAM_EXPORT HybridValues { return *this; } + /** 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; + } + /** 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; } // TODO(Shangjie)- insert_or_assign() , similar to Values.h - /** - * 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); }; - /** * 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); }; + /** + * 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); }; + /** 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.