BatchJacobianFactor.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/base/timing.h>
#include <gtsam/dllexport.h>
#include <gtsam/linear/FlatGaussianFactor.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/VectorValues.h>
 
#include <Eigen/StdVector>
#include <algorithm>
#include <array>
#include <cassert>
#include <map>
#include <stdexcept>
#include <string>
#include <tuple>
#include <type_traits>
#include <utility>
#include <vector>
 
namespace gtsam {
 
namespace internal {
class BatchJacobianFactorElimination;

class SparseNormalAccumulator {
 public:
  virtual ~SparseNormalAccumulator() = default;

  virtual void addHessianBlock(DenseIndex rowOffset, DenseIndex columnOffset,
                               DenseIndex rows, DenseIndex columns,
                               const double* values) = 0;

  virtual void addRhsBlock(DenseIndex offset, DenseIndex dimension,
                           const double* values) = 0;
};
}  // namespace internal

class GTSAM_EXPORT BatchJacobianFactorBase : public GaussianFactor,
                                             public FlatGaussianFactor {
 public:
  using GaussianFactor::GaussianFactor;

  virtual size_t rows() const = 0;

  virtual const SharedDiagonal& get_model() const = 0;

  virtual JacobianFactor toJacobianFactor() const = 0;
 
 private:
  friend class internal::BatchJacobianFactorElimination;

  virtual size_t scatterInto(
      VerticalBlockMatrix& target, size_t rowOffset,
      const std::vector<DenseIndex>& targetBlockIndices) const = 0;

  virtual void updateSparseNormal(
      const std::vector<DenseIndex>& scalarOffsets,
      internal::SparseNormalAccumulator* accumulator) const = 0;
 
 public:
  void print(
      const std::string& s = "",
      const KeyFormatter& formatter = DefaultKeyFormatter) const override {
    toJacobianFactor().print(s, formatter);
  }

  bool equals(const GaussianFactor& factor, double tol = 1e-9) const override {
    if (const auto* batch =
            dynamic_cast<const BatchJacobianFactorBase*>(&factor)) {
      return toJacobianFactor().equals(batch->toJacobianFactor(), tol);
    }
    return toJacobianFactor().equals(factor, tol);
  }

  double error(const VectorValues& values) const override {
    return toJacobianFactor().error(values);
  }

  double deltaError(const VectorValues& values, double* oldError = nullptr,
                    double* newError = nullptr) const override {
    return toJacobianFactor().deltaError(values, oldError, newError);
  }

  Matrix augmentedJacobian() const override {
    return toJacobianFactor().augmentedJacobian();
  }

  std::pair<Matrix, Vector> jacobian() const override {
    return toJacobianFactor().jacobian();
  }

  Matrix augmentedInformation() const override {
    return toJacobianFactor().augmentedInformation();
  }

  Matrix information() const override {
    return toJacobianFactor().information();
  }

  void hessianDiagonalAdd(VectorValues& diagonal) const override {
    toJacobianFactor().hessianDiagonalAdd(diagonal);
  }

  void hessianDiagonal(double* diagonal) const override {
    toJacobianFactor().hessianDiagonal(diagonal);
  }

  std::map<Key, Matrix> hessianBlockDiagonal() const override {
    return toJacobianFactor().hessianBlockDiagonal();
  }

  GaussianFactor::shared_ptr negate() const override {
    return toJacobianFactor().negate();
  }

  void updateHessian(const KeyVector& keys,
                     SymmetricBlockMatrix* info) const override {
    toJacobianFactor().updateHessian(keys, info);
  }
 
 private:
  virtual void updateHessian(const std::vector<DenseIndex>& slotIndices,
                             SymmetricBlockMatrix* info) const = 0;

  virtual void updateHessian(const std::vector<DenseIndex>& slotIndices,
                             SymmetricBlockMatrix* info, DenseIndex beginCol,
                             DenseIndex endCol) const = 0;

  virtual void buildMappedSlots(const std::vector<DenseIndex>& slotIndices,
                                std::vector<DenseIndex>& mappedSlots) const = 0;

  virtual void updateHessianWithMappedSlots(
      const std::vector<DenseIndex>& mappedSlots,
      SymmetricBlockMatrix* info) const = 0;

  virtual void updateHessianWithMappedSlots(
      const std::vector<DenseIndex>& mappedSlots,
      const std::vector<DenseIndex>& mappedScalarOffsets,
      SymmetricBlockMatrix* info) const {
    (void)mappedScalarOffsets;
    updateHessianWithMappedSlots(mappedSlots, info);
  }

  virtual void updateFrontalHessianWithMappedSlots(
      const std::vector<DenseIndex>& mappedSlots, DenseIndex numFrontalBlocks,
      VerticalBlockMatrix* frontalRows) const = 0;
 
 public:
  virtual void multiplyHessianAdd(double alpha,
                                  const std::vector<size_t>& scalarOffsets,
                                  const double* x, double* y) const override {
    if (scalarOffsets.size() != size()) {
      throw std::invalid_argument(
          "BatchJacobianFactorBase::multiplyHessianAdd: offset count "
          "mismatch.");
    }
    VectorValues valuesX, valuesY;
    for (size_t position = 0; position < size(); ++position) {
      const Key key = keys()[position];
      const DenseIndex dimension =
          getDim(begin() + static_cast<DenseIndex>(position));
      valuesX.emplace(key, Eigen::Map<const Vector>(x + scalarOffsets[position],
                                                    dimension));
      valuesY.emplace(key, Vector::Zero(dimension));
    }
    toJacobianFactor().multiplyHessianAdd(alpha, valuesX, valuesY);
    for (size_t position = 0; position < size(); ++position) {
      const Key key = keys()[position];
      Eigen::Map<Vector> output(
          y + scalarOffsets[position],
          getDim(begin() + static_cast<DenseIndex>(position)));
      output += valuesY.at(key);
    }
  }

  virtual void gradientAtZeroAdd(const std::vector<size_t>& scalarOffsets,
                                 double* gradient) const override {
    if (scalarOffsets.size() != size()) {
      throw std::invalid_argument(
          "BatchJacobianFactorBase::gradientAtZeroAdd: offset count "
          "mismatch.");
    }
    const VectorValues factorGradient = toJacobianFactor().gradientAtZero();
    for (size_t position = 0; position < size(); ++position) {
      Eigen::Map<Vector> output(
          gradient + scalarOffsets[position],
          getDim(begin() + static_cast<DenseIndex>(position)));
      output += factorGradient.at(keys()[position]);
    }
  }

  virtual void hessianBlockDiagonalAdd(
      const std::vector<size_t>& blockSlots,
      std::vector<Matrix>* diagonalBlocks) const override {
    if (blockSlots.size() != size()) {
      throw std::invalid_argument(
          "BatchJacobianFactorBase::hessianBlockDiagonalAdd: slot count "
          "mismatch.");
    }
    const std::map<Key, Matrix> factorDiagonal =
        toJacobianFactor().hessianBlockDiagonal();
    for (size_t position = 0; position < size(); ++position) {
      diagonalBlocks->at(blockSlots[position]) +=
          factorDiagonal.at(keys()[position]);
    }
  }

  void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info,
                     DenseIndex beginCol, DenseIndex endCol) const override {
    toJacobianFactor().updateHessian(keys, info, beginCol, endCol);
  }

  void multiplyHessianAdd(double alpha, const VectorValues& x,
                          VectorValues& y) const override {
    toJacobianFactor().multiplyHessianAdd(alpha, x, y);
  }

  VectorValues gradientAtZero() const override {
    return toJacobianFactor().gradientAtZero();
  }

  void gradientAtZero(double* d) const override {
    toJacobianFactor().gradientAtZero(d);
  }

  Vector gradient(Key key, const VectorValues& x) const override {
    return toJacobianFactor().gradient(key, x);
  }
};

template <int ErrorDim, int... BlockDims>
class BatchJacobianFactor : public BatchJacobianFactorBase {
 public:
  static_assert(ErrorDim != Eigen::Dynamic,
                "BatchJacobianFactor requires a fixed error dimension.");
  static_assert(((BlockDims != Eigen::Dynamic) && ...),
                "BatchJacobianFactor requires fixed block dimensions.");
 
  using Base = BatchJacobianFactorBase;
  using This = BatchJacobianFactor<ErrorDim, BlockDims...>;
  using shared_ptr = std::shared_ptr<This>;
  static constexpr size_t NumSlots = sizeof...(BlockDims);
  using SlotIndices = std::array<DenseIndex, NumSlots>;
  using HessianSlots = std::array<DenseIndex, NumSlots + 1>;
  using RhsVector = Eigen::Matrix<double, ErrorDim, 1>;
  template <int BlockDim>
  using BlockMatrix = Eigen::Matrix<double, ErrorDim, BlockDim>;
  template <int BlockDim>
  using BlockVector =
      std::vector<BlockMatrix<BlockDim>,
                  Eigen::aligned_allocator<BlockMatrix<BlockDim>>>;
  using Blocks = std::tuple<BlockVector<BlockDims>...>;
 
 private:
  std::vector<size_t> keyDims_;
  std::vector<SlotIndices> rowSlots_;
  Blocks blocks_;
  std::vector<RhsVector, Eigen::aligned_allocator<RhsVector>> rhs_;
  SharedDiagonal model_;

  template <size_t... Indices>
  void reserveBlocks(size_t rowCount, std::index_sequence<Indices...>) {
    (std::get<Indices>(blocks_).reserve(rowCount), ...);
  }

  template <size_t Slot>
  void addBlock(const Matrix& block) {
    using BlockVectorType = typename std::tuple_element<Slot, Blocks>::type;
    using BlockType = typename BlockVectorType::value_type;
    constexpr int BlockDim = BlockType::ColsAtCompileTime;
    if (block.rows() != ErrorDim || block.cols() != BlockDim) {
      throw std::invalid_argument(
          "BatchJacobianFactor::addRow: incompatible block dimension.");
    }
    std::get<Slot>(blocks_).push_back(block);
  }

  template <size_t... Indices>
  void addBlocks(const std::vector<Matrix>& blocks,
                 std::index_sequence<Indices...>) {
    (addBlock<Indices>(blocks[Indices]), ...);
  }

  template <size_t Slot>
  void copyBlockToDense(size_t rowIndex, VerticalBlockMatrix* dense) const {
    using BlockVectorType = typename std::tuple_element<Slot, Blocks>::type;
    using BlockType = typename BlockVectorType::value_type;
    constexpr int BlockDim = BlockType::ColsAtCompileTime;
    const DenseIndex keySlot = rowSlots_[rowIndex][Slot];
    (*dense)(keySlot).block(static_cast<DenseIndex>(rowIndex * ErrorDim), 0,
                            ErrorDim, BlockDim) =
        std::get<Slot>(blocks_)[rowIndex];
  }

  template <size_t... Indices>
  void copyBlocksToDense(size_t rowIndex, VerticalBlockMatrix* dense,
                         std::index_sequence<Indices...>) const {
    (copyBlockToDense<Indices>(rowIndex, dense), ...);
  }

  template <size_t Slot>
  void scatterBlock(size_t rowIndex, size_t rowOffset,
                    VerticalBlockMatrix* target,
                    const std::vector<DenseIndex>& targetBlockIndices) const {
    using BlockVectorType = typename std::tuple_element<Slot, Blocks>::type;
    using BlockType = typename BlockVectorType::value_type;
    constexpr int BlockDim = BlockType::ColsAtCompileTime;
    const DenseIndex keySlot = rowSlots_[rowIndex][Slot];
    const DenseIndex targetBlock = targetBlockIndices[keySlot];
    if (targetBlock < 0) return;
    (*target)(targetBlock)
        .block(static_cast<DenseIndex>(rowOffset + rowIndex * ErrorDim), 0,
               ErrorDim, BlockDim) = std::get<Slot>(blocks_)[rowIndex];
  }

  template <size_t... Indices>
  void scatterBlocks(size_t rowIndex, size_t rowOffset,
                     VerticalBlockMatrix* target,
                     const std::vector<DenseIndex>& targetBlockIndices,
                     std::index_sequence<Indices...>) const {
    (scatterBlock<Indices>(rowIndex, rowOffset, target, targetBlockIndices),
     ...);
  }

  static bool slotInRange(DenseIndex slot, DenseIndex beginCol,
                          DenseIndex endCol) {
    return internal::BlockColumnRange{beginCol, endCol}(slot);
  }

  template <size_t Slot>
  void updateAugmentedDiagonal(size_t rowIndex, DenseIndex targetSlot,
                               DenseIndex targetScalarOffset,
                               const RhsVector* weights,
                               SymmetricBlockMatrix* info) const {
    if (targetSlot < 0) return;
    if constexpr (Slot == NumSlots) {
      const RhsVector& b = rhs_[rowIndex];
      Eigen::Matrix<double, 1, 1> contribution;
      contribution(0, 0) = weights
                               ? (weights->array() * b.array().square()).sum()
                               : b.squaredNorm();
      if (targetScalarOffset >= 0) {
        info->updateFixedDiagonalBlockAt<1>(targetScalarOffset, contribution);
      } else {
        info->updateDiagonalBlock(targetSlot, contribution);
      }
    } else {
      using BlockType =
          typename std::tuple_element<Slot, Blocks>::type::value_type;
      constexpr int BlockDim = BlockType::ColsAtCompileTime;
      const auto& A = std::get<Slot>(blocks_)[rowIndex];
      Eigen::Matrix<double, BlockDim, BlockDim> contribution;
      if (weights) {
        contribution.noalias() = A.transpose() * weights->asDiagonal() * A;
      } else {
        contribution.noalias() = A.transpose() * A;
      }
      if (targetScalarOffset >= 0) {
        info->updateFixedDiagonalBlockAt<BlockDim>(targetScalarOffset,
                                                   contribution);
      } else {
        info->updateDiagonalBlock(targetSlot, contribution);
      }
    }
  }

  template <typename MatrixType>
  void updateOffDiagonalNormalized(DenseIndex targetI, DenseIndex targetJ,
                                   DenseIndex scalarOffsetI,
                                   DenseIndex scalarOffsetJ,
                                   const MatrixType& block,
                                   SymmetricBlockMatrix* info) const {
    assert((targetI != targetJ) &&
           "BatchJacobianFactor: duplicate mapped Hessian slots are not "
           "supported.");
    constexpr int Rows = MatrixType::RowsAtCompileTime;
    constexpr int Cols = MatrixType::ColsAtCompileTime;
    static_assert(Rows != Eigen::Dynamic && Cols != Eigen::Dynamic);
    if (scalarOffsetI >= 0 && scalarOffsetJ >= 0) {
      if (targetI < targetJ) {
        info->updateFixedOffDiagonalBlockAt<Rows, Cols>(scalarOffsetI,
                                                        scalarOffsetJ, block);
      } else {
        info->updateFixedOffDiagonalBlockAt<Cols, Rows>(
            scalarOffsetJ, scalarOffsetI, block.transpose());
      }
      return;
    }
    if (targetI < targetJ) {
      info->updateOffDiagonalBlock(targetI, targetJ, block);
      return;
    }
    info->updateOffDiagonalBlock(targetJ, targetI, block.transpose());
  }

  template <size_t I, size_t J>
  void updateAugmentedOffDiagonal(size_t rowIndex, DenseIndex targetI,
                                  DenseIndex targetJ, DenseIndex scalarOffsetI,
                                  DenseIndex scalarOffsetJ,
                                  const RhsVector* weights,
                                  SymmetricBlockMatrix* info) const {
    static_assert(I < J, "BatchJacobianFactor expects upper-triangular order.");
    if constexpr (J == NumSlots) {
      using BlockType =
          typename std::tuple_element<I, Blocks>::type::value_type;
      constexpr int BlockDim = BlockType::ColsAtCompileTime;
      const auto& A = std::get<I>(blocks_)[rowIndex];
      const RhsVector& b = rhs_[rowIndex];
      Eigen::Matrix<double, BlockDim, 1> contribution;
      if (weights) {
        const RhsVector weightedRhs = weights->asDiagonal() * b;
        contribution.noalias() = A.transpose() * weightedRhs;
      } else {
        contribution.noalias() = A.transpose() * b;
      }
      updateOffDiagonalNormalized(targetI, targetJ, scalarOffsetI,
                                  scalarOffsetJ, contribution, info);
    } else {
      using BlockTypeI =
          typename std::tuple_element<I, Blocks>::type::value_type;
      using BlockTypeJ =
          typename std::tuple_element<J, Blocks>::type::value_type;
      constexpr int BlockDimI = BlockTypeI::ColsAtCompileTime;
      constexpr int BlockDimJ = BlockTypeJ::ColsAtCompileTime;
      const auto& Ai = std::get<I>(blocks_)[rowIndex];
      const auto& Aj = std::get<J>(blocks_)[rowIndex];
      Eigen::Matrix<double, BlockDimI, BlockDimJ> contribution;
      if (weights) {
        contribution.noalias() = Ai.transpose() * weights->asDiagonal() * Aj;
      } else {
        contribution.noalias() = Ai.transpose() * Aj;
      }
      updateOffDiagonalNormalized(targetI, targetJ, scalarOffsetI,
                                  scalarOffsetJ, contribution, info);
    }
  }

  template <size_t J, size_t... Is>
  void updateMappedPreviousAugmentedSlots(
      size_t rowIndex, const DenseIndex* mappedSlots,
      const DenseIndex* mappedScalarOffsets, const RhsVector* weights,
      SymmetricBlockMatrix* info, DenseIndex beginCol, DenseIndex endCol,
      std::index_sequence<Is...>) const {
    (void)rowIndex;
    (void)weights;
    (void)info;
    (void)beginCol;
    (void)endCol;
    const DenseIndex targetSlot = mappedSlots[J];
    ((mappedSlots[Is] >= 0 && targetSlot >= 0 &&
              slotInRange(std::max(mappedSlots[Is], targetSlot), beginCol,
                          endCol)
          ? updateAugmentedOffDiagonal<Is, J>(
                rowIndex, mappedSlots[Is], targetSlot,
                mappedScalarOffsets ? mappedScalarOffsets[Is] : -1,
                mappedScalarOffsets ? mappedScalarOffsets[J] : -1, weights,
                info)
          : void()),
     ...);
  }

  template <size_t J>
  void updateMappedAugmentedColumn(size_t rowIndex,
                                   const DenseIndex* mappedSlots,
                                   const DenseIndex* mappedScalarOffsets,
                                   const RhsVector* weights,
                                   SymmetricBlockMatrix* info,
                                   DenseIndex beginCol,
                                   DenseIndex endCol) const {
    const DenseIndex targetSlot = mappedSlots[J];
    if (slotInRange(targetSlot, beginCol, endCol)) {
      updateAugmentedDiagonal<J>(
          rowIndex, targetSlot,
          mappedScalarOffsets ? mappedScalarOffsets[J] : -1, weights, info);
    }
    updateMappedPreviousAugmentedSlots<J>(
        rowIndex, mappedSlots, mappedScalarOffsets, weights, info, beginCol,
        endCol, std::make_index_sequence<J>{});
  }

  template <size_t... Js>
  void updateMappedAugmentedColumns(size_t rowIndex,
                                    const DenseIndex* mappedSlots,
                                    const DenseIndex* mappedScalarOffsets,
                                    const RhsVector* weights,
                                    SymmetricBlockMatrix* info,
                                    DenseIndex beginCol, DenseIndex endCol,
                                    std::index_sequence<Js...>) const {
    (updateMappedAugmentedColumn<Js>(rowIndex, mappedSlots, mappedScalarOffsets,
                                     weights, info, beginCol, endCol),
     ...);
  }

  void updateMappedHessianRow(size_t rowIndex, const DenseIndex* mappedSlots,
                              const DenseIndex* mappedScalarOffsets,
                              const RhsVector* weights,
                              SymmetricBlockMatrix* info, DenseIndex beginCol,
                              DenseIndex endCol) const {
    updateMappedAugmentedColumns(rowIndex, mappedSlots, mappedScalarOffsets,
                                 weights, info, beginCol, endCol,
                                 std::make_index_sequence<NumSlots + 1>{});
  }

  template <typename DestinationType, typename XprType>
  static void addFrontalBlock(DestinationType* destination,
                              const XprType& xpr) {
    if constexpr (XprType::SizeAtCompileTime == 1) {
      assert(destination->rows() == 1 && destination->cols() == 1);
      (*destination)(0, 0) += xpr.coeff(0, 0);
    } else {
      *destination += xpr.eval();
    }
  }

  template <size_t I, size_t J>
  void updateFrontalHessianBlock(size_t rowIndex, const DenseIndex* mappedSlots,
                                 const RhsVector* weights,
                                 DenseIndex numFrontalBlocks,
                                 VerticalBlockMatrix* frontalRows) const {
    static_assert(I < NumSlots);
    static_assert(J <= NumSlots);
    const DenseIndex targetI = mappedSlots[I];
    const DenseIndex targetJ = mappedSlots[J];
    if (targetI < 0 || targetI >= numFrontalBlocks || targetJ < 0) return;
 
    const auto& Ai = std::get<I>(blocks_)[rowIndex];
    const DenseIndex targetRow = frontalRows->offset(targetI);
    auto destination = (*frontalRows)(targetJ).middleRows(
        targetRow, static_cast<DenseIndex>(Ai.cols()));
    if constexpr (J == NumSlots) {
      const RhsVector& b = rhs_[rowIndex];
      if (weights) {
        addFrontalBlock(&destination,
                        Ai.transpose() * weights->asDiagonal() * b);
      } else {
        addFrontalBlock(&destination, Ai.transpose() * b);
      }
    } else {
      const auto& Aj = std::get<J>(blocks_)[rowIndex];
      if (weights) {
        addFrontalBlock(&destination,
                        Ai.transpose() * weights->asDiagonal() * Aj);
      } else {
        addFrontalBlock(&destination, Ai.transpose() * Aj);
      }
    }
  }

  template <size_t I, size_t... Js>
  void updateFrontalHessianRowBlock(size_t rowIndex,
                                    const DenseIndex* mappedSlots,
                                    const RhsVector* weights,
                                    DenseIndex numFrontalBlocks,
                                    VerticalBlockMatrix* frontalRows,
                                    std::index_sequence<Js...>) const {
    (updateFrontalHessianBlock<I, Js>(rowIndex, mappedSlots, weights,
                                      numFrontalBlocks, frontalRows),
     ...);
  }

  template <size_t... Is>
  void updateFrontalHessianRowGroup(size_t rowIndex,
                                    const DenseIndex* mappedSlots,
                                    const RhsVector* weights,
                                    DenseIndex numFrontalBlocks,
                                    VerticalBlockMatrix* frontalRows,
                                    std::index_sequence<Is...>) const {
    (updateFrontalHessianRowBlock<Is>(rowIndex, mappedSlots, weights,
                                      numFrontalBlocks, frontalRows,
                                      std::make_index_sequence<NumSlots + 1>{}),
     ...);
  }

  RhsVector rowWeights(size_t rowIndex) const {
    RhsVector weights = RhsVector::Ones();
    if (!model_ || model_->isUnit()) return weights;
 
    const auto constrained =
        model_->isConstrained()
            ? std::dynamic_pointer_cast<noiseModel::Constrained>(model_)
            : nullptr;
    const size_t rowOffset = rowIndex * ErrorDim;
    for (size_t row = 0; row < ErrorDim; ++row) {
      const size_t modelRow = rowOffset + row;
      if (!constrained || !constrained->constrained(modelRow)) {
        weights(static_cast<DenseIndex>(row)) = model_->precision(modelRow);
      }
    }
    return weights;
  }

  template <size_t Slot>
  void multiplyRowBlock(size_t rowIndex,
                        const std::vector<size_t>& scalarOffsets,
                        const double* x, RhsVector* residual) const {
    using BlockType =
        typename std::tuple_element<Slot, Blocks>::type::value_type;
    constexpr int BlockDim = BlockType::ColsAtCompileTime;
    const size_t keySlot = static_cast<size_t>(rowSlots_[rowIndex][Slot]);
    const Eigen::Map<const Eigen::Matrix<double, BlockDim, 1>> xBlock(
        x + scalarOffsets[keySlot]);
    residual->noalias() += std::get<Slot>(blocks_)[rowIndex] * xBlock;
  }

  template <size_t... Slots>
  void multiplyRowBlocks(size_t rowIndex,
                         const std::vector<size_t>& scalarOffsets,
                         const double* x, RhsVector* residual,
                         std::index_sequence<Slots...>) const {
    (multiplyRowBlock<Slots>(rowIndex, scalarOffsets, x, residual), ...);
  }

  template <size_t Slot>
  void addVectorValuesRowBlock(size_t rowIndex, const VectorValues& values,
                               RhsVector* residual) const {
    const size_t keySlot = static_cast<size_t>(rowSlots_[rowIndex][Slot]);
    residual->noalias() +=
        std::get<Slot>(blocks_)[rowIndex] * values.at(keys_[keySlot]);
  }

  template <size_t... Slots>
  void addVectorValuesRowBlocks(size_t rowIndex, const VectorValues& values,
                                RhsVector* residual,
                                std::index_sequence<Slots...>) const {
    (addVectorValuesRowBlock<Slots>(rowIndex, values, residual), ...);
  }

  template <size_t Slot>
  void hessianDiagonalVectorRowAdd(size_t rowIndex,
                                   VectorValues* diagonal) const {
    const size_t keySlot = static_cast<size_t>(rowSlots_[rowIndex][Slot]);
    const auto& block = std::get<Slot>(blocks_)[rowIndex];
    diagonal->at(keys_[keySlot]).array() +=
        block.array().square().colwise().sum().transpose();
  }

  template <size_t... Slots>
  void hessianDiagonalVectorRowAdds(size_t rowIndex, VectorValues* diagonal,
                                    std::index_sequence<Slots...>) const {
    (hessianDiagonalVectorRowAdd<Slots>(rowIndex, diagonal), ...);
  }

  template <size_t Slot>
  void transposeRowBlockAdd(size_t rowIndex,
                            const std::vector<size_t>& scalarOffsets,
                            double alpha, const RhsVector& residual,
                            double* y) const {
    using BlockType =
        typename std::tuple_element<Slot, Blocks>::type::value_type;
    constexpr int BlockDim = BlockType::ColsAtCompileTime;
    const size_t keySlot = static_cast<size_t>(rowSlots_[rowIndex][Slot]);
    Eigen::Map<Eigen::Matrix<double, BlockDim, 1>> yBlock(
        y + scalarOffsets[keySlot]);
    yBlock.noalias() +=
        alpha * std::get<Slot>(blocks_)[rowIndex].transpose() * residual;
  }

  template <size_t... Slots>
  void transposeRowBlocksAdd(size_t rowIndex,
                             const std::vector<size_t>& scalarOffsets,
                             double alpha, const RhsVector& residual, double* y,
                             std::index_sequence<Slots...>) const {
    (transposeRowBlockAdd<Slots>(rowIndex, scalarOffsets, alpha, residual, y),
     ...);
  }

  template <size_t Slot>
  void hessianBlockDiagonalRowAdd(size_t rowIndex,
                                  const std::vector<size_t>& blockSlots,
                                  const RhsVector& weights,
                                  std::vector<Matrix>* diagonalBlocks) const {
    const size_t keySlot = static_cast<size_t>(rowSlots_[rowIndex][Slot]);
    const auto& block = std::get<Slot>(blocks_)[rowIndex];
    (*diagonalBlocks)[blockSlots[keySlot]].noalias() +=
        block.transpose() * weights.asDiagonal() * block;
  }

  template <size_t... Slots>
  void hessianBlockDiagonalRowAdds(size_t rowIndex,
                                   const std::vector<size_t>& blockSlots,
                                   const RhsVector& weights,
                                   std::vector<Matrix>* diagonalBlocks,
                                   std::index_sequence<Slots...>) const {
    (hessianBlockDiagonalRowAdd<Slots>(rowIndex, blockSlots, weights,
                                       diagonalBlocks),
     ...);
  }

  template <size_t I, size_t J>
  void updateSparseNormalBlock(
      size_t rowIndex, const std::vector<DenseIndex>& scalarOffsets,
      const RhsVector& weights,
      internal::SparseNormalAccumulator* accumulator) const {
    using BlockTypeI = typename std::tuple_element<I, Blocks>::type::value_type;
    using BlockTypeJ = typename std::tuple_element<J, Blocks>::type::value_type;
    constexpr int BlockDimI = BlockTypeI::ColsAtCompileTime;
    constexpr int BlockDimJ = BlockTypeJ::ColsAtCompileTime;
    const auto& blockI = std::get<I>(blocks_)[rowIndex];
    const auto& blockJ = std::get<J>(blocks_)[rowIndex];
    Eigen::Matrix<double, BlockDimI, BlockDimJ> contribution;
    contribution.noalias() = blockI.transpose() * weights.asDiagonal() * blockJ;
    const size_t keySlotI = static_cast<size_t>(rowSlots_[rowIndex][I]);
    const size_t keySlotJ = static_cast<size_t>(rowSlots_[rowIndex][J]);
    accumulator->addHessianBlock(scalarOffsets[keySlotI],
                                 scalarOffsets[keySlotJ], BlockDimI, BlockDimJ,
                                 contribution.data());
  }

  template <size_t J, size_t... Is>
  void updateSparseNormalColumn(size_t rowIndex,
                                const std::vector<DenseIndex>& scalarOffsets,
                                const RhsVector& weights,
                                internal::SparseNormalAccumulator* accumulator,
                                std::index_sequence<Is...>) const {
    (updateSparseNormalBlock<Is, J>(rowIndex, scalarOffsets, weights,
                                    accumulator),
     ...);
  }

  template <size_t... Js>
  void updateSparseNormalBlocks(size_t rowIndex,
                                const std::vector<DenseIndex>& scalarOffsets,
                                const RhsVector& weights,
                                internal::SparseNormalAccumulator* accumulator,
                                std::index_sequence<Js...>) const {
    (updateSparseNormalColumn<Js>(rowIndex, scalarOffsets, weights, accumulator,
                                  std::make_index_sequence<Js + 1>{}),
     ...);
  }

  template <size_t Slot>
  void updateSparseNormalRhs(
      size_t rowIndex, const std::vector<DenseIndex>& scalarOffsets,
      const RhsVector& weightedRhs,
      internal::SparseNormalAccumulator* accumulator) const {
    using BlockType =
        typename std::tuple_element<Slot, Blocks>::type::value_type;
    constexpr int BlockDim = BlockType::ColsAtCompileTime;
    Eigen::Matrix<double, BlockDim, 1> contribution;
    contribution.noalias() =
        std::get<Slot>(blocks_)[rowIndex].transpose() * weightedRhs;
    const size_t keySlot = static_cast<size_t>(rowSlots_[rowIndex][Slot]);
    accumulator->addRhsBlock(scalarOffsets[keySlot], BlockDim,
                             contribution.data());
  }

  template <size_t... Slots>
  void updateSparseNormalRhsBlocks(
      size_t rowIndex, const std::vector<DenseIndex>& scalarOffsets,
      const RhsVector& weightedRhs,
      internal::SparseNormalAccumulator* accumulator,
      std::index_sequence<Slots...>) const {
    (updateSparseNormalRhs<Slots>(rowIndex, scalarOffsets, weightedRhs,
                                  accumulator),
     ...);
  }
 
 public:
  BatchJacobianFactor(const KeyVector& keys, std::vector<size_t> keyDims,
                      const SharedDiagonal& model = SharedDiagonal())
      : Base(keys), keyDims_(std::move(keyDims)), model_(model) {
    if (keyDims_.size() != keys_.size()) {
      throw std::invalid_argument(
          "BatchJacobianFactor: key dimension count must match keys.");
    }
  }

  GaussianFactor::shared_ptr clone() const override {
    return std::static_pointer_cast<GaussianFactor>(
        std::make_shared<This>(*this));
  }

  void reserve(size_t rowCount) {
    rowSlots_.reserve(rowCount);
    rhs_.reserve(rowCount);
    reserveBlocks(rowCount, std::make_index_sequence<NumSlots>{});
  }

  void addRow(const SlotIndices& slots, const std::vector<Matrix>& blocks,
              const Vector& rhs) {
    if (blocks.size() != NumSlots || rhs.size() != ErrorDim) {
      throw std::invalid_argument(
          "BatchJacobianFactor::addRow: incompatible row dimensions.");
    }
    rowSlots_.push_back(slots);
    addBlocks(blocks, std::make_index_sequence<NumSlots>{});
    RhsVector fixedRhs = rhs;
    rhs_.push_back(fixedRhs);
  }

  void addUnaryRow(
      DenseIndex keySlot,
      const typename std::tuple_element<0, Blocks>::type::value_type& block,
      const RhsVector& rhs) {
    static_assert(NumSlots == 1,
                  "BatchJacobianFactor::addUnaryRow requires one slot.");
    rowSlots_.push_back(SlotIndices{keySlot});
    std::get<0>(blocks_).push_back(block);
    rhs_.push_back(rhs);
  }

  size_t rows() const override { return rhs_.size() * ErrorDim; }

  const SharedDiagonal& get_model() const override { return model_; }

  DenseIndex getDim(const_iterator variable) const override {
    return static_cast<DenseIndex>(keyDims_.at(variable - begin()));
  }

  const std::vector<SlotIndices>& rowSlots() const { return rowSlots_; }

  template <size_t Slot>
  const typename std::tuple_element<Slot, Blocks>::type::value_type& block(
      size_t rowIndex) const {
    static_assert(Slot < NumSlots,
                  "BatchJacobianFactor block slot is invalid.");
    return std::get<Slot>(blocks_).at(rowIndex);
  }

  const RhsVector& rowRhs(size_t rowIndex) const { return rhs_.at(rowIndex); }

  double deltaError(const VectorValues& values, double* oldError = nullptr,
                    double* newError = nullptr) const override {
    if (model_ && !model_->isUnit()) {
      return Base::deltaError(values, oldError, newError);
    }
 
    double oldValue = 0.0;
    double newValue = 0.0;
    for (size_t row = 0; row < rowSlots_.size(); ++row) {
      const RhsVector& rhs = rhs_[row];
      RhsVector residual = -rhs;
      addVectorValuesRowBlocks(row, values, &residual,
                               std::make_index_sequence<NumSlots>{});
      oldValue += 0.5 * rhs.squaredNorm();
      newValue += 0.5 * residual.squaredNorm();
    }
    if (oldError) *oldError = oldValue;
    if (newError) *newError = newValue;
    return oldValue - newValue;
  }

  void hessianDiagonalAdd(VectorValues& diagonal) const override {
    if (model_ && !model_->isUnit()) {
      Base::hessianDiagonalAdd(diagonal);
      return;
    }
    for (size_t position = 0; position < keys_.size(); ++position) {
      auto [entry, inserted] =
          diagonal.emplace(keys_[position], keyDims_[position]);
      if (inserted) entry->second.setZero();
    }
    for (size_t row = 0; row < rowSlots_.size(); ++row) {
      hessianDiagonalVectorRowAdds(row, &diagonal,
                                   std::make_index_sequence<NumSlots>{});
    }
  }

  void multiplyHessianAdd(double alpha,
                          const std::vector<size_t>& scalarOffsets,
                          const double* x, double* y) const override {
    if (scalarOffsets.size() != keys_.size()) {
      throw std::invalid_argument(
          "BatchJacobianFactor::multiplyHessianAdd: offset count mismatch.");
    }
    for (size_t rowIndex = 0; rowIndex < rowSlots_.size(); ++rowIndex) {
      RhsVector residual = RhsVector::Zero();
      multiplyRowBlocks(rowIndex, scalarOffsets, x, &residual,
                        std::make_index_sequence<NumSlots>{});
      residual.array() *= rowWeights(rowIndex).array();
      transposeRowBlocksAdd(rowIndex, scalarOffsets, alpha, residual, y,
                            std::make_index_sequence<NumSlots>{});
    }
  }

  void gradientAtZeroAdd(const std::vector<size_t>& scalarOffsets,
                         double* gradient) const override {
    if (scalarOffsets.size() != keys_.size()) {
      throw std::invalid_argument(
          "BatchJacobianFactor::gradientAtZeroAdd: offset count mismatch.");
    }
    for (size_t rowIndex = 0; rowIndex < rowSlots_.size(); ++rowIndex) {
      RhsVector weightedRhs = rhs_[rowIndex];
      weightedRhs.array() *= rowWeights(rowIndex).array();
      transposeRowBlocksAdd(rowIndex, scalarOffsets, -1.0, weightedRhs,
                            gradient, std::make_index_sequence<NumSlots>{});
    }
  }

  void hessianBlockDiagonalAdd(
      const std::vector<size_t>& blockSlots,
      std::vector<Matrix>* diagonalBlocks) const override {
    if (blockSlots.size() != keys_.size()) {
      throw std::invalid_argument(
          "BatchJacobianFactor::hessianBlockDiagonalAdd: slot count "
          "mismatch.");
    }
    for (size_t rowIndex = 0; rowIndex < rowSlots_.size(); ++rowIndex) {
      hessianBlockDiagonalRowAdds(rowIndex, blockSlots, rowWeights(rowIndex),
                                  diagonalBlocks,
                                  std::make_index_sequence<NumSlots>{});
    }
  }

  JacobianFactor toJacobianFactor() const override {
    if (rowSlots_.empty()) return JacobianFactor();
    VerticalBlockMatrix dense(keyDims_, static_cast<DenseIndex>(rows()), true);
    dense.matrix().setZero();
    for (size_t rowIndex = 0; rowIndex < rowSlots_.size(); ++rowIndex) {
      copyBlocksToDense(rowIndex, &dense, std::make_index_sequence<NumSlots>{});
      dense(keys_.size())
          .block(static_cast<DenseIndex>(rowIndex * ErrorDim), 0, ErrorDim, 1) =
          rhs_[rowIndex];
    }
    return JacobianFactor(keys_, std::move(dense), model_);
  }
 
 private:
  size_t scatterInto(
      VerticalBlockMatrix& target, size_t rowOffset,
      const std::vector<DenseIndex>& targetBlockIndices) const override {
    if (targetBlockIndices.size() != keys_.size()) {
      throw std::invalid_argument(
          "BatchJacobianFactor::scatterInto: target index count mismatch.");
    }
    const size_t rhsBlock = target.nBlocks() - 1;
    for (size_t rowIndex = 0; rowIndex < rowSlots_.size(); ++rowIndex) {
      scatterBlocks(rowIndex, rowOffset, &target, targetBlockIndices,
                    std::make_index_sequence<NumSlots>{});
      target(rhsBlock).block(
          static_cast<DenseIndex>(rowOffset + rowIndex * ErrorDim), 0, ErrorDim,
          1) = rhs_[rowIndex];
    }
    return rows();
  }
 
  void updateSparseNormal(
      const std::vector<DenseIndex>& scalarOffsets,
      internal::SparseNormalAccumulator* accumulator) const override {
    if (!accumulator) {
      throw std::invalid_argument(
          "BatchJacobianFactor::updateSparseNormal: null accumulator.");
    }
    if (scalarOffsets.size() != keys_.size()) {
      throw std::invalid_argument(
          "BatchJacobianFactor::updateSparseNormal: offset count mismatch.");
    }
    if (model_ && !model_->isUnit() && model_->isConstrained()) {
      throw std::invalid_argument(
          "BatchJacobianFactor::updateSparseNormal: constrained noise model "
          "is not supported.");
    }
    for (size_t rowIndex = 0; rowIndex < rowSlots_.size(); ++rowIndex) {
      const RhsVector weights = rowWeights(rowIndex);
      updateSparseNormalBlocks(rowIndex, scalarOffsets, weights, accumulator,
                               std::make_index_sequence<NumSlots>{});
      const RhsVector weightedRhs = weights.asDiagonal() * rhs_[rowIndex];
      updateSparseNormalRhsBlocks(rowIndex, scalarOffsets, weightedRhs,
                                  accumulator,
                                  std::make_index_sequence<NumSlots>{});
    }
  }

  void updateHessian(const std::vector<DenseIndex>& slotIndices,
                     SymmetricBlockMatrix* info) const override {
    if (rows() == 0) return;
    const DenseIndex rhsSlot = static_cast<DenseIndex>(info->nBlocks() - 1);
    std::vector<DenseIndex> mappedSlots;
    if (slotIndices.size() == keys_.size() + 1 && !slotIndices.empty() &&
        slotIndices.back() == rhsSlot) {
      buildMappedSlots(slotIndices, mappedSlots);
      updateHessianWithMappedSlots(mappedSlots, nullptr, info, 0,
                                   info->nBlocks());
      return;
    }
    if (slotIndices.size() != keys_.size()) {
      throw std::invalid_argument(
          "BatchJacobianFactor::updateHessian: slot index count mismatch.");
    }
 
    std::vector<DenseIndex> slots = slotIndices;
    slots.push_back(rhsSlot);
    buildMappedSlots(slots, mappedSlots);
    updateHessianWithMappedSlots(mappedSlots, nullptr, info, 0,
                                 info->nBlocks());
  }

  void updateHessian(const std::vector<DenseIndex>& slotIndices,
                     SymmetricBlockMatrix* info, DenseIndex beginCol,
                     DenseIndex endCol) const override {
    if (rows() == 0) return;
    if (slotIndices.size() != keys_.size()) {
      throw std::invalid_argument(
          "BatchJacobianFactor::updateHessian: slot index count mismatch.");
    }
 
    std::vector<DenseIndex> slots;
    slots.reserve(slotIndices.size() + 1);
    bool foundCol = false;
    for (const DenseIndex slot : slotIndices) {
      slots.push_back(slot);
      if (slotInRange(slot, beginCol, endCol)) foundCol = true;
    }
    slots.push_back(info->nBlocks() - 1);
    if (slotInRange(slots.back(), beginCol, endCol)) foundCol = true;
    if (!foundCol) return;
 
    std::vector<DenseIndex> mappedSlots;
    buildMappedSlots(slots, mappedSlots);
    updateHessianWithMappedSlots(mappedSlots, nullptr, info, beginCol, endCol);
  }

  void buildMappedSlots(const std::vector<DenseIndex>& slotIndices,
                        std::vector<DenseIndex>& mappedSlots) const override {
    const size_t stride = NumSlots + 1;
    if (slotIndices.size() != keys_.size() + 1) {
      throw std::invalid_argument(
          "BatchJacobianFactor::buildMappedSlots: slot index count mismatch.");
    }
    mappedSlots.resize(rowSlots_.size() * stride);
    auto* out = mappedSlots.data();
    for (const auto& rowSlot : rowSlots_) {
      for (size_t j = 0; j < NumSlots; ++j) {
        *out++ = slotIndices[rowSlot[j]];
      }
      *out++ = slotIndices.back();
    }
  }

  void updateHessianWithMappedSlots(const std::vector<DenseIndex>& mappedSlots,
                                    SymmetricBlockMatrix* info) const override {
    updateHessianWithMappedSlots(mappedSlots, nullptr, info, 0,
                                 info->nBlocks());
  }
 
  void updateHessianWithMappedSlots(
      const std::vector<DenseIndex>& mappedSlots,
      const std::vector<DenseIndex>& mappedScalarOffsets,
      SymmetricBlockMatrix* info) const override {
    updateHessianWithMappedSlots(mappedSlots, &mappedScalarOffsets, info, 0,
                                 info->nBlocks());
  }
 
  void updateFrontalHessianWithMappedSlots(
      const std::vector<DenseIndex>& mappedSlots, DenseIndex numFrontalBlocks,
      VerticalBlockMatrix* frontalRows) const override {
    if (rows() == 0) return;
    const size_t stride = NumSlots + 1;
    if (mappedSlots.size() != rowSlots_.size() * stride) {
      throw std::invalid_argument(
          "BatchJacobianFactor::updateFrontalHessianWithMappedSlots: mapped "
          "slot count mismatch.");
    }
    RhsVector weights;
    const RhsVector* weightsPtr = nullptr;
    const DenseIndex* rowSlotsPtr = mappedSlots.data();
    for (size_t rowIndex = 0; rowIndex < rowSlots_.size(); ++rowIndex) {
      if (model_ && !model_->isUnit()) {
        weights = rowWeights(rowIndex);
        weightsPtr = &weights;
      }
      updateFrontalHessianRowGroup(rowIndex, rowSlotsPtr + rowIndex * stride,
                                   weightsPtr, numFrontalBlocks, frontalRows,
                                   std::make_index_sequence<NumSlots>{});
    }
  }
 
 private:
  void updateHessianWithMappedSlots(
      const std::vector<DenseIndex>& mappedSlots,
      const std::vector<DenseIndex>* mappedScalarOffsets,
      SymmetricBlockMatrix* info, DenseIndex beginCol,
      DenseIndex endCol) const {
    gttic(updateHessian_BatchJacobianFactor);
    if (rows() == 0) return;
    if (model_ && !model_->isUnit() && model_->isConstrained()) {
      throw std::invalid_argument(
          "BatchJacobianFactor::updateHessian: cannot update information with "
          "constrained noise model");
    }
    const size_t stride = NumSlots + 1;
    if (mappedSlots.size() != rowSlots_.size() * stride) {
      throw std::invalid_argument(
          "BatchJacobianFactor::updateHessianWithMappedSlots: mapped slot "
          "count mismatch.");
    }
    if (mappedScalarOffsets &&
        mappedScalarOffsets->size() != mappedSlots.size()) {
      throw std::invalid_argument(
          "BatchJacobianFactor::updateHessianWithMappedSlots: mapped scalar "
          "offset count mismatch.");
    }
 
    const DenseIndex rhsSlot = static_cast<DenseIndex>(info->nBlocks() - 1);
    for (const DenseIndex slot : mappedSlots) {
      if (slot > rhsSlot) {
        throw std::invalid_argument(
            "BatchJacobianFactor::updateHessianWithMappedSlots: invalid "
            "mapped slot index.");
      }
    }
#ifndef NDEBUG
    if (mappedScalarOffsets) {
      for (size_t i = 0; i < mappedSlots.size(); ++i) {
        const DenseIndex slot = mappedSlots[i];
        const DenseIndex scalarOffset = (*mappedScalarOffsets)[i];
        assert((slot < 0 && scalarOffset < 0) ||
               (slot >= 0 && scalarOffset == info->blockScalarOffset(slot)));
      }
    }
#endif
 
    RhsVector weights;
    const RhsVector* weightsPtr = nullptr;
    if (model_ && !model_->isUnit()) {
      for (size_t rowIndex = 0; rowIndex < rowSlots_.size(); ++rowIndex) {
        weights = model_->invsigmas()
                      .template segment<ErrorDim>(
                          static_cast<DenseIndex>(rowIndex * ErrorDim))
                      .array()
                      .square();
        weightsPtr = &weights;
        updateMappedHessianRow(
            rowIndex, mappedSlots.data() + rowIndex * stride,
            mappedScalarOffsets
                ? mappedScalarOffsets->data() + rowIndex * stride
                : nullptr,
            weightsPtr, info, beginCol, endCol);
      }
      return;
    }
 
    const DenseIndex* rowSlotsPtr = mappedSlots.data();
    const DenseIndex* rowOffsetsPtr =
        mappedScalarOffsets ? mappedScalarOffsets->data() : nullptr;
    for (size_t rowIndex = 0; rowIndex < rowSlots_.size(); ++rowIndex) {
      updateMappedHessianRow(
          rowIndex, rowSlotsPtr + rowIndex * stride,
          rowOffsetsPtr ? rowOffsetsPtr + rowIndex * stride : nullptr, nullptr,
          info, beginCol, endCol);
    }
  }
 
 public:
  void updateHessian(const KeyVector& infoKeys,
                     SymmetricBlockMatrix* info) const override {
    std::vector<DenseIndex> slots;
    slots.reserve(keys_.size() + 1);
    for (Key key : keys_) {
      slots.push_back(Slot(infoKeys, key));
    }
    slots.push_back(info->nBlocks() - 1);
    updateHessian(slots, info);
  }

  void updateHessian(const KeyVector& infoKeys, SymmetricBlockMatrix* info,
                     DenseIndex beginCol, DenseIndex endCol) const override {
    std::vector<DenseIndex> slots;
    slots.reserve(keys_.size());
    for (Key key : keys_) {
      slots.push_back(Slot(infoKeys, key));
    }
    updateHessian(slots, info, beginCol, endCol);
  }
};
 
}  // namespace gtsam