ParameterMatrix.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/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