gtsam
4.1.0
gtsam

This class implements the linear SPCG solver presented in Dellaert et al in IROS'10.
Given a linear leastsquares problem \( f(x) = A x  b^2 \). We split the problem into \( f(x) = A_t  b_t^2 + A_c  b_c^2 \) where \( A_t \) denotes the "tree" part, and \( A_c \) denotes the "constraint" part.
\(A_t \) is factorized into \( Q_t R_t \), and we compute \( c_t = Q_t^{1} b_t \), and \( x_t = R_t^{1} c_t \) accordingly.
Then we solve a reparametrized problem \( f(y) = y^2 + A_c R_t^{1} y \bar{b_y}^2 \), where \( y = R_t(x  x_t) \), and \( \bar{b_y} = (b_c  A_c x_t) \)
In the matrix form, it is equivalent to solving \( [A_c R_t^{1} ; I ] y = [\bar{b_y} ; 0] \). We can solve it with the leastsquares variation of the conjugate gradient method.
To use it in nonlinear optimization, please see the following example
LevenbergMarquardtParams parameters; parameters.linearSolverType = NonlinearOptimizerParams::CONJUGATE_GRADIENT; parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>(); LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); Values result = optimizer.optimize();
Constructors  
SubgraphSolver (const GaussianFactorGraph &A, const Parameters ¶meters, const Ordering &ordering)  
Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG Will throw exception if there are ternary factors or higher arity, as we use Kruskal's algorithm to split the graph, treating binary factors as edges.  
SubgraphSolver (const GaussianFactorGraph &Ab1, const boost::shared_ptr< GaussianFactorGraph > &Ab2, const Parameters ¶meters, const Ordering &ordering)  
The user specifies the subgraph part and the constraints part. More...  
SubgraphSolver (const boost::shared_ptr< GaussianBayesNet > &Rc1, const boost::shared_ptr< GaussianFactorGraph > &Ab2, const Parameters ¶meters)  
The same as above, but we assume A1 was solved by caller. More...  
virtual  ~SubgraphSolver () 
Destructor.  
Implement interface  
VectorValues  optimize () const 
Optimize from zero.  
VectorValues  optimize (const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map< Key, Vector > &lambda, const VectorValues &initial) override 
Interface that IterativeSolver subclasses have to implement.  
std::pair< boost::shared_ptr< GaussianFactorGraph >, boost::shared_ptr< GaussianFactorGraph > >  splitGraph (const GaussianFactorGraph &gfg) 
Split graph using Kruskal algorithm, treating binary factors as edges.  
Public Types  
typedef SubgraphSolverParameters  Parameters 
Public Types inherited from gtsam::IterativeSolver  
typedef boost::shared_ptr< IterativeSolver >  shared_ptr 
Protected Attributes  
Parameters  parameters_ 
boost::shared_ptr< SubgraphPreconditioner >  pc_ 
preconditioner object  
Additional Inherited Members  
Public Member Functions inherited from gtsam::IterativeSolver  
GTSAM_EXPORT VectorValues  optimize (const GaussianFactorGraph &gfg, boost::optional< const KeyInfo & >=boost::none, boost::optional< const std::map< Key, Vector > & > lambda=boost::none) 
GTSAM_EXPORT VectorValues  optimize (const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map< Key, Vector > &lambda) 
gtsam::SubgraphSolver::SubgraphSolver  (  const GaussianFactorGraph &  Ab1, 
const boost::shared_ptr< GaussianFactorGraph > &  Ab2,  
const Parameters &  parameters,  
const Ordering &  ordering  
) 
The user specifies the subgraph part and the constraints part.
May throw exception if A1 is underdetermined. An ordering is required to eliminate Ab1. We take Ab1 as a const reference, as it will be transformed into Rc1, but take Ab2 as a shared pointer as we need to keep it around.
gtsam::SubgraphSolver::SubgraphSolver  (  const boost::shared_ptr< GaussianBayesNet > &  Rc1, 
const boost::shared_ptr< GaussianFactorGraph > &  Ab2,  
const Parameters &  parameters  
) 
The same as above, but we assume A1 was solved by caller.
We take two shared pointers as we keep both around.