HybridNonlinearFactor.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/DiscreteValues.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridValues.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Symbol.h>
 
#include <vector>
 
namespace gtsam {

using NonlinearFactorValuePair =
  std::pair<NoiseModelFactor::shared_ptr, double>;

class GTSAM_EXPORT HybridNonlinearFactor : public HybridFactor {
 public:
  using Base = HybridFactor;
  using This = HybridNonlinearFactor;
  using shared_ptr = std::shared_ptr<HybridNonlinearFactor>;
  using sharedFactor = std::shared_ptr<NoiseModelFactor>;

  using FactorValuePairs = DecisionTree<Key, NonlinearFactorValuePair>;
 
 private:
  FactorValuePairs factors_;

  AlgebraicDecisionTree<Key> errorTree(
    const VectorValues& continuousValues) const override {
    throw std::runtime_error(
      "HybridNonlinearFactor::errorTree: use errorTree(gtsam::Values) for nonlinear values.");
  }
 
public:

  HybridNonlinearFactor() = default;

  HybridNonlinearFactor(
    const DiscreteKey& discreteKey,
    const std::vector<NoiseModelFactor::shared_ptr>& factors);

  HybridNonlinearFactor(const DiscreteKey& discreteKey,
                        const std::vector<NonlinearFactorValuePair>& pairs);

  HybridNonlinearFactor(const DiscreteKeys& discreteKeys,
                        const FactorValuePairs& factors);


  AlgebraicDecisionTree<Key> errorTree(const Values& continuousValues) const;

  double error(const Values& continuousValues,
               const DiscreteValues& assignment) const;

  double error(const HybridValues& hybridValues) const override;

  size_t dim() const;

  const FactorValuePairs& factors() const { return factors_; }

  GaussianFactor::shared_ptr linearize(const Values& continuousValues,
    const DiscreteValues& assignment) const;

  std::shared_ptr<HybridGaussianFactor> linearize(
    const Values& continuousValues) const;

  HybridNonlinearFactor::shared_ptr prune(
      const DecisionTreeFactor& discreteProbs) const;

  std::shared_ptr<Factor> restrict(
      const DiscreteValues& assignment) const override;


  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
    DefaultKeyFormatter) const override;

  bool equals(const HybridFactor& other, double tol = 1e-9) const override;

 
 private:
  struct ConstructorHelper;
 
  // Private constructor using ConstructorHelper above.
  HybridNonlinearFactor(const ConstructorHelper& helper);
 
#if GTSAM_ENABLE_BOOST_SERIALIZATION
  friend class boost::serialization::access;
  template <class Archive>
  void serialize(Archive& ar, const unsigned int /*version*/) {
    // Serialize base class
    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
    // Serialize derived class members
    ar& BOOST_SERIALIZATION_NVP(factors_);
  }
#endif
};
 
// traits
template <>
struct traits<HybridNonlinearFactor> : public Testable<HybridNonlinearFactor> {
};
 
}  // namespace gtsam