NonlinearFactorGraph.h/NonlinearFactorGraph.cpp

一、预先定义

使用NonlinearFactorGraph::saveGraph进行的GraphViz配置选择

struct GTSAM_EXPORT GraphvizFormatting{
/*......*/
};
1
2
3

二、NonlinearFactorGraph类

/**
非线性因子图是由非线性因子导出的非高斯（即非线性因子）图。值结构通常（在SAM中）比向量更一般，例如Rot3或Pose3，它们是非线性流形中的对象。线性化非线性因子图在线性化点处的切向量空间上创建线性因子图。因为切线空间是真正的向量空间，所以配置类型将是线性化因子图中的VectorValues。
*/
1
2
3

  /**
   * A non-linear factor graph is a graph of non-Gaussian, i.e. non-linear factors,
   * which derive from NonlinearFactor. The values structures are typically (in SAM) more general
   * than just vectors, e.g., Rot3 or Pose3, which are objects in non-linear manifolds.
   * Linearizing the non-linear factor graph creates a linear factor graph on the
   * tangent vector space at the linearization point. Because the tangent space is a true
   * vector space, the config type will be an VectorValues in that linearized factor graph.
   */
  class GTSAM_EXPORT NonlinearFactorGraph: public FactorGraph<NonlinearFactor> {
  };
1
2
3
4
5
6
7
8
9
10

2.1 构造与析构函数

    /** Default constructor */
    NonlinearFactorGraph() {}

    /** Construct from iterator over factors */
    template<typename ITERATOR>
    NonlinearFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {}

    /** Construct from container of factors (shared_ptr or plain objects) */
    template<class CONTAINER>
    explicit NonlinearFactorGraph(const CONTAINER& factors) : Base(factors) {}

    /** Implicit copy/downcast constructor to override explicit template container constructor */
    template<class DERIVEDFACTOR>
    NonlinearFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}

    /// Destructor
    virtual ~NonlinearFactorGraph() {}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

从迭代器拷贝
从容器拷贝(shared_ptr或者纯对象)
用于重写显式模板容器构造函数的隐式复制/向下转换构造函数

2.2 一些输出

    /** print */
    void print(
        const std::string& str = "NonlinearFactorGraph: ",
        const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;

    /** print errors along with factors*/
    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;

    /** Test equality */
    bool equals(const NonlinearFactorGraph& other, double tol = 1e-9) const;

    /// Write the graph in GraphViz format for visualization
    void saveGraph(std::ostream& stm, const Values& values = Values(),
      const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(),
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;

    /**
     * Write the graph in GraphViz format to file for visualization.
     *
     * This is a wrapper friendly version since wrapped languages don't have
     * access to C++ streams.
     */
    void saveGraph(const std::string& file, const Values& values = Values(),
      const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(),
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

2.3 非标准error和probability

$0.5{\sum }_i(h_i(X_i)-z{)}^2{\sigma }^2$

    /** unnormalized error, \f$ 0.5 \sum_i (h_i(X_i)-z)^2/\sigma^2 \f$ in the most common case */
    double error(const Values& values) const;

    /** Unnormalized probability. O(n) */
    double probPrime(const Values& values) const;
1
2
3
4
5

2.4

    /**
     * Create a symbolic factor graph
     */
    boost::shared_ptr<SymbolicFactorGraph> symbolic() const;

    /**
     * Compute a fill-reducing ordering using COLAMD.
     */
    Ordering orderingCOLAMD() const;
1
2
3
4
5
6
7
8
9

在COLAMD计算填充减少顺序

2.5 带有约束的消元

    /**
     * Compute a fill-reducing ordering with constraints using CCOLAMD
     *
     * @param constraints is a map of Key->group, where 0 is unconstrained, and higher
     * group numbers are further back in the ordering. Only keys with nonzero group
     * indices need to appear in the constraints, unconstrained is assumed for all
     * other variables
     */
    Ordering orderingCOLAMDConstrained(const FastMap<Key, int>& constraints) const;
1
2
3
4
5
6
7
8
9

使用CCOLAMD计算具有约束的填充减少排序
约束是Key->group的映射，其中0是无约束的，更高的组号在排序中更靠后。仅具有非零组索引的键需要出现在约束中，所有其他变量都假定为无约束

2.6 线性化和阻尼函数

    /// Linearize a nonlinear factor graph
    boost::shared_ptr<GaussianFactorGraph> linearize(const Values& linearizationPoint) const;

    /// typdef for dampen functions used below
    typedef std::function<void(const boost::shared_ptr<HessianFactor>& hessianFactor)> Dampen;
1
2
3
4
5

2.7 线性化为HessianFactor

2.7.1 重载1

将其预分配并线性化为HessianFactor，在构建新图时避免了许多malloc和指针间接，因此在密集解决方案适合您的问题时非常有用。
可选的lambda函数可用于在填充的Hessian上应用阻尼。
没有利用并行性，因为所有因素都写入同一内存。

    /**
     * Instead of producing a GaussianFactorGraph, pre-allocate and linearize directly
     * into a HessianFactor. Avoids the many mallocs and pointer indirection in constructing
     * a new graph, and hence useful in case a dense solve is appropriate for your problem.
     * An optional lambda function can be used to apply damping on the filled Hessian.
     * No parallelism is exploited, because all the factors write in the same memory.
     */
    boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
        const Values& values, const Dampen& dampen = nullptr) const;
1
2
3
4
5
6
7
8
9

2.7.2 重载2

如果Ordering给定的话

    /**
     * Instead of producing a GaussianFactorGraph, pre-allocate and linearize directly
     * into a HessianFactor. Avoids the many mallocs and pointer indirection in constructing
     * a new graph, and hence useful in case a dense solve is appropriate for your problem.
     * An ordering is given that still decides how the Hessian is laid out.
     * An optional lambda function can be used to apply damping on the filled Hessian.
     * No parallelism is exploited, because all the factors write in the same memory.
     */
    boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
        const Values& values, const Ordering& ordering, const Dampen& dampen = nullptr) const;
1
2
3
4
5
6
7
8
9
10

2.8

线性化并一次求解

    /// Linearize and solve in one pass.
    /// Calls linearizeToHessianFactor, densely solves the normal equations, and updates the values.
    Values updateCholesky(const Values& values,
                          const Dampen& dampen = nullptr) const;

    /// Linearize and solve in one pass.
    /// Calls linearizeToHessianFactor, densely solves the normal equations, and updates the values.
    Values updateCholesky(const Values& values, const Ordering& ordering,
                          const Dampen& dampen = nullptr) const;
1
2
3
4
5
6
7
8
9

2.9 clone

    /// Clone() performs a deep-copy of the graph, including all of the factors
    NonlinearFactorGraph clone() const;
1
2

2.10 rekey

执行所有因子的深度复制，并根据mapping更改Keys
rekey_mapping是旧键>新键的映射

    /**
     * Rekey() performs a deep-copy of all of the factors, and changes
     * keys according to a mapping.
     *
     * Keys not specified in the mapping will remain unchanged.
     *
     * @param rekey_mapping is a map of old->new keys
     * @result a cloned graph with updated keys
     */
    NonlinearFactorGraph rekey(const std::map<Key,Key>& rekey_mapping) const;
1
2
3
4
5
6
7
8
9
10

2.11 添加用于自动求导的factor

$|h(x)-z{|}_R^2$

    /**
     * Directly add ExpressionFactor that implements |h(x)-z|^2_R
     * @param h expression that implements measurement function
     * @param z measurement
     * @param R model
     */
    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));
    }
1
2
3
4
5
6
7
8
9
10
11

2.12 添加先验

    /**
     * Convenience method which adds a PriorFactor to the factor graph.
     * @param key    Variable key
     * @param prior  The variable's prior value
     * @param model  Noise model for prior factor
     */
    template<typename T>
    void addPrior(Key key, const T& prior,
                  const SharedNoiseModel& model = nullptr) {
      emplace_shared<PriorFactor<T>>(key, prior, model);
    }

    /**
     * Convenience method which adds a PriorFactor to the factor graph.
     * @param key         Variable key
     * @param prior       The variable's prior value
     * @param covariance  Covariance matrix.
     * 
     * Note that the smart noise model associated with the prior factor
     * automatically picks the right noise model (e.g. a diagonal noise model
     * if the provided covariance matrix is diagonal).
     */
    template<typename T>
    void addPrior(Key key, const T& prior, const Matrix& covariance) {
      emplace_shared<PriorFactor<T>>(key, prior, covariance);
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26

三、private成员函数

从“分散”而不是“排序”进行线性化。因为不包括gttic，所以被设为私有

    /**
     * Linearize from Scatter rather than from Ordering.  Made private because
     *  it doesn't include gttic.
     */
    boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
        const Values& values, const Scatter& scatter, const Dampen& dampen = nullptr) const;

    /** Serialization function */
    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));
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14

四、ALLOW_DEPRECATED

#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
    /** \deprecated */
    boost::shared_ptr<HessianFactor> GTSAM_DEPRECATED linearizeToHessianFactor(
        const Values& values, boost::none_t, const Dampen& dampen = nullptr) const
      {return linearizeToHessianFactor(values, dampen);}

    /** \deprecated */
    Values GTSAM_DEPRECATED updateCholesky(const Values& values, boost::none_t,
                          const Dampen& dampen = nullptr) const
      {return updateCholesky(values, dampen);}
#endif
1
2
3
4
5
6
7
8
9
10
11