C++ (Cpp) Matrix3d::diagonalの例

コード例 #1

ファイル: math_functions.hpp プロジェクト: caomw/g2o_tutorial

 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;
 }