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/IterativeSolver.h>
#include <gtsam/linear/Preconditioner.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/dllexport.h>

#include <boost/shared_ptr.hpp>

#include <map>

namespace gtsam {

  // Forward declarations
  class GaussianBayesNet;
  class GaussianFactorGraph;
  class VectorValues;

  struct GTSAM_EXPORT SubgraphPreconditionerParameters : public PreconditionerParameters {
    typedef boost::shared_ptr<SubgraphPreconditionerParameters> shared_ptr;
    SubgraphPreconditionerParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
      : builderParams(p) {}
    SubgraphBuilderParameters builderParams;
  };

  class GTSAM_EXPORT SubgraphPreconditioner : public Preconditioner {

  public:
    typedef boost::shared_ptr<SubgraphPreconditioner> shared_ptr;
    typedef boost::shared_ptr<const GaussianBayesNet> sharedBayesNet;
    typedef boost::shared_ptr<const GaussianFactorGraph> sharedFG;
    typedef boost::shared_ptr<const VectorValues> sharedValues;
    typedef boost::shared_ptr<const Errors> sharedErrors;

  private:
    sharedFG Ab2_;
    sharedBayesNet Rc1_;
    sharedValues xbar_;  
    sharedErrors b2bar_; 

    KeyInfo keyInfo_;
    SubgraphPreconditionerParameters parameters_;

  public:

    SubgraphPreconditioner(const SubgraphPreconditionerParameters &p = SubgraphPreconditionerParameters());

    SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar,
                           const SubgraphPreconditionerParameters &p = SubgraphPreconditionerParameters());

    virtual ~SubgraphPreconditioner() {}

    void print(const std::string& s = "SubgraphPreconditioner") const;

    const sharedFG& Ab2() const { return Ab2_; }

    const sharedBayesNet& Rc1() const { return Rc1_; }

    const sharedErrors 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 {
      assert(xbar_);
      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