RegularHessianFactor.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/linear/HessianFactor.h>
#include <gtsam/linear/RegularJacobianFactor.h>
#include <gtsam/linear/VectorValues.h> // For VectorValues and Scatter
#include <vector>
#include <stdexcept> // For std::invalid_argument
 
namespace gtsam {
 
  template<size_t D>
  class RegularHessianFactor : public HessianFactor {
 
  public:
 
    typedef Eigen::Matrix<double, D, 1> VectorD;
    typedef Eigen::Matrix<double, D, D> MatrixD;
 
    RegularHessianFactor(const KeyVector& js,
      const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) :
      HessianFactor(js, Gs, gs, f) {
      checkInvariants();
    }
 
    RegularHessianFactor(Key j1, Key j2, const MatrixD& G11, const MatrixD& G12,
      const VectorD& g1, const MatrixD& G22, const VectorD& g2, double f) :
      HessianFactor(j1, j2, G11, G12, g1, G22, g2, f) {
    }
 
    RegularHessianFactor(Key j1, Key j2, Key j3,
      const MatrixD& G11, const MatrixD& G12, const MatrixD& G13, const VectorD& g1,
      const MatrixD& G22, const MatrixD& G23, const VectorD& g2,
      const MatrixD& G33, const VectorD& g3, double f) :
      HessianFactor(j1, j2, j3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f) {
    }
 
    template<typename KEYS>
    RegularHessianFactor(const KEYS& keys,
      const SymmetricBlockMatrix& augmentedInformation) :
      HessianFactor(keys, augmentedInformation) {
      checkInvariants();
    }
 
    RegularHessianFactor(const RegularJacobianFactor<D>& jf)
      : HessianFactor(jf) {
    }
 
    RegularHessianFactor(const GaussianFactorGraph& factors,
      const Scatter& scatter)
      : HessianFactor(factors, scatter) {
      checkInvariants();
    }
 
    RegularHessianFactor(const GaussianFactorGraph& factors)
      : HessianFactor(factors) {
      checkInvariants();
    }
 
  private:
 
    void checkInvariants() const {
      if (info_.cols() != 0 && // Allow zero-key factors (e.g. priors on anchor nodes)
        info_.cols() != 1 + (info_.nBlocks() - 1) * static_cast<DenseIndex>(D))
        throw std::invalid_argument(
          "RegularHessianFactor constructor was given non-regular factors or "
          "incorrect template dimension D");
    }
 
    // Use Eigen magic to access raw memory for efficiency in Hessian products
    typedef Eigen::Map<VectorD> DMap;
    typedef Eigen::Map<const VectorD> ConstDMap;
 
    // Scratch space for multiplyHessianAdd to avoid re-allocation
    // According to link below this is thread-safe.
    // http://stackoverflow.com/questions/11160964/multiple-copies-of-the-same-object-c-thread-safe
    mutable std::vector<VectorD, Eigen::aligned_allocator<VectorD>> y_;
 
  public:
 
    void multiplyHessianAdd(double alpha, const VectorValues& x,
      VectorValues& y) const override {
      // Note: This implementation just calls the base class. The raw memory versions
      // below are specifically optimized for the regular structure of this class.
      // Consider using those directly or ensuring the base class implementation
      // is efficient enough for your use case if calling this version.
      HessianFactor::multiplyHessianAdd(alpha, x, y);
    }
 
    void multiplyHessianAdd(double alpha, const double* x,
      double* yvalues) const {
      // Ensure scratch space is properly sized
      const size_t n = size();
      if (y_.size() != n) {
        y_.resize(n);
      }
      for (VectorD& yi : y_)
        yi.setZero();
 
      // Loop over columns (j) of the Hessian H=info_
      // Accessing x only once per column j
      // Fill temporary y_ vector corresponding to rows i
      for (DenseIndex j = 0; j < static_cast<DenseIndex>(n); ++j) {
        Key key_j = keys_[j];
        ConstDMap xj(x + key_j * D); // Map to the j-th block of x
 
        DenseIndex i = 0;
        // Off-diagonal blocks G_ij * x_j for i < j
        for (; i < j; ++i) {
          y_[i] += info_.aboveDiagonalBlock(i, j) * xj;
        }
        // Diagonal block G_jj * x_j
        y_[i] += info_.diagonalBlock(j) * xj;
        // Off-diagonal blocks G_ij * x_j for i > j (using transpose G_ji^T * x_j)
        for (i = j + 1; i < static_cast<DenseIndex>(n); ++i) {
          y_[i] += info_.aboveDiagonalBlock(j, i).transpose() * xj;
        }
      }
 
      // Add accumulated results from y_ to the output yvalues
      for (DenseIndex i = 0; i < static_cast<DenseIndex>(n); ++i) {
        Key key_i = keys_[i];
        DMap map_yi(yvalues + key_i * D); // Map to the i-th block of yvalues
        map_yi += alpha * y_[i];
      }
    }
 
    void multiplyHessianAdd(double alpha, const double* x, double* yvalues,
      const std::vector<size_t>& offsets) const {
      // Ensure scratch space is properly sized
      const size_t n = size();
      if (y_.size() != n) {
        y_.resize(n);
      }
      for (VectorD& yi : y_)
        yi.setZero();
 
      // Loop over columns (j) of the Hessian H=info_
      for (DenseIndex j = 0; j < static_cast<DenseIndex>(n); ++j) {
        Key key_j = keys_[j];
        size_t offset_j = offsets[key_j];
        // Ensure block size matches D (redundant if checkInvariants worked, but safe)
        size_t dim_j = offsets[key_j + 1] - offset_j;
        if (dim_j != D) throw std::runtime_error("RegularHessianFactor::multiplyHessianAdd: Mismatched dimension in offset map.");
        ConstDMap xj(x + offset_j); // Map to the j-th block of x using offset
 
        DenseIndex i = 0;
        // Off-diagonal blocks G_ij * x_j for i < j
        for (; i < j; ++i) {
          y_[i] += info_.aboveDiagonalBlock(i, j) * xj;
        }
        // Diagonal block G_jj * x_j
        y_[i] += info_.diagonalBlock(j) * xj;
        // Off-diagonal blocks G_ij * x_j for i > j (using transpose G_ji^T * x_j)
        for (i = j + 1; i < static_cast<DenseIndex>(n); ++i) {
          y_[i] += info_.aboveDiagonalBlock(j, i).transpose() * xj;
        }
      }
 
      // Add accumulated results from y_ to the output yvalues using offsets
      for (DenseIndex i = 0; i < static_cast<DenseIndex>(n); ++i) {
        Key key_i = keys_[i];
        size_t offset_i = offsets[key_i];
        size_t dim_i = offsets[key_i + 1] - offset_i;
        if (dim_i != D) throw std::runtime_error("RegularHessianFactor::multiplyHessianAdd: Mismatched dimension in offset map.");
        DMap map_yi(yvalues + offset_i); // Map to the i-th block of yvalues using offset
        map_yi += alpha * y_[i];
      }
    }
 
    void hessianDiagonal(double* d) const override {
      // Loop over all variables (diagonal blocks) in the factor
      const size_t n = size();
      for (DenseIndex pos = 0; pos < static_cast<DenseIndex>(n); ++pos) {
        Key j = keys_[pos];
        // Get the diagonal block G_jj and add its diagonal elements to d
        DMap(d + D * j) += info_.diagonal(pos);
      }
    }
 
    void gradientAtZero(double* d) const override {
      // The linear term g is stored as the last block column of info_ (negated).
      // We add (-g) to d.
      const size_t n = size();
      for (DenseIndex pos = 0; pos < static_cast<DenseIndex>(n); ++pos) {
        Key j = keys_[pos];
        // info_.aboveDiagonalBlock(pos, n) accesses the block corresponding to g_j
        DMap(d + D * j) += info_.aboveDiagonalBlock(pos, n);
      }
    }
 
    /* ************************************************************************* */
 
  }; // end class RegularHessianFactor
 
  // traits
  template<size_t D> struct traits<RegularHessianFactor<D> > : public Testable<
    RegularHessianFactor<D> > {
  };
 
} // namespace gtsam