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