HybridFactorGraph.h

Go to the documentation of this file.

 
#pragma once
 
#include <gtsam/discrete/DiscreteFactor.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 
#include <string>
 
#include "DCSAM_types.h"
#include "dcsam/DCFactor.h"
#include "dcsam/DCFactorGraph.h"
 
namespace dcsam {
 
class HybridFactorGraph {
 protected:
  // Separate internal factor graphs for different types of factors
  gtsam::NonlinearFactorGraph nonlinearGraph_;
  gtsam::DiscreteFactorGraph discreteGraph_;
  DCFactorGraph dcGraph_;
 
 public:
  HybridFactorGraph();
 
  template <typename NonlinearFactorType>
  void push_nonlinear(const NonlinearFactorType &nonlinearFactor) {
    nonlinearGraph_.push_back(
        boost::make_shared<NonlinearFactorType>(nonlinearFactor));
  }
 
  void push_nonlinear(
      boost::shared_ptr<gtsam::NonlinearFactor> nonlinearFactor);
 
  template <typename DiscreteFactorType>
  void push_discrete(const DiscreteFactorType &discreteFactor) {
    discreteGraph_.push_back(
        boost::make_shared<DiscreteFactorType>(discreteFactor));
  }
 
  void push_discrete(boost::shared_ptr<gtsam::DiscreteFactor> discreteFactor);
 
  template <typename DCFactorType>
  void push_dc(const DCFactorType &dcFactor) {
    dcGraph_.push_back(boost::make_shared<DCFactorType>(dcFactor));
  }
 
  void push_dc(boost::shared_ptr<DCFactor> dcFactor);
 
  void print(const std::string &str = "HybridFactorGraph",
             const gtsam::KeyFormatter &keyFormatter =
                 gtsam::DefaultKeyFormatter) const;
 
  gtsam::FastSet<gtsam::Key> keys() const;
 
  gtsam::NonlinearFactorGraph nonlinearGraph() const;
 
  gtsam::DiscreteFactorGraph discreteGraph() const;
 
  DCFactorGraph dcGraph() const;
 
  bool empty() const;
 
  bool equals(const HybridFactorGraph &other, double tol = 1e-9) const;
 
  size_t size() const;
 
  size_t size_nonlinear() const;
 
  size_t size_discrete() const;
 
  size_t size_dc() const;
 
  void clear();
};
 
}  // namespace dcsam