SubgraphPreconditioner.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010-2019, 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/linear/SubgraphBuilder.h>
#include <gtsam/linear/Errors.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/Preconditioner.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/dllexport.h>
 
#include <memory>
 
#include <map>
 
namespace gtsam {
 
  // Forward declarations
  class GaussianBayesNet;
  class GaussianFactorGraph;
  class VectorValues;
 
  struct GTSAM_EXPORT SubgraphPreconditionerParameters : public PreconditionerParameters {
    typedef std::shared_ptr<SubgraphPreconditionerParameters> shared_ptr;
    SubgraphPreconditionerParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
      : builderParams(p) {}
    SubgraphBuilderParameters builderParams;
  };
 
  class GTSAM_EXPORT SubgraphPreconditioner : public Preconditioner {
 
  public:
    typedef std::shared_ptr<SubgraphPreconditioner> shared_ptr;
 
  private:
    GaussianFactorGraph Ab2_;
    GaussianBayesNet Rc1_;
    VectorValues xbar_;  
    Errors b2bar_; 
 
    KeyInfo keyInfo_;
    SubgraphPreconditionerParameters parameters_;
 
  public:
 
    SubgraphPreconditioner(const SubgraphPreconditionerParameters &p = SubgraphPreconditionerParameters());
 
    SubgraphPreconditioner(const GaussianFactorGraph& Ab2, const GaussianBayesNet& Rc1, const VectorValues& xbar,
                           const SubgraphPreconditionerParameters &p = SubgraphPreconditionerParameters());
 
    ~SubgraphPreconditioner() override {}
 
    void print(const std::string& s = "SubgraphPreconditioner") const;
 
    const GaussianFactorGraph& Ab2() const { return Ab2_; }
 
    const GaussianBayesNet& Rc1() const { return Rc1_; }
 
    const Errors b2bar() const { return b2bar_; }
 
    /* x = xbar + inv(R1)*y */
    VectorValues x(const VectorValues& y) const;
 
    /* A zero VectorValues with the structure of xbar */
    VectorValues zero() const {
      return VectorValues::Zero(xbar_);
    }
 
    void transposeMultiplyAdd2(double alpha, Errors::const_iterator begin,
        Errors::const_iterator end, VectorValues& y) const;
 
    /* error, given y */
    double error(const VectorValues& y) const;
 
    VectorValues gradient(const VectorValues& y) const;
 
    Errors operator*(const VectorValues& y) const;
 
    void multiplyInPlace(const VectorValues& y, Errors& e) const;
 
    VectorValues operator^(const Errors& e) const;
 
    void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& y) const;
 
    /*****************************************************************************/
    /* implement virtual functions of Preconditioner */
 
    void solve(const Vector& y, Vector &x) const override;
 
    void transposeSolve(const Vector& y, Vector& x) const override;
 
    void build(
      const GaussianFactorGraph &gfg,
      const KeyInfo &info,
      const std::map<Key,Vector> &lambda
      ) override;
    /*****************************************************************************/
  };
 
} // namespace gtsam