C++ MatrixBase::transpose方法代码示例

void normalizeVec(
    const Eigen::MatrixBase<Derived>& x,
    typename Derived::PlainObject& x_norm,
    typename Gradient<Derived, Derived::RowsAtCompileTime, 1>::type* dx_norm,
    typename Gradient<Derived, Derived::RowsAtCompileTime, 2>::type* ddx_norm) {

  typename Derived::Scalar xdotx = x.squaredNorm();
  typename Derived::Scalar norm_x = std::sqrt(xdotx);
  x_norm = x / norm_x;

  if (dx_norm) {
    dx_norm->setIdentity(x.rows(), x.rows());
    (*dx_norm) -= x * x.transpose() / xdotx;
    (*dx_norm) /= norm_x;

    if (ddx_norm) {
      auto dx_norm_transpose = transposeGrad(*dx_norm, x.rows());
      auto ddx_norm_times_norm = -matGradMultMat(x_norm, x_norm.transpose(), (*dx_norm), dx_norm_transpose);
      auto dnorm_inv = -x.transpose() / (xdotx * norm_x);
      (*ddx_norm) = ddx_norm_times_norm / norm_x;
      auto temp = (*dx_norm) * norm_x;
      int n = x.rows();
      for (int col = 0; col < n; col++) {
        auto column_as_matrix = (dnorm_inv(0, col) * temp);
        for (int row_block = 0; row_block < n; row_block++) {
          ddx_norm->block(row_block * n, col, n, 1) += column_as_matrix.col(row_block);
        }
      }
    }
  }
}

void MSF_MeasurementBase<EKFState_T>::calculateAndApplyCorrection(
    shared_ptr<EKFState_T> state, MSF_Core<EKFState_T>& core,
    const Eigen::MatrixBase<H_type>& H_delayed,
    const Eigen::MatrixBase<Res_type> & res_delayed,
    const Eigen::MatrixBase<R_type>& R_delayed) {

  EIGEN_STATIC_ASSERT_FIXED_SIZE (H_type);
  EIGEN_STATIC_ASSERT_FIXED_SIZE (R_type);

  // Get measurements.
  /// Correction from EKF update.
  Eigen::Matrix<double, MSF_Core<EKFState_T>::nErrorStatesAtCompileTime, 1> correction_;

  R_type S;
  Eigen::Matrix<double, MSF_Core<EKFState_T>::nErrorStatesAtCompileTime,
      R_type::RowsAtCompileTime> K;
  typename MSF_Core<EKFState_T>::ErrorStateCov & P = state->P;

  S = H_delayed * P * H_delayed.transpose() + R_delayed;
  K = P * H_delayed.transpose() * S.inverse();

  correction_ = K * res_delayed;
  const typename MSF_Core<EKFState_T>::ErrorStateCov KH =
      (MSF_Core<EKFState_T>::ErrorStateCov::Identity() - K * H_delayed);
  P = KH * P * KH.transpose() + K * R_delayed * K.transpose();

  // Make sure P stays symmetric.
  P = 0.5 * (P + P.transpose());

  core.applyCorrection(state, correction_);
}

SOPHUS_FUNC bool isOrthogonal(Eigen::MatrixBase<D> const& R) {
  using Scalar = typename D::Scalar;
  static int const N = D::RowsAtCompileTime;
  static int const M = D::ColsAtCompileTime;

  static_assert(N == M, "must be a square matrix");
  static_assert(N >= 2, "must have compile time dimension >= 2");

  return (R * R.transpose() - Matrix<Scalar, N, N>::Identity()).norm() <
         Constants<Scalar>::epsilon();
}

 inline void unSkew(const Eigen::MatrixBase<Matrix3> & M,
                    const Eigen::MatrixBase<Vector3> & v)
 {
   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
   EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
   assert((M + M.transpose()).isMuchSmallerThan(M));
   
   Vector3 & v_ = const_cast<Eigen::MatrixBase<Vector3> &>(v).derived();
   typedef typename Vector3::RealScalar Scalar;
   
   v_[0] = Scalar(0.5) * (M(2,1) - M(1,2));
   v_[1] = Scalar(0.5) * (M(0,2) - M(2,0));
   v_[2] = Scalar(0.5) * (M(1,0) - M(0,1));
 }

      void least_squares_eqcon(Eigen::MatrixBase<data1__>  &x, const Eigen::MatrixBase<data2__> &A, const Eigen::MatrixBase<data3__> &b, const Eigen::MatrixBase<data4__> &B, const Eigen::MatrixBase<data5__> &d)
      {
        typedef Eigen::Matrix<typename Eigen::MatrixBase<data4__>::Scalar, Eigen::Dynamic, Eigen::Dynamic> B_matrixD;
        B_matrixD Q, R, temp_B, temp_R;
        typedef Eigen::Matrix<typename Eigen::MatrixBase<data2__>::Scalar, Eigen::Dynamic, Eigen::Dynamic> A_matrixD;
        A_matrixD A1, A2, temp_A;
        typedef Eigen::Matrix<typename Eigen::MatrixBase<data5__>::Scalar, Eigen::Dynamic, Eigen::Dynamic> d_matrixD;
        d_matrixD y;
        typedef Eigen::Matrix<typename Eigen::MatrixBase<data3__>::Scalar, Eigen::Dynamic, Eigen::Dynamic> b_matrixD;
        b_matrixD z, rhs;
        typedef Eigen::Matrix<typename Eigen::MatrixBase<data1__>::Scalar, Eigen::Dynamic, Eigen::Dynamic> x_matrixD;
        x_matrixD x_temp(A.cols(), b.cols());
        typename A_matrixD::Index p(B.rows()), n(A.cols());
  #ifdef DEBUG
        typename A_matrixD::Index m(b.rows());
  #endif

        // build Q and R
        Eigen::HouseholderQR<B_matrixD> qr(B.transpose());
        Q=qr.householderQ();
        temp_B=qr.matrixQR();
        temp_R=Eigen::TriangularView<B_matrixD, Eigen::Upper>(temp_B);
        R=temp_R.topRows(p);
        assert((R.rows()==p) && (R.cols()==p));
        assert((Q.rows()==n) && (Q.cols()==n));

        // build A1 and A2
        temp_A=A*Q;
        A1=temp_A.leftCols(p);
        A2=temp_A.rightCols(n-p);
#ifdef DEBUG
        assert((A1.rows()==m) && (A1.cols()==p));
        assert((A2.rows()==m) && (A2.cols()==n-p));
#endif
        assert(A1.cols()==p);
        assert(A2.cols()==n-p);

        // solve for y
        y=R.transpose().lu().solve(d);

        // setup the unconstrained optimization
        rhs=b-A1*y;
        least_squares_uncon(z, A2, rhs);

        // build the solution
        x_temp.topRows(p)=y;
        x_temp.bottomRows(n-p)=z;
        x=Q*x_temp;
      }

SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase<D> const& sR) {
  using Scalar = typename D::Scalar;
  static int const N = D::RowsAtCompileTime;
  static int const M = D::ColsAtCompileTime;
  using Eigen::numext::pow;
  using Eigen::numext::sqrt;

  Scalar det = sR.determinant();

  if (det <= Scalar(0)) {
    return false;
  }

  Scalar scale_sqr = pow(det, Scalar(2. / N));

  static_assert(N == M, "must be a square matrix");
  static_assert(N >= 2, "must have compile time dimension >= 2");

  return (sR * sR.transpose() - scale_sqr * Matrix<Scalar, N, N>::Identity())
             .template lpNorm<Eigen::Infinity>() <
         sqrt(Constants<Scalar>::epsilon());
}

 inline void approximateNearestOrthogonalMatrix(const Eigen::MatrixBase<Derived>& R)
 {
   Eigen::Matrix3d E = R.transpose() * R;
   E.diagonal().array() -= 1;
   const_cast<Eigen::MatrixBase<Derived>&>(R) -= 0.5 * R * E;
 }

inline
typename Eigen::MatrixBase<Derived>::ConstTransposeReturnType
static trans(const Eigen::MatrixBase<Derived>& mat) {
    return mat.transpose();
}