RegularImplicitSchurFactor.h

Go to the documentation of this file.

 
#pragma once
 
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/linear/FlatGaussianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
 
#include <iosfwd>
#include <map>
#include <string>
#include <vector>
 
namespace gtsam {

template<class CAMERA>
class RegularImplicitSchurFactor: public GaussianFactor,
                                  public FlatGaussianFactor {
 
public:
  typedef RegularImplicitSchurFactor This; 
  typedef std::shared_ptr<This> shared_ptr; 
 
protected:
 
  // This factor is closely related to a CameraSet
  typedef CameraSet<CAMERA> Set;
 
  typedef typename CAMERA::Measurement Z;
  static const int D = traits<CAMERA>::dimension; 
  static const int ZDim = traits<Z>::dimension; 
 
  typedef Eigen::Matrix<double, ZDim, D> MatrixZD; 
  typedef Eigen::Matrix<double, D, D> MatrixDD; 
  typedef Eigen::Matrix<double, D, 1> DVector;
  typedef Eigen::Matrix<double, ZDim, 1> ZVector;
  typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks;
 
  FBlocks FBlocks_; 
  const Matrix PointCovariance_; 
  const Matrix E_; 
  const Vector b_; 

  MatrixDD cameraHessianBlock(size_t position) const {
    const MatrixZD& cameraJacobian = FBlocks_[position];
    const Matrix23 pointJacobian =
        E_.block<ZDim, 3>(ZDim * position, 0);
    return cameraJacobian.transpose() *
           (cameraJacobian -
            pointJacobian * PointCovariance_ * pointJacobian.transpose() *
                cameraJacobian);
  }

  DVector cameraHessianDiagonal(size_t position) const {
    const MatrixZD& cameraJacobian = FBlocks_[position];
    const Eigen::Matrix<double, D, 3> cameraPointInformation =
        cameraJacobian.transpose() *
        E_.block<ZDim, 3>(ZDim * position, 0);
    DVector diagonal;
    for (int column = 0; column < D; ++column) {
      diagonal(column) = cameraJacobian.col(column).squaredNorm();
      diagonal(column) -=
          cameraPointInformation.row(column) * PointCovariance_ *
          cameraPointInformation.row(column).transpose();
    }
    return diagonal;
  }

  template <class OFFSET>
  void multiplyHessianAddFlat(double alpha, const OFFSET& offset,
                              const double* x, double* y) const {
    typedef Eigen::Map<DVector> DMap;
    typedef Eigen::Map<const DVector> ConstDMap;
 
    Vector3 pointProjection = Vector3::Zero();
    for (size_t position = 0; position < size(); ++position) {
      const ZVector cameraError =
          FBlocks_[position] * ConstDMap(x + offset(position));
      pointProjection.noalias() +=
          E_.block<ZDim, 3>(ZDim * position, 0).transpose() * cameraError;
    }
    const Vector3 pointCorrection = PointCovariance_ * pointProjection;
 
    for (size_t position = 0; position < size(); ++position) {
      const ZVector cameraError =
          FBlocks_[position] * ConstDMap(x + offset(position));
      const ZVector projectedError =
          cameraError -
          E_.block<ZDim, 3>(ZDim * position, 0) * pointCorrection;
      DMap output(y + offset(position));
      output.noalias() +=
          alpha * FBlocks_[position].transpose() * projectedError;
    }
  }

  template <class OFFSET>
  void gradientAtZeroAddFlat(const OFFSET& offset, double* gradient) const {
    typedef Eigen::Map<DVector> DMap;
 
    Vector3 pointProjection = Vector3::Zero();
    for (size_t position = 0; position < size(); ++position) {
      pointProjection.noalias() +=
          E_.block<ZDim, 3>(ZDim * position, 0).transpose() *
          b_.segment<ZDim>(ZDim * position);
    }
    const Vector3 pointCorrection = PointCovariance_ * pointProjection;
 
    for (size_t position = 0; position < size(); ++position) {
      const ZVector projectedRhs =
          b_.segment<ZDim>(ZDim * position) -
          E_.block<ZDim, 3>(ZDim * position, 0) * pointCorrection;
      DMap output(gradient + offset(position));
      output.noalias() -= FBlocks_[position].transpose() * projectedRhs;
    }
  }
 
public:

  RegularImplicitSchurFactor() {
  }


  RegularImplicitSchurFactor(const KeyVector& keys, const FBlocks& Fs,
                             const Matrix& E, const Matrix& P, const Vector& b)
      : GaussianFactor(keys), FBlocks_(Fs), PointCovariance_(P), E_(E), b_(b) {}

  ~RegularImplicitSchurFactor() override {
  }
 
  const FBlocks& Fs() const {
    return FBlocks_;
  }
 
  const Matrix& E() const {
    return E_;
  }
 
  const Vector& b() const {
    return b_;
  }
 
  const Matrix& getPointCovariance() const {
    return PointCovariance_;
  }

  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
      DefaultKeyFormatter) const override {
    std::cout << " RegularImplicitSchurFactor " << std::endl;
    Factor::print(s);
    for (size_t pos = 0; pos < size(); ++pos) {
      std::cout << "Fblock:\n" << FBlocks_[pos] << std::endl;
    }
    std::cout << "PointCovariance:\n" << PointCovariance_ << std::endl;
    std::cout << "E:\n" << E_ << std::endl;
    std::cout << "b:\n" << b_.transpose() << std::endl;
  }

  bool equals(const GaussianFactor& lf, double tol) const override {
    const This* f = dynamic_cast<const This*>(&lf);
    if (!f)
      return false;
    for (size_t k = 0; k < FBlocks_.size(); ++k) {
      if (keys_[k] != f->keys_[k])
        return false;
      if (!equal_with_abs_tol(FBlocks_[k], f->FBlocks_[k], tol))
        return false;
    }
    return equal_with_abs_tol(PointCovariance_, f->PointCovariance_, tol)
        && equal_with_abs_tol(E_, f->E_, tol)
        && equal_with_abs_tol(b_, f->b_, tol);
  }

  DenseIndex getDim(const_iterator variable) const override {
    return D;
  }
 
  void updateHessian(const KeyVector& keys,
                         SymmetricBlockMatrix* info) const override {
    throw std::runtime_error(
        "RegularImplicitSchurFactor::updateHessian not implemented");
  }
 
  void updateHessian(const KeyVector& keys,
                         SymmetricBlockMatrix* info,
                         DenseIndex beginCol, DenseIndex endCol) const override {
    throw std::runtime_error(
        "RegularImplicitSchurFactor::updateHessian not implemented");
  }
 
  Matrix augmentedJacobian() const override {
    throw std::runtime_error(
        "RegularImplicitSchurFactor::augmentedJacobian not implemented");
    return Matrix();
  }
  std::pair<Matrix, Vector> jacobian() const override {
    throw std::runtime_error(
        "RegularImplicitSchurFactor::jacobian not implemented");
    return {Matrix(), Vector()};
  }

  Matrix augmentedInformation() const override {
    // Do the Schur complement
    SymmetricBlockMatrix augmentedHessian =
        Set::SchurComplement(FBlocks_, E_, b_);
    return augmentedHessian.selfadjointView();
  }

  Matrix information() const override {
    Matrix augmented = augmentedInformation();
    int m = this->keys_.size();
    size_t M = D * m;
    return augmented.block(0, 0, M, M);
  }

  using GaussianFactor::hessianDiagonal;

  void hessianDiagonalAdd(VectorValues &d) const override {
    for (size_t position = 0; position < size(); ++position) {
      const DVector diagonal = cameraHessianDiagonal(position);
      auto result = d.emplace(keys_[position], diagonal);
      if (!result.second) {
        result.first->second += diagonal;
      }
    }
  }

  void hessianDiagonal(double* d) const override {
    typedef Eigen::Map<DVector> DMap;
    for (size_t position = 0; position < size(); ++position) {
      DMap(d + D * keys_[position]) += cameraHessianDiagonal(position);
    }
  }

  std::map<Key, Matrix> hessianBlockDiagonal() const override {
    std::map<Key, Matrix> blocks;
    for (size_t position = 0; position < size(); ++position) {
      blocks[keys_[position]] = cameraHessianBlock(position);
    }
    return blocks;
  }
 
  void hessianBlockDiagonalAdd(
      const std::vector<size_t>& blockSlots,
      std::vector<Matrix>* diagonalBlocks) const override {
    if (blockSlots.size() != size()) {
      throw std::invalid_argument(
          "RegularImplicitSchurFactor::hessianBlockDiagonalAdd: block slot "
          "count mismatch");
    }
    for (size_t position = 0; position < size(); ++position) {
      diagonalBlocks->at(blockSlots[position]).noalias() +=
          cameraHessianBlock(position);
    }
  }
 
  GaussianFactor::shared_ptr clone() const override {
    return std::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
        FBlocks_, PointCovariance_, E_, b_);
    throw std::runtime_error(
        "RegularImplicitSchurFactor::clone not implemented");
  }
 
  GaussianFactor::shared_ptr negate() const override {
    return std::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
        FBlocks_, PointCovariance_, E_, b_);
    throw std::runtime_error(
        "RegularImplicitSchurFactor::negate not implemented");
  }
 
  // Raw Vector version of y += F'*alpha*(I - E*P*E')*F*x, for testing
  static
  void multiplyHessianAdd(const Matrix& F, const Matrix& E,
      const Matrix& PointCovariance, double alpha, const Vector& x, Vector& y) {
    Vector e1 = F * x;
    Vector d1 = E.transpose() * e1;
    Vector d2 = PointCovariance * d1;
    Vector e2 = E * d2;
    Vector e3 = alpha * (e1 - e2);
    y += F.transpose() * e3;
  }
 
  typedef std::vector<Vector2, Eigen::aligned_allocator<Vector2>> Error2s;

  void projectError2(const Error2s& e1, Error2s& e2) const {
 
    // d1 = E.transpose() * (e1-ZDim*b) = (3*2m)*2m
    Vector3 d1;
    d1.setZero();
    for (size_t k = 0; k < size(); k++)
      d1 += E_.block<ZDim, 3>(ZDim * k, 0).transpose()
          * (e1[k] - ZDim * b_.segment<ZDim>(k * ZDim));
 
    // d2 = E.transpose() * e1 = (3*2m)*2m
    Vector3 d2 = PointCovariance_ * d1;
 
    // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
    for (size_t k = 0; k < size(); k++)
      e2[k] = e1[k] - ZDim * b_.segment<ZDim>(k * ZDim)
          - E_.block<ZDim, 3>(ZDim * k, 0) * d2;
  }
 
  /*
   * This definition matches the linearized error in the Hessian Factor:
   * LinError(x) = x'*H*x - 2*x'*eta + f
   * with:
   * H   = F' * (I-E'*P*E) * F = F' * Q * F
   * eta = F' * (I-E'*P*E) * b = F' * Q * b
   * f = nonlinear error
   * (x'*H*x - 2*x'*eta + f) = x'*F'*Q*F*x - 2*x'*F'*Q *b + f = x'*F'*Q*(F*x - 2*b) + f
   */
  double error(const VectorValues& x) const override {
 
    // resize does not do malloc if correct size
    e1.resize(size());
    e2.resize(size());
 
    // e1 = F * x - b = (2m*dm)*dm
    for (size_t k = 0; k < size(); ++k)
      e1[k] = FBlocks_[k] * x.at(keys_[k]);
    projectError2(e1, e2);
 
    double result = 0;
    for (size_t k = 0; k < size(); ++k)
      result += dot(e1[k], e2[k]);
 
    double f = b_.squaredNorm();
    return 0.5 * (result + f);
  }
 
  // needed to be GaussianFactor - (I - E*P*E')*(F*x - b)
  // This is wrong and does not match the definition in Hessian,
  // but it matches the definition of the Jacobian factor (JF)
  double errorJF(const VectorValues& x) const {
 
    // resize does not do malloc if correct size
    e1.resize(size());
    e2.resize(size());
 
    // e1 = F * x - b = (2m*dm)*dm
    for (size_t k = 0; k < size(); ++k)
      e1[k] = FBlocks_[k] * x.at(keys_[k]) - b_.segment<ZDim>(k * ZDim);
    projectError(e1, e2);
 
    double result = 0;
    for (size_t k = 0; k < size(); ++k)
      result += dot(e2[k], e2[k]);
 
    // std::cout << "implicitFactor::error result " << result << std::endl;
    return 0.5 * result;
  }
  void projectError(const Error2s& e1, Error2s& e2) const {
 
    // d1 = E.transpose() * e1 = (3*2m)*2m
    Vector3 d1;
    d1.setZero();
    for (size_t k = 0; k < size(); k++)
      d1 += E_.block<ZDim, 3>(ZDim * k, 0).transpose() * e1[k];
 
    // d2 = E.transpose() * e1 = (3*2m)*2m
    Vector3 d2 = PointCovariance_ * d1;
 
    // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
    for (size_t k = 0; k < size(); k++)
      e2[k] = e1[k] - E_.block<ZDim, 3>(ZDim * k, 0) * d2;
  }

  mutable Error2s e1, e2;

  void multiplyHessianAdd(double alpha, const double* x, double* y) const {
    multiplyHessianAddFlat(
        alpha, [&](size_t position) { return D * keys_[position]; }, x, y);
  }
 
  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(
          "RegularImplicitSchurFactor::multiplyHessianAdd: offset count "
          "mismatch");
    }
    multiplyHessianAddFlat(
        alpha, [&](size_t position) { return scalarOffsets[position]; }, x, y);
  }

  void multiplyHessianAdd(double alpha, const VectorValues& x,
      VectorValues& y) const override {
 
    // resize does not do malloc if correct size
    e1.resize(size());
    e2.resize(size());
 
    // e1 = F * x = (2m*dm)*dm
    for (size_t k = 0; k < size(); ++k)
      e1[k] = FBlocks_[k] * x.at(keys_[k]);
 
    projectError(e1, e2);
 
    // y += F.transpose()*e2 = (2d*2m)*2m
    for (size_t k = 0; k < size(); ++k) {
      Key key = keys_[k];
      static const Vector empty;
      std::pair<VectorValues::iterator, bool> it = y.tryInsert(key, empty);
      Vector& yi = it.first->second;
      // Create the value as a zero vector if it does not exist.
      if (it.second)
        yi = Vector::Zero(FBlocks_[k].cols());
      yi += FBlocks_[k].transpose() * alpha * e2[k];
    }
  }

  void multiplyHessianDummy(double alpha, const VectorValues& x,
      VectorValues& y) const {
 
    for (size_t k = 0; k < size(); ++k) {
      static const Vector empty;
      Key key = keys_[k];
      std::pair<VectorValues::iterator, bool> it = y.tryInsert(key, empty);
      Vector& yi = it.first->second;
      yi = x.at(key);
    }
  }

  VectorValues gradientAtZero() const override {
    // calculate Q*b
    e1.resize(size());
    e2.resize(size());
    for (size_t k = 0; k < size(); k++)
      e1[k] = b_.segment<ZDim>(ZDim * k);
    projectError(e1, e2);
 
    // g = F.transpose()*e2
    VectorValues g;
    for (size_t k = 0; k < size(); ++k) {
      Key key = keys_[k];
      g.insert(key, -FBlocks_[k].transpose() * e2[k]);
    }
 
    // return it
    return g;
  }
 
  void gradientAtZeroAdd(const std::vector<size_t>& scalarOffsets,
                         double* gradient) const override {
    if (scalarOffsets.size() != size()) {
      throw std::invalid_argument(
          "RegularImplicitSchurFactor::gradientAtZeroAdd: offset count "
          "mismatch");
    }
    gradientAtZeroAddFlat(
        [&](size_t position) { return scalarOffsets[position]; }, gradient);
  }

  void gradientAtZero(double* d) const override {
    gradientAtZeroAddFlat(
        [&](size_t position) { return D * keys_[position]; }, d);
  }

  Vector gradient(Key key, const VectorValues& x) const override {
    throw std::runtime_error(
        "gradient for RegularImplicitSchurFactor is not implemented yet");
  }
 
};
// end class RegularImplicitSchurFactor
 
template<class CAMERA>
const int RegularImplicitSchurFactor<CAMERA>::D;
 
template<class CAMERA>
const int RegularImplicitSchurFactor<CAMERA>::ZDim;
 
// traits
template<class CAMERA> struct traits<RegularImplicitSchurFactor<CAMERA> > : public Testable<
    RegularImplicitSchurFactor<CAMERA> > {
};
 
}