MultifrontalSolver.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/ForestTraversal.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/MultifrontalParameters.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/symbolic/IndexedJunctionTree.h>
 
#include <iosfwd>
#include <map>
#include <memory>
#include <stdexcept>
#include <string>
#include <unordered_set>
#include <vector>
 
namespace gtsam {
 
class GaussianBayesTree;
class MultifrontalClique;

class GTSAM_EXPORT MultifrontalSolverNotSupported : public std::runtime_error {
 public:
  explicit MultifrontalSolverNotSupported(const std::string& reason)
      : std::runtime_error(BuildMessage(reason)) {}
 
 private:
  static std::string BuildMessage(const std::string& reason) {
    std::string message = "MultifrontalSolver not supported: " + reason + ". ";
    message +=
        "Enable GTSAM_ALLOW_DEPRECATED_SINCE_V43 to default to the legacy "
        "solver, or set linearSolverType = MULTIFRONTAL_CHOLESKY.";
    return message;
  }
};

class GTSAM_EXPORT MultifrontalSolver
    : public ForestTraversal<MultifrontalSolver, MultifrontalClique> {
 public:
  using Parameters = MultifrontalParameters;

  struct PrecomputedData {
    std::map<Key, size_t> dims;         
    std::unordered_set<Key> fixedKeys;  
    IndexedJunctionTree
        indexedJunctionTree;        
    std::vector<size_t> rowCounts;  
  };

  using CliquePtr = std::shared_ptr<MultifrontalClique>;
  using Node = MultifrontalClique;
 
 protected:
  std::vector<CliquePtr> roots_;       
  std::vector<CliquePtr> cliques_;     
  std::map<Key, size_t> dims_;         
  mutable VectorValues solution_;      
  std::unordered_set<Key> fixedKeys_;  
  bool loaded_ = false;                
  bool eliminated_ = false;            
  bool partiallyEliminated_ = false;   
  Ordering ordering_;                  
  size_t firstPhaseSize_ = 0;          
  Parameters params_;                  
  double lastOldError_ = 0.0;          
  double lastNewError_ = 0.0;          
  bool hasDeltaError_ = false;         
 
 public:
  MultifrontalSolver(const GaussianFactorGraph& graph, const Ordering& ordering,
                     const Parameters& params = Parameters{});

  MultifrontalSolver(const GaussianFactorGraph& graph, const Ordering& ordering,
                     size_t firstPhaseSize,
                     const Parameters& params = Parameters{});

  MultifrontalSolver(PrecomputedData data, const Ordering& ordering,
                     const Parameters& params = Parameters{});

  MultifrontalSolver(PrecomputedData data, const Ordering& ordering,
                     size_t firstPhaseSize,
                     const Parameters& params = Parameters{});

  static PrecomputedData Precompute(const GaussianFactorGraph& graph,
                                    const Ordering& ordering);

  void load(const GaussianFactorGraph& graph);

  void eliminateInPlace();

  void eliminateInPlace(const GaussianFactorGraph& graph);

  void eliminatePartialInPlace();

  void eliminatePartialInPlace(const GaussianFactorGraph& graph);

  GaussianFactorGraph remainingFactorGraph() const;

  GaussianBayesTree computeBayesTree() const;

  const VectorValues& updateSolution();

  const VectorValues& updateSolution(const VectorValues& retainedSolution);

  double deltaError(double* oldError = nullptr,
                    double* newError = nullptr) const;

  const std::vector<CliquePtr>& roots() const { return roots_; }

  size_t cliqueCount() const { return cliques_.size(); }

  void print(const std::string& s = "",
             const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;

  friend std::ostream& operator<<(std::ostream& os,
                                  const MultifrontalSolver& solver);
};
 
std::ostream& operator<<(std::ostream& os, const MultifrontalSolver& solver);
 
}  // namespace gtsam