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>

namespace gtsam {

  // Forward declarations
  class Values;
  class Ordering;
  class GaussianFactorGraph;
  class SymbolicFactorGraph;

  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; 
    std::map<size_t, Point2> factorPositions; 
    GraphvizFormatting() :
      paperHorizontalAxis(Y), paperVerticalAxis(X),
      figureWidthInches(5), figureHeightInches(5), scale(1),
      mergeSimilarFactors(false), plotFactorPoints(true),
      connectKeysToFactor(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) {}

    void print(const std::string& str = "NonlinearFactorGraph: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) 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;

    FastSet<Key> keys() const;

    double error(const Values& c) const;

    double probPrime(const Values& c) 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;

    NonlinearFactorGraph clone() const;

    NonlinearFactorGraph rekey(const std::map<Key,Key>& rekey_mapping) const;

  private:

    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));
    }
  };

} // namespace