Added nonlinear Values

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.