ParameterMatrix.h

/* ----------------------------------------------------------------------------
 
 * 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/Matrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/VectorSpace.h>
 
#include <iostream>
 
namespace gtsam {
 
template <int M>
class ParameterMatrix {
  using MatrixType = Eigen::Matrix<double, M, -1>;
 
 private:
  MatrixType matrix_;
 
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  enum { dimension = Eigen::Dynamic };
 
  ParameterMatrix(const size_t N) : matrix_(M, N) { matrix_.setZero(); }
 
  ParameterMatrix(const MatrixType& matrix) : matrix_(matrix) {}
 
  size_t rows() const { return matrix_.rows(); }
 
  size_t cols() const { return matrix_.cols(); }
 
  MatrixType matrix() const { return matrix_; }
 
  Eigen::Matrix<double, -1, M> transpose() const { return matrix_.transpose(); }
 
  Eigen::Matrix<double, 1, -1> row(size_t index) const {
    return matrix_.row(index);
  }
 
  auto row(size_t index) -> Eigen::Block<MatrixType, 1, -1, false> {
    return matrix_.row(index);
  }
 
  Eigen::Matrix<double, M, 1> col(size_t index) const {
    return matrix_.col(index);
  }
 
  auto col(size_t index) -> Eigen::Block<MatrixType, M, 1, true> {
    return matrix_.col(index);
  }
 
  void setZero() { matrix_.setZero(); }
 
  ParameterMatrix<M> operator+(const ParameterMatrix<M>& other) const {
    return ParameterMatrix<M>(matrix_ + other.matrix());
  }
 
  ParameterMatrix<M> operator+(
      const Eigen::Matrix<double, -1, 1>& other) const {
    // This form avoids a deep copy and instead typecasts `other`.
    Eigen::Map<const MatrixType> other_(other.data(), M, cols());
    return ParameterMatrix<M>(matrix_ + other_);
  }
 
  ParameterMatrix<M> operator-(const ParameterMatrix<M>& other) const {
    return ParameterMatrix<M>(matrix_ - other.matrix());
  }
 
  ParameterMatrix<M> operator-(
      const Eigen::Matrix<double, -1, 1>& other) const {
    Eigen::Map<const MatrixType> other_(other.data(), M, cols());
    return ParameterMatrix<M>(matrix_ - other_);
  }
 
  MatrixType operator*(const Eigen::Matrix<double, -1, -1>& other) const {
    return matrix_ * other;
  }
 
 
  void print(const std::string& s = "") const {
    std::cout << (s == "" ? s : s + " ") << matrix_ << std::endl;
  }
 
  bool equals(const ParameterMatrix<M>& other, double tol = 1e-8) const {
    return gtsam::equal_with_abs_tol(matrix_, other.matrix(), tol);
  }
 
  inline size_t dim() const { return matrix_.size(); }
 
  inline Vector vector() const {
    using RowMajor = Eigen::Matrix<double, -1, -1, Eigen::RowMajor>;
    Vector result(matrix_.size());
    Eigen::Map<RowMajor>(&result(0), rows(), cols()) = matrix_;
    return result;
  }
 
  inline static ParameterMatrix identity() {
    // throw std::runtime_error(
    //     "ParameterMatrix::identity(): Don't use this function");
    return ParameterMatrix(0);
  }
 
};
 
// traits for ParameterMatrix
template <int M>
struct traits<ParameterMatrix<M>>
    : public internal::VectorSpace<ParameterMatrix<M>> {};
 
/* ************************************************************************* */
// Stream operator that takes a ParameterMatrix. Used for printing.
template <int M>
inline std::ostream& operator<<(std::ostream& os,
                                const ParameterMatrix<M>& parameterMatrix) {
  os << parameterMatrix.matrix();
  return os;
}
 
}  // namespace gtsam