BinaryJacobianFactor.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/JacobianFactor.h>
#include <gtsam/base/SymmetricBlockMatrix.h>
#include <gtsam/base/timing.h>

namespace gtsam {

template<int M, int N1, int N2>
struct BinaryJacobianFactor: JacobianFactor {

  BinaryJacobianFactor(Key key1, const Eigen::Matrix<double, M, N1>& A1,
      Key key2, const Eigen::Matrix<double, M, N2>& A2,
      const Eigen::Matrix<double, M, 1>& b, //
      const SharedDiagonal& model = SharedDiagonal()) :
      JacobianFactor(key1, A1, key2, A2, b, model) {
  }

  inline Key key1() const {
    return keys_[0];
  }
  inline Key key2() const {
    return keys_[1];
  }

  // Fixed-size matrix update
  void updateHessian(const KeyVector& infoKeys,
      SymmetricBlockMatrix* info) const {
    gttic(updateHessian_BinaryJacobianFactor);
    // Whiten the factor if it has a noise model
    const SharedDiagonal& model = get_model();
    if (model && !model->isUnit()) {
      if (model->isConstrained())
        throw std::invalid_argument(
            "BinaryJacobianFactor::updateHessian: cannot update information with "
                "constrained noise model");
      BinaryJacobianFactor whitenedFactor(key1(), model->Whiten(getA(begin())),
          key2(), model->Whiten(getA(end())), model->whiten(getb()));
      whitenedFactor.updateHessian(infoKeys, info);
    } else {
      // First build an array of slots
      DenseIndex slot1 = Slot(infoKeys, key1());
      DenseIndex slot2 = Slot(infoKeys, key2());
      DenseIndex slotB = info->nBlocks() - 1;

      const Matrix& Ab = Ab_.matrix();
      Eigen::Block<const Matrix, M, N1> A1(Ab, 0, 0);
      Eigen::Block<const Matrix, M, N2> A2(Ab, 0, N1);
      Eigen::Block<const Matrix, M, 1> b(Ab, 0, N1 + N2);

      // We perform I += A'*A to the upper triangle
      info->diagonalBlock(slot1).rankUpdate(A1.transpose());
      info->updateOffDiagonalBlock(slot1, slot2, A1.transpose() * A2);
      info->updateOffDiagonalBlock(slot1, slotB, A1.transpose() * b);
      info->diagonalBlock(slot2).rankUpdate(A2.transpose());
      info->updateOffDiagonalBlock(slot2, slotB, A2.transpose() * b);
      info->updateDiagonalBlock(slotB, b.transpose() * b);
    }
  }
};

template<int M, int N1, int N2>
struct traits<BinaryJacobianFactor<M, N1, N2> > : Testable<
    BinaryJacobianFactor<M, N1, N2> > {
};

} //namespace gtsam