PCGSolver.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
 
 * -------------------------------------------------------------------------- */

 
#pragma once
 
#include <gtsam/base/Matrix.h>
#include <gtsam/linear/ConjugateGradientSolver.h>
#include <gtsam/linear/VectorValues.h>
 
#include <string>
 
namespace gtsam {
 
class GaussianFactorGraph;
class Preconditioner;
struct PreconditionerParameters;

struct GTSAM_EXPORT PCGSolverParameters : public ConjugateGradientParameters {
  typedef ConjugateGradientParameters Base;
  typedef std::shared_ptr<PCGSolverParameters> shared_ptr;

  std::shared_ptr<PreconditionerParameters> preconditioner;
  bool parallel = true;   
  size_t numThreads = 0;  
 
  PCGSolverParameters() {}
 
  PCGSolverParameters(
      const std::shared_ptr<PreconditionerParameters>& preconditioner)
      : preconditioner(preconditioner) {}
 
  void print(std::ostream& os) const override;
  void print(const std::string& s) const;
};

struct GTSAM_EXPORT PCGSolverResult {
  VectorValues solution;         
  ConjugateGradientStats stats;  
  double operatorSetupSeconds =
      0.0;  
  double preconditionerSetupSeconds =
      0.0;  
  double solveSeconds =
      0.0;  
};

class GTSAM_EXPORT PCGSolver : public IterativeSolver {
 public:
  typedef IterativeSolver Base;
  typedef std::shared_ptr<PCGSolver> shared_ptr;
 
 protected:
  PCGSolverParameters parameters_;
  std::shared_ptr<Preconditioner> preconditioner_;
 
 public:
  PCGSolver(const PCGSolverParameters& p);
  ~PCGSolver() override {}
 
  using IterativeSolver::optimize;

  VectorValues optimize(const GaussianFactorGraph& gfg, const KeyInfo& keyInfo,
                        const std::map<Key, Vector>& lambda,
                        const VectorValues& initial) override;

  PCGSolverResult optimizeDetailed(const GaussianFactorGraph& gfg,
                                   bool collectResidualHistory = true);

  PCGSolverResult optimizeDetailed(const GaussianFactorGraph& gfg,
                                   const KeyInfo& keyInfo,
                                   const std::map<Key, Vector>& lambda,
                                   const VectorValues& initial,
                                   bool collectResidualHistory = true);
};

class GTSAM_EXPORT GaussianFactorGraphSystem {
  class Impl;
 
  const Preconditioner& preconditioner_;
  std::shared_ptr<Impl> impl_;
 
  const std::vector<Matrix>& hessianBlockDiagonal() const;
 
  friend class PCGSolver;
 
 public:
  GaussianFactorGraphSystem(const GaussianFactorGraph& gfg,
                            const Preconditioner& preconditioner,
                            const KeyInfo& info,
                            const std::map<Key, Vector>& lambda,
                            bool parallel = true, size_t numThreads = 0);

  void residual(const Vector& x, Vector& r) const;

  void multiply(const Vector& x, Vector& y) const;

  void leftPrecondition(const Vector& x, Vector& y) const;

  void rightPrecondition(const Vector& x, Vector& y) const;

  void scal(const double alpha, Vector& x) const;

  double dot(const Vector& x, const Vector& y) const;

  void axpy(const double alpha, const Vector& x, Vector& y) const;

  void getb(Vector& b) const;

  size_t numThreads() const;
};


GTSAM_EXPORT VectorValues
buildVectorValues(const Vector& v, const Ordering& ordering,
                  const std::map<Key, size_t>& dimensions);

 
}  // namespace gtsam