HybridValues.h

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
 
 * -------------------------------------------------------------------------- */
 
#pragma once
 
#include <gtsam/discrete/Assignment.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/inference/Key.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/Values.h>
 
#include <map>
#include <string>
#include <vector>
 
namespace gtsam {
 
class GTSAM_EXPORT HybridValues {
 private:
  VectorValues continuous_;
 
  DiscreteValues discrete_;
 
  Values nonlinear_;
 
 public:
 
  HybridValues() = default;
 
  HybridValues(const VectorValues &cv, const DiscreteValues &dv)
      : continuous_(cv), discrete_(dv){}
 
  HybridValues(const VectorValues& cv, const DiscreteValues& dv,
               const Values& v)
      : continuous_(cv), discrete_(dv), nonlinear_(v){}
 
 
  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
  }
 
  bool equals(const HybridValues& other, double tol = 1e-9) const {
    return continuous_.equals(other.continuous_, tol) &&
           discrete_.equals(other.discrete_, tol);
  }
 
 
  const VectorValues& continuous() const { return continuous_; }
 
  const DiscreteValues& discrete() const { return discrete_; }
 
  const Values& nonlinear() const { return nonlinear_; }
 
  bool existsVector(Key j) { return continuous_.exists(j); }
 
  bool existsDiscrete(Key j) { return (discrete_.find(j) != discrete_.end()); }
 
  bool existsNonlinear(Key j) {
    return nonlinear_.exists(j);
  }
 
  bool exists(Key j) {
    return existsVector(j) || existsDiscrete(j) || existsNonlinear(j);
  }
 
  void insert(Key j, const Vector& value) { continuous_.insert(j, value); }
 
  void insert(Key j, size_t value) { discrete_[j] = value; }
 
  void insert_or_assign(Key j, const Vector& value) {
    continuous_.insert_or_assign(j, value);
  }
 
  void insert_or_assign(Key j, size_t value) {
    discrete_[j] = value;
  }
 
  HybridValues& insert(const VectorValues& values) {
    continuous_.insert(values);
    return *this;
  }
 
  HybridValues& insert(const DiscreteValues& values) {
    discrete_.insert(values);
    return *this;
  }
 
  HybridValues& insert(const Values& values) {
    nonlinear_.insert(values);
    return *this;
  }
 
  HybridValues& insert(const HybridValues& values) {
    continuous_.insert(values.continuous());
    discrete_.insert(values.discrete());
    nonlinear_.insert(values.nonlinear());
    return *this;
  }
 
  Vector& at(Key j) { return continuous_.at(j); }
 
  size_t& atDiscrete(Key j) { return discrete_.at(j); }
 
  HybridValues& update(const VectorValues& values) {
    continuous_.update(values);
    return *this;
  }
 
  HybridValues& update(const DiscreteValues& values) {
    discrete_.update(values);
    return *this;
  }
 
  HybridValues& update(const HybridValues& values) {
    continuous_.update(values.continuous());
    discrete_.update(values.discrete());
    return *this;
  }
 
  VectorValues continuousSubset(const KeyVector& keys) const {
    VectorValues measurements;
    for (const auto& key : keys) {
      measurements.insert(key, continuous_.at(key));
    }
    return measurements;
  }
 
 
  std::string html(
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
    std::stringstream ss;
    ss << this->continuous_.html(keyFormatter);
    ss << this->discrete_.html(keyFormatter);
    return ss.str();
  }
 
};
 
// traits
template <>
struct traits<HybridValues> : public Testable<HybridValues> {};
 
}  // namespace gtsam