Eigen

はじめに

クォータニオンクラスの関数は

に書いてあるが，四則演算の方法が謎だったので調べたら，

に書いてあるように，coeffsとかを使えばいいことが分かった．

クォータニオンが謎だったので検証した．

クォータニオンの代入

#include <iostream>
#include <math.h>
#include <Eigen/Dense>
int main(){
  Eigen::Quaternion<double> q = Eigen::Quaternion<double>(cos(M_PI/6/2), 0, sin(M_PI/6/2), 0);
  std::cout << "w : " << q.w() << ", x : " << q.x() << ", y : " << q.y() << ", z : " << q.z() << std::endl;
  std::cout << q.toRotationMatrix() << std::endl;
}

を hoge.cpp として

g++ -g -Wall -O3 -I/usr/include/eigen3 hoge.cpp -o fuga
./fuga

をすると

w : 0.965926, x : 0, y : 0.258819, z : 0
0.866025        0      0.5
       0        1        0
    -0.5        0 0.866025

となる．

#include <iostream>
#include <math.h>
#include <Eigen/Dense>
int main(){
  Eigen::AngleAxis<double> aaZ(0, Eigen::Vector3d::UnitZ());
  Eigen::AngleAxis<double> aaY(M_PI/6, Eigen::Vector3d::UnitY());
  Eigen::AngleAxis<double> aaX(0, Eigen::Vector3d::UnitX());
  Eigen::Quaternion<double> q = aaZ * aaY * aaX;
  std::cout << "w : " << q.w() << ", x : " << q.x() << ", y : " << q.y() << ", z : " << q.z() << std::endl;
  std::cout << q.toRotationMatrix() << std::endl;
}

だと

w : 0.965926, x : 0, y : 0.258819, z : 0
0.866025        0      0.5
       0        1        0
    -0.5        0 0.866025

となって，一致する．

本当は

#include <iostream>
#include <math.h>
#include <Eigen/Dense>
int main(){
  Eigen::Quaternion<double> q = Eigen::Quaternion<double>(M_PI/6, 0, 1, 0);
  q.normalize();
  std::cout << "w : " << q.w() << ", x : " << q.x() << ", y : " << q.y() << ", z : " << q.z() << std::endl;
  std::cout << q.toRotationMatrix() << std::endl;
}

としたときに，上のやつ（AngleAxisを使ったやつ）と一致して欲しかったが，

w : 0.46386, x : 0, y : 0.885908, z : 0
-0.569667         0  0.821876
        0         1         0
-0.821876         0 -0.569667

となって，全然違う．

これらから分かるのは， $$ \theta : 回転角度\\ 回転軸 : x,y,z $$ のときに，

Eigen::Quaternion<double> q = Eigen::Quaternion<double>(θ, x, y, z);

ではなく，

Eigen::Quaternion<double> q = Eigen::Quaternion<double>(cos(θ/2), sin(θ/2)*x, sin(θ/2)*y, sin(θ/2)*z);

が正解，ということ．

クォータニオンの演算

#include <iostream>
#include <math.h>
#include <Eigen/Dense>
int main(){
  Eigen::Quaternion<double> q1 = Eigen::Quaternion<double>(cos(M_PI/6/2), 0, sin(M_PI/6/2), 0);
  Eigen::Quaternion<double> q2 = Eigen::Quaternion<double>(cos(M_PI/3/2), (-1/sqrt(3))* sin(M_PI/6/2), (1/sqrt(3)) * sin(M_PI/6/2), (1/sqrt(3)) * sin(M_PI/6/2));
  Eigen::Quaternion<double> sum;
  sum.coeffs() = q1.coeffs() + q2.coeffs();
  std::cout << "q1    " << "w : " << q1.w() << ", x : " << q1.x() << ", y : " << q1.y() << ", z : " << q1.z() << std::endl;
  std::cout << "q2    " << "w : " << q2.w() << ", x : " << q2.x() << ", y : " << q2.y() << ", z : " << q2.z() << std::endl;
  std::cout << "sum   " << "w : " << sum.w() << ", x : " << sum.x() << ", y : " << sum.y() << ", z : " << sum.z() << std::endl;
}

で

q1    w : 0.965926, x : 0, y : 0.258819, z : 0
q2    w : 0.866025, x : -0.149429, y : 0.149429, z : 0.149429
sum   w : 1.83195, x : -0.149429, y : 0.408248, z : 0.149429

となるので，足し算は確かにいい感じ．

#include <iostream>
#include <math.h>
#include <Eigen/Dense>
int main(){
  Eigen::Quaternion<double> q1 = Eigen::Quaternion<double>(cos(M_PI/6/2), 0, sin(M_PI/6/2), 0);
  Eigen::Quaternion<double> q2 = Eigen::Quaternion<double>(cos(M_PI/3/2), (-1/sqrt(3))* sin(M_PI/3/2), (1/sqrt(3)) * sin(M_PI/3/2), (1/sqrt(3)) * sin(M_PI/3/2));
  Eigen::Quaternion<double> product;
  product = q1 * q2;
  std::cout << "q1      " << "theta : " << M_PI/6 << ", axis : (" << 1 << ", " << 0 << ", " << 0 << ")" << ", cos(theta/2) : " << cos(M_PI/6/2) << std::endl;
  std::cout << "q1      " << "w : " << q1.w() << ", x : " << q1.x() << ", y : " << q1.y() << ", z : " << q1.z() << std::endl;
  std::cout << "q2      " << "theta : " << M_PI/3 << ", axis : (" << -1/sqrt(3) << ", " << 1/sqrt(3) << ", " << 1/sqrt(3) << ")" << ", cos(theta/2) : " << cos(M_PI/3/2) << std::endl;
  std::cout << "q2      " << "w : " << q2.w() << ", x : " << q2.x() << ", y : " << q2.y() << ", z : " << q2.z() << std::endl;
  std::cout << "product " << "w : " << product.w() << ", x : " << product.x() << ", y : " << product.y() << ", z : " << product.z() << std::endl;
}

は

q1      theta : 0.523599, axis : (1, 0, 0), cos(theta/2) : 0.965926
q1      w : 0.965926, x : 0, y : 0.258819, z : 0
q2      theta : 1.0472, axis : (-0.57735, 0.57735, 0.57735), cos(theta/2) : 0.866025
q2      w : 0.866025, x : -0.288675, y : 0.288675, z : 0.288675
product w : 0.761802, x : -0.204124, y : 0.502983, z : 0.353553

となる．

この下で，クォータニオン便利ノートの2ページ目の右上にあるクォータニオンの掛け算を行列で表すのが上手く行くか確かめた．

#include <iostream>
#include <math.h>
#include <Eigen/Dense>
int main(){
  Eigen::Quaternion<double> q1 = Eigen::Quaternion<double>(cos(M_PI/6/2), 0, sin(M_PI/6/2), 0);
  Eigen::Quaternion<double> q2 = Eigen::Quaternion<double>(cos(M_PI/3/2), (-1/sqrt(3))* sin(M_PI/3/2), (1/sqrt(3)) * sin(M_PI/3/2), (1/sqrt(3)) * sin(M_PI/3/2));
  Eigen::Matrix<double, 4, 4> q1_matrix;
  q1_matrix <<
    q1.w(), -q1.x(), -q1.y(), -q1.z(),
    q1.x(),  q1.w(), -q1.z(),  q1.y(),
    q1.y(),  q1.z(),  q1.w(), -q1.x(),
    q1.z(), -q1.y(),  q1.x(),  q1.w();
  Eigen::Matrix<double, 4, 4> q1_matrix2;
  q1_matrix2 <<
    cos(M_PI/6/2), 0, -sin(M_PI/6/2), 0,
    0, cos(M_PI/6/2), 0, sin(M_PI/6/2),
    sin(M_PI/6/2), 0, cos(M_PI/6/2), 0,
    0, -sin(M_PI/6/2), 0, cos(M_PI/6/2);
  Eigen::Vector4d q2_vec;
  q2_vec << q2.w(), q2.x(), q2.y(), q2.z();
  Eigen::Vector4d q2_vec2;
  q2_vec2 << cos(M_PI/3/2), (-1/sqrt(3))* sin(M_PI/3/2), (1/sqrt(3)) * sin(M_PI/3/2), (1/sqrt(3)) * sin(M_PI/3/2);
  Eigen::Quaternion<double> product = q1 * q2;
  Eigen::Vector4d product_vec = q1_matrix * q2_vec;
  Eigen::Vector4d product_vec2 = q1_matrix2 * q2_vec2;
  std::cout << "product      " << "w : " << product.w() << ", x : " << product.x() << ", y : " << product.y() << ", z : " << product.z() << std::endl;
  std::cout << "product_vec  " << "w : " << product_vec[0] << ", x : " << product_vec[1] << ", y : " << product_vec[2] << ", z : " << product_vec[3] << std::endl;
  std::cout << "product_vec2 " << "w : " << product_vec2[0] << ", x : " << product_vec2[1] << ", y : " << product_vec2[2] << ", z : " << product_vec2[3] << std::endl;
}

としたら

product      w : 0.761802, x : -0.204124, y : 0.502983, z : 0.353553
product_vec  w : 0.761802, x : -0.204124, y : 0.502983, z : 0.353553
product_vec2 w : 0.761802, x : -0.204124, y : 0.502983, z : 0.353553

となり，一致した！！！

上のだとクォータニオンが正規化されているので，正規化されていないクォータニオンでもいけるか確かめてみた

#include <iostream>
#include <math.h>
#include <Eigen/Dense>
int main(){
  Eigen::Quaternion<double> q1 = Eigen::Quaternion<double>(1, 2, 3, 4);
  Eigen::Quaternion<double> q2 = Eigen::Quaternion<double>(cos(M_PI/3/2), (-1/sqrt(3))* sin(M_PI/3/2), (1/sqrt(3)) * sin(M_PI/3/2), (1/sqrt(3)) * sin(M_PI/3/2));
  Eigen::Matrix<double, 4, 4> q1_matrix;
  q1_matrix <<
    q1.w(), -q1.x(), -q1.y(), -q1.z(),
    q1.x(),  q1.w(), -q1.z(),  q1.y(),
    q1.y(),  q1.z(),  q1.w(), -q1.x(),
    q1.z(), -q1.y(),  q1.x(),  q1.w();
  Eigen::Matrix<double, 4, 4> q1_matrix2;
  q1_matrix2 <<
    1, -2, -3, -4,
    2,  1, -4,  3,
    3,  4,  1, -2,
    4, -3,  2,  1;
  Eigen::Vector4d q2_vec;
  q2_vec << q2.w(), q2.x(), q2.y(), q2.z();
  Eigen::Vector4d q2_vec2;
  q2_vec2 << cos(M_PI/3/2), (-1/sqrt(3))* sin(M_PI/3/2), (1/sqrt(3)) * sin(M_PI/3/2), (1/sqrt(3)) * sin(M_PI/3/2);
  Eigen::Quaternion<double> product = q1 * q2;
  Eigen::Vector4d product_vec = q1_matrix * q2_vec;
  Eigen::Vector4d product_vec2 = q1_matrix2 * q2_vec2;
  std::cout << "product      " << "w : " << product.w() << ", x : " << product.x() << ", y : " << product.y() << ", z : " << product.z() << std::endl;
  std::cout << "product_vec  " << "w : " << product_vec[0] << ", x : " << product_vec[1] << ", y : " << product_vec[2] << ", z : " << product_vec[3] << std::endl;
  std::cout << "product_vec2 " << "w : " << product_vec2[0] << ", x : " << product_vec2[1] << ", y : " << product_vec2[2] << ", z : " << product_vec2[3] << std::endl;
}

結果は

product      w : -0.57735, x : 1.1547, y : 1.1547, z : 5.19615
product_vec  w : -0.57735, x : 1.1547, y : 1.1547, z : 5.19615
product_vec2 w : -0.57735, x : 1.1547, y : 1.1547, z : 5.19615

となり，一致したので，クォータニオンを4×4の行列計算に使えることが分かった．