NonlinearFactorGraph.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
 
 * -------------------------------------------------------------------------- */
 
// \callgraph
 
#pragma once
 
#include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/nonlinear/PriorFactor.h>
 
#include <boost/shared_ptr.hpp>
#include <functional>
 
namespace gtsam {
 
  // Forward declarations
  class Values;
  class Ordering;
  class GaussianFactorGraph;
  class SymbolicFactorGraph;
  template<typename T>
  class Expression;
  template<typename T>
  class ExpressionFactor;
 
  struct GTSAM_EXPORT GraphvizFormatting {
    enum Axis { X, Y, Z, NEGX, NEGY, NEGZ }; 
    Axis paperHorizontalAxis; 
    Axis paperVerticalAxis; 
    double figureWidthInches; 
    double figureHeightInches; 
    double scale; 
    bool mergeSimilarFactors; 
    bool plotFactorPoints; 
    bool connectKeysToFactor; 
    bool binaryEdges; 
    std::map<size_t, Point2> factorPositions; 
    GraphvizFormatting() :
      paperHorizontalAxis(Y), paperVerticalAxis(X),
      figureWidthInches(5), figureHeightInches(5), scale(1),
      mergeSimilarFactors(false), plotFactorPoints(true),
      connectKeysToFactor(true), binaryEdges(true) {}
  };
 
 
  class GTSAM_EXPORT NonlinearFactorGraph: public FactorGraph<NonlinearFactor> {
 
  public:
 
    typedef FactorGraph<NonlinearFactor> Base;
    typedef NonlinearFactorGraph This;
    typedef boost::shared_ptr<This> shared_ptr;
 
    NonlinearFactorGraph() {}
 
    template<typename ITERATOR>
    NonlinearFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {}
 
    template<class CONTAINER>
    explicit NonlinearFactorGraph(const CONTAINER& factors) : Base(factors) {}
 
    template<class DERIVEDFACTOR>
    NonlinearFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
 
    virtual ~NonlinearFactorGraph() {}
 
    void print(
        const std::string& str = "NonlinearFactorGraph: ",
        const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
    void printErrors(const Values& values, const std::string& str = "NonlinearFactorGraph: ",
      const KeyFormatter& keyFormatter = DefaultKeyFormatter,
      const std::function<bool(const Factor* /*factor*/, double /*whitenedError*/, size_t /*index*/)>&
        printCondition = [](const Factor *,double, size_t) {return true;}) const;
 
    bool equals(const NonlinearFactorGraph& other, double tol = 1e-9) const;
 
    void saveGraph(std::ostream& stm, const Values& values = Values(),
      const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(),
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
 
    void saveGraph(const std::string& file, const Values& values = Values(),
      const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(),
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
 
    double error(const Values& values) const;
 
    double probPrime(const Values& values) const;
 
    boost::shared_ptr<SymbolicFactorGraph> symbolic() const;
 
    Ordering orderingCOLAMD() const;
 
    Ordering orderingCOLAMDConstrained(const FastMap<Key, int>& constraints) const;
 
    boost::shared_ptr<GaussianFactorGraph> linearize(const Values& linearizationPoint) const;
 
    typedef std::function<void(const boost::shared_ptr<HessianFactor>& hessianFactor)> Dampen;
 
    boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
        const Values& values, const Dampen& dampen = nullptr) const;
 
    boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
        const Values& values, const Ordering& ordering, const Dampen& dampen = nullptr) const;
 
    Values updateCholesky(const Values& values,
                          const Dampen& dampen = nullptr) const;
 
    Values updateCholesky(const Values& values, const Ordering& ordering,
                          const Dampen& dampen = nullptr) const;
 
    NonlinearFactorGraph clone() const;
 
    NonlinearFactorGraph rekey(const std::map<Key,Key>& rekey_mapping) const;
 
    template<typename T>
    void addExpressionFactor(const SharedNoiseModel& R, const T& z,
                             const Expression<T>& h) {
      push_back(boost::make_shared<ExpressionFactor<T> >(R, z, h));
    }
 
    template<typename T>
    void addPrior(Key key, const T& prior,
                  const SharedNoiseModel& model = nullptr) {
      emplace_shared<PriorFactor<T>>(key, prior, model);
    }
 
    template<typename T>
    void addPrior(Key key, const T& prior, const Matrix& covariance) {
      emplace_shared<PriorFactor<T>>(key, prior, covariance);
    }
 
  private:
 
    boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
        const Values& values, const Scatter& scatter, const Dampen& dampen = nullptr) const;
 
    friend class boost::serialization::access;
    template<class ARCHIVE>
    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
      ar & boost::serialization::make_nvp("NonlinearFactorGraph",
                boost::serialization::base_object<Base>(*this));
    }
 
  public:
 
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
    boost::shared_ptr<HessianFactor> GTSAM_DEPRECATED linearizeToHessianFactor(
        const Values& values, boost::none_t, const Dampen& dampen = nullptr) const
      {return linearizeToHessianFactor(values, dampen);}
 
    Values GTSAM_DEPRECATED updateCholesky(const Values& values, boost::none_t,
                          const Dampen& dampen = nullptr) const
      {return updateCholesky(values, dampen);}
#endif
 
  };
 
template<>
struct traits<NonlinearFactorGraph> : public Testable<NonlinearFactorGraph> {
};
 
} //\ namespace gtsam