NonlinearFactorGraph

Overview¶

The NonlinearFactorGraph class in GTSAM is a key component for representing and solving nonlinear factor graphs. A factor graph is a bipartite graph that represents the factorization of a function, commonly used in probabilistic graphical models. In the context of GTSAM, it is used to represent the structure of optimization problems, e.g., in the domain of simultaneous localization and mapping (SLAM) or structure from motion (SfM).

Key Functionalities¶

Construction and Initialization¶

• Constructor: The class provides a default constructor to initialize an empty nonlinear factor graph.

Factor Management¶

• add: This method allows adding a new factor to the graph. Factors represent constraints or measurements in the optimization problem.
• reserve: Pre-allocates space for a specified number of factors, optimizing memory usage when the number of factors is known in advance.

Graph Operations¶

• resize: Adjusts the size of the factor graph, which can be useful when dynamically modifying the graph structure.
• remove: Removes a factor from the graph, identified by its index.

Querying and Access¶

• size: Returns the number of factors currently in the graph.
• empty: Checks if the graph contains any factors.
• at: Accesses a specific factor by its index.
• back: Retrieves the last factor in the graph.
• front: Retrieves the first factor in the graph.

Optimization and Linearization¶

• linearize: Converts the nonlinear factor graph into a linear factor graph at a given linearization point. This is a crucial step in iterative optimization algorithms like Gauss-Newton or Levenberg-Marquardt.

  The linearization process involves computing the Jacobian matrices of the nonlinear functions, resulting in a linear approximation:

  $$f(x) \approx f(x_0) + A(x - x_0)$$

  (1)

  where $A$ is the Jacobian matrix evaluated at the point $x_0$.