MultifrontalClique.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/SymmetricBlockMatrix.h>
#include <gtsam/base/VerticalBlockMatrix.h>
#include <gtsam/dllexport.h>
#include <gtsam/inference/Key.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/MultifrontalParameters.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/internal/BatchHessianMapping.h>
#include <gtsam/nonlinear/LMDampingParams.h>
#include <gtsam/symbolic/SymbolicFactor.h>
 
#include <cstdint>
#include <iosfwd>
#include <map>
#include <memory>
#include <optional>
#include <string>
#include <unordered_map>
#include <unordered_set>
#include <vector>
 
namespace gtsam {
 
class BatchJacobianFactorBase;
class GaussianConditional;
class HessianFactor;
class JacobianFactor;

using KeyDimMap = std::map<Key, size_t>;
 
namespace internal {

template <typename KeyRange>
inline size_t sumDims(const KeyDimMap& dims, const KeyRange& keys) {
  size_t dim = 0;
  for (Key key : keys) {
    auto it = dims.find(key);
    if (it != dims.end()) dim += it->second;
  }
  return dim;
}
 
}  // namespace internal

class GTSAM_EXPORT MultifrontalClique {
 public:
  using shared_ptr = std::shared_ptr<MultifrontalClique>;
  using Children = std::vector<shared_ptr>;
  struct ChildInfo {
    shared_ptr clique;
    KeySet separatorKeys;
  };
 
  std::weak_ptr<MultifrontalClique> parent;  
  Children children;        
  size_t frontalDim = 0;    
  size_t separatorDim = 0;  

  explicit MultifrontalClique(std::vector<size_t> factorIndices,
                              const std::weak_ptr<MultifrontalClique>& parent,
                              const KeyVector& frontals,
                              const KeySet& separatorKeys,
                              const KeyDimMap& dims, size_t vbmRows,
                              VectorValues* solution,
                              const std::unordered_set<Key>* fixedKeys,
                              size_t numEliminatedFrontals);


  void finalize(std::vector<ChildInfo> children,
                const MultifrontalParameters& params);

  void fillAb(const GaussianFactorGraph& graph);

  void prepareForElimination();

  void factorize();

  void addIdentityDamping(double lambda);

  void addDiagonalDamping(double lambda, double minDiagonal,
                          double maxDiagonal);

  void addExactDiagonalDamping(double lambda,
                               const VectorValues& hessianDiagonal,
                               double minDiagonal, double maxDiagonal);



  int problemSize() const {
    return static_cast<int>(frontalDim + separatorDim);
  }

  size_t numFrontals() const { return frontalPtrs_.size(); }

  bool fullyEliminated() const { return numFrontals() == totalFrontals_; }

  const KeyVector& orderedKeys() const { return orderedKeys_; }

  std::shared_ptr<GaussianConditional> conditional() const;

  std::shared_ptr<HessianFactor> remainingFactor() const;

  const VerticalBlockMatrix& Ab() const { return Ab_; }

  const SymmetricBlockMatrix& info() const { return info_; }

  bool useQR() const { return solveMode_ == SolveMode::QrLeaf; }

  bool useCompactCholesky() const {
    return solveMode_ == SolveMode::CompactCholeskyLeaf ||
           solveMode_ == SolveMode::FusedStarCandidate ||
           solveMode_ == SolveMode::FusedStarCholeskyLeaf;
  }

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



  void eliminateInPlace();

  void eliminateInPlace(double lambda, const LMDampingParams& dampingParams,
                        const VectorValues& exactHessianDiagonal);

  void updateSolution();

  double lastOldError() const { return lastOldError_; }

  double lastNewError() const { return lastNewError_; }

  double constantTermError() const;
 
  friend std::ostream& operator<<(std::ostream& os,
                                  const MultifrontalClique& clique);
 
 private:
  enum class SolveMode {
    Cholesky,
    QrLeaf,
    CompactCholeskyLeaf,
    FusedStarCandidate,
    FusedStarCholeskyLeaf
  };

  void cacheSolutionPointers(VectorValues* delta, const KeyVector& frontals,
                             const KeyVector& separatorKeys);

  DenseIndex blockIndex(Key key) const;
 
  enum class FactorLoadKind : uint8_t { Empty, Jacobian, Batch };
 
  struct FactorLoadPlan {
    FactorLoadKind kind = FactorLoadKind::Empty;
    size_t factorIndex = 0;
    SharedDiagonal model;
    size_t rows = 0;
    size_t abRowOffset = 0;
    std::vector<DenseIndex> blockIndices;
    internal::BatchHessianMapping localMapping;
    internal::BatchHessianMapping parentMapping;
    internal::BatchHessianMapping separatorMapping;
    bool canDirectUpdate = false;

    static FactorLoadPlan forNullFactor(size_t factorIndex);

    static FactorLoadPlan forJacobian(size_t factorIndex,
                                      const JacobianFactor& factor,
                                      const MultifrontalClique& clique);

    static FactorLoadPlan forBatch(size_t factorIndex,
                                   const BatchJacobianFactorBase& factor,
                                   const MultifrontalClique& clique);

    bool isDirectBatch() const {
      return kind == FactorLoadKind::Batch && canDirectUpdate;
    }

    bool needsMaterializedRows(const MultifrontalClique& clique) const;

    void assignMaterializedRows(size_t* nextRow);

    void assertInvariants(const GaussianFactor* factor,
                          const MultifrontalClique& clique) const;

    bool supportsFusedStarLeaf() const;

    void buildSolveMappings(const BatchJacobianFactorBase& factor,
                            const MultifrontalClique& clique);
 
   private:
    void mapKeys(const KeyVector& keys, const MultifrontalClique& clique);

    void buildLocalBatchMapping(const BatchJacobianFactorBase& factor,
                                const MultifrontalClique& clique);

    internal::BatchHessianMapping buildRetainedMapping(
        const BatchJacobianFactorBase& factor, DenseIndex numFrontals,
        const std::vector<DenseIndex>& targetIndices,
        const std::vector<DenseIndex>& targetScalarOffsets) const;
  };

  void buildLoadPlans(const GaussianFactorGraph& graph);

  void allocateSolveStorage();

  void resolveLeafSolveMode();

  void buildFusedStarMappings();

  void updateParentInfo(SymmetricBlockMatrix& parentInfo) const;

  void updateParentMaterializedColumn(SymmetricBlockMatrix& parentInfo,
                                      DenseIndex sourceSeparatorBlock) const;

  void updateParentQrColumnScratch(DenseIndex sourceSeparatorBlock,
                                   Matrix* scratch) const;

  double parentRhsDiagonal() const;

  void updateSeparatorInfo(SymmetricBlockMatrix& separatorInfo) const;

  void prepareCompactCholesky();

  void factorizeCompactCholesky();

  void factorizeFusedStarCholesky();

  void updateFusedStarInfo(SymmetricBlockMatrix& targetInfo,
                           const std::vector<DenseIndex>& targetIndices,
                           const std::vector<DenseIndex>& targetScalarOffsets,
                           bool useParentMappedSlots) const;

  void updateCholeskyInfo(SymmetricBlockMatrix& targetInfo,
                          const std::vector<DenseIndex>& targetIndices,
                          const std::vector<DenseIndex>& targetBlockOffsets,
                          bool useParentMappedSlots) const;

  void updateDirectFactors(SymmetricBlockMatrix& targetInfo,
                           const std::vector<DenseIndex>& targetIndices,
                           bool useParentMappedSlots) const;

  void gatherUpdatesSequential();

  void gatherUpdatesParallel(size_t numThreads);

  void gatherSameSeparatorUpdates();

  std::vector<size_t> blockDims(const KeyDimMap& dims,
                                const KeyVector& frontals,
                                const KeySet& separatorKeys) const;

  void applyDampingQR(double lambda, const LMDampingParams& dampingParams,
                      const VectorValues& exactHessianDiagonal);

  void applyDampingCholesky(double lambda, const LMDampingParams& dampingParams,
                            const VectorValues& exactHessianDiagonal);

  size_t addJacobianFactor(const JacobianFactor& factor, size_t rowOffset,
                           const FactorLoadPlan& plan);

  size_t addBatchJacobianFactor(const BatchJacobianFactorBase& factor,
                                size_t rowOffset, const FactorLoadPlan& plan);
 
  void setParentIndices(const std::vector<DenseIndex>& indices,
                        const std::vector<DenseIndex>& scalarOffsets) {
    parentIndices_ = indices;
    parentScalarOffsets_ = scalarOffsets;
  }
 
  // Construction-time metadata (set once in the constructor).
  std::vector<size_t> factorIndices_;
  KeyVector orderedKeys_;     
  size_t totalFrontals_ = 0;  
  const std::unordered_set<Key>* fixedKeys_ = nullptr;
  std::unordered_map<Key, DenseIndex> blockIndexCache_;
  std::vector<Vector*> frontalPtrs_;          
  std::vector<const Vector*> separatorPtrs_;  
  std::vector<size_t> blockDims_;  
  size_t factorRows_ = 0;  
  mutable const GaussianFactorGraph* activeLoadGraph_ = nullptr;
  mutable bool allBatchFactors_ = false;
  mutable bool hasDirectBatchFactors_ = false;
  mutable std::vector<FactorLoadPlan> loadPlans_;
  mutable bool loadPlansBuilt_ = false;
  mutable size_t materializedRows_ = 0;  
  mutable size_t fillRows_ = 0;
 
  // Finalize-time metadata (set once after children are known).
  std::vector<DenseIndex>
      parentIndices_;  
  std::vector<DenseIndex>
      parentScalarOffsets_;  
  std::vector<DenseIndex> separatorIndices_;  
  std::vector<DenseIndex>
      separatorScalarOffsets_;  
  SolveMode solveMode_ = SolveMode::Cholesky;
  SolveMode starFallbackMode_ = SolveMode::Cholesky;
  bool deferredSingleFactorAutoQr_ = false;
  std::vector<DenseIndex> parentSchurScalarOffsets_;
  std::vector<DenseIndex> separatorSchurScalarOffsets_;
 
  std::vector<std::vector<size_t>> sameSeparatorChildGroups_;
  std::vector<std::vector<DenseIndex>> sameSeparatorParentIndices_;
  std::vector<std::vector<DenseIndex>> sameSeparatorParentScalarOffsets_;
  std::vector<SymmetricBlockMatrix> sameSeparatorInfos_;
  std::vector<uint8_t> childInSameSeparatorGroup_;
 
  struct ParentGatherPlan;
  // Built after deferred modes resolve and reused across numerical reloads.
  std::shared_ptr<ParentGatherPlan> parentGatherPlan_;
 
  // Lazily allocated after load-plan construction. Direct batch factors need
  // no rows; QR additionally reserves frontal damping rows.
  VerticalBlockMatrix Ab_;
 
  // Cached factorization storage reused by parent updates and result exports.
  mutable VerticalBlockMatrix RSd_;  
  mutable SymmetricBlockMatrix info_;
 
  // Elimination-time state.
  bool RSdReady_ = false;
  bool infoReady_ = false;
 
  // Solve-time scratch space.
  Vector rhsScratch_;        
  Vector separatorScratch_;  
 
  // Solve-time cached error contributions.
  double lastOldError_ = 0.0;
  double lastNewError_ = 0.0;
};
 
std::ostream& operator<<(std::ostream& os, const MultifrontalClique& clique);
 
}  // namespace gtsam