Eigen、 Sophusまとめ

3年目くらいに書いたものだからアップデートが必要かも

Eigen

• Rotation matrix: Eigen::Matrix3d
• Rotation vector: Eigen::AngleAxisd
• Euler angle: Eigen::Vector3d
• Quaternion: Eigen::Quaterniond
• Euclidean transform: Eigen::Isometry3d
• Affine transform: Eigen::Affine3d
• Perspective transformation: Eigen::Projective3d

参考 https://github.com/gaoxiang12/slambook-en

• メンバ変数に用いる場合はpublicでマクロを定義する

class Foo
{
  ...
  Eigen::Vector2d v;
  ...
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW // このマクロを追加．
  ...
};

• STLコンテナを用いる場合にはallocatorを利用
  • typedefしとくと楽

#include<Eigen/StdVector> 
typedef std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;

参考 https://qiita.com/vs4sh/items/2032fd1a5dab780d432c

Gauss-Newton法

例えばDirect法でGauss-Newtonを用いるとして

$$x_{t+1} = x_t + \Delta$$

$$\Delta = - {({J}^T J)}^{-1} {J}^T r $$

で$J$、$r$が求められていたら、

Eigen::VectorXf xi = Eigen::VectorXf::Zero(6);　// 姿勢のベクトル

Eigen::MatrixXf J = Eigen::MatrixXf::Zero(width*height, 6);
Eigen::VectorXf residuals = Eigen::VectorXf::Zero(width*height);

// ここでJとresidualsを求める

Eigen::MatrixXf Jt = J.transpose();
Eigen::VectorXf b = Jt * residuals;

Eigen::Matrix<float, 6, 6> H = Jt * J;
Eigen::Matrix<float, 6, 1> inc = -(H.ldlt().solve(b));  // 逆行列は使わない

xi = (Sophus::SE3f::exp(xi) * Sophus::SE3f::exp(inc)).log();

で$\Delta$が求まり$x_{t+1}$が計算できる

行列計算で逆行列は使うべきではない

行列の分解

• LU分解
  • 一般の行列$A$を左下三角行列$L$と右上三角行列$U$の積$A=LU$に分解
  • PartialPivLU
  • FullPivLU 精度がPartialPivLUよりいい

  Eigen::PartialPivLU<Eigen::MatrixXd> lu(A);
  x = lu.solve(b);

  // もしくは
  Eigen::PartialPivLU<Eigen::MatrixXd> lu;
  lu.compute(A);
  x = lu.solve(b);

• コレスキー分解

  • $A$が対称正定値行列の場合のLU分解$A=L{L}^T$
  • LU分解より2倍計算量が小さい
  • $y={L}^T\delta x$として$A\delta x = L{L}^T \delta x=Ly=a$から$A\delta L=a$を解く
  • LLT
  • LDLT 精度がLLTよりいい

• QR分解

  • 最小二乗法により長方形の係数行列をもつ方程式$Ax=b$を解く場合はQR分解を用いる
  • HouseholderQR
  • ColPivHouseholderQR
  • FullPivHouseholderQR

• 固有値分解

  • EigenSolver 実数行列
  • ComplexEigenSolver 複素数行列
  • SelfAdjointEigenSolver 実対称、自己随伴行列
  • GeneralizedEigenSolver
  • GeneralizedSelfAdjointEigenSolver

• SVD

  • JacobiSVD 精度がいい サイズが増えると遅い
  • BDCSVD 速い
    • singularValues() 特異値
    • matrixU() 左特異ベクトルを並べた行列
    • matrixV() 右特異ベクトルを並べた行列
    • solve() 方程式を解く
    • compute() 分解

参考 https://qiita.com/MusicScience37/items/13fefa6bed25ab8fb6e9

Sophus

Lie群、Lie代数のライブラリ Eigenとの相性がいい

Use logarithmic map to get the Lie algebra

// Rotation matrix with 90 degrees along Z axis  
Matrix3d R = AngleAxisd(M_PI/2, Vector3d(0, 0, 1)).toRotationMatrix();  
Sophus::SO3d SO3_R(R);  
Vector3d so3 = SO3_R.log();  
cout << "so3 hat=\n" << Sophus::SO3d::hat(so3) << endl;  
cout << "so3 hat vee= " << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)). transpose() << endl;  
  
// SE3  
Vector3d t(1, 0, 0);  
Sophus::SE3d SE3_Rt(R, t);  
// Lie Algebra is 6d vector  
typedef Eigen::Matrix<double, 6, 1> Vector6d;  
Vector6d se3 = SE3_Rt.log();  

se3からSE3の変換は$\exp(\hat{\xi})$だけどSophusはexp()に$\xi$をそのまま渡せば計算してくれる
log()も同様

クオータニオンと並進ベクトルからSE3への変換

Eigen::Quaternionf q(w, x, y, z);  
Eigen::Vector3f t(1.f, 0.f, 0.f);  
Sophus::SE3f pose(q, t);