Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
行列演算ライブラリEigenのGeometry関連(回転行列, クォータニオン)の関数サンプルプログラム
/*
FileName: GeometrySample.cpp
Discription: EigenのGeometry関連の関数のサンプル
Author: Atsushi Sakai
Update: 2013/03/16
*/
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Geometry> //EigenのGeometry関連の関数を使う場合,これが必要
//データ表示用マクロ
#define PRINT_MAT(X) cout << #X << ":\n" << X << endl << endl
using namespace std;
using namespace Eigen;
void Sample1(){
cout<<"====1. 2次元空間における回転行列を使用した点の回転====="<<endl;
//変換したい点を作成
VectorXf point_in(2), point_out(2);
point_in<<1,1;
PRINT_MAT(point_in);
//回転行列の作成
Matrix2f rot;
rot=Rotation2Df(M_PI); //90度反時計回りに回転
PRINT_MAT(rot);
//回転行列をかけて回転
point_out=rot*point_in;
PRINT_MAT(point_out);
}
void Sample2(){
cout<<"====2. 3次元空間における軸回転関数を使用した点の回転====="<<endl;
//変換したい点を作成
VectorXf point_in_3d(3), point_out_3d(3);
point_in_3d<<1,1,1; //[x y z]
PRINT_MAT(point_in_3d);
//ある軸に対する回転行列を作成
Matrix3f AxisAngle;
Vector3f axis;
axis<<0,0,1; //z軸を指定
AxisAngle=AngleAxisf(M_PI,axis); //Z軸周りに90度反時計回りに回転
PRINT_MAT(AxisAngle);
//回転行列をかけて回転
point_out_3d=AxisAngle*point_in_3d;
PRINT_MAT(point_out_3d);
}
void Sample3(){
cout<<"====3. クォータニオンを使用した点の回転====="<<endl;
//変換したい点を作成
VectorXf point_in_3d(3), point_out_3d(3);
point_in_3d<<1,1,1; //[x y z]
PRINT_MAT(point_in_3d);
//ある軸に対する回転のクォータニオンを作成
Quaternionf quat;
Vector3f axis;
axis<<0,1,0; //Y軸を指定
quat=AngleAxisf(M_PI,axis); //Y軸周りに90度反時計回りに回転
//PRINT_MAT(quat); //Quaternion型はcoutで出力できない
//クォータニオンを回転行列に変換する方法
MatrixXf t;
t=quat.matrix();
PRINT_MAT(t); //Quaternion型はcoutで出力できない
//回転行列をかけて回転
point_out_3d=quat*point_in_3d;
PRINT_MAT(point_out_3d);
}
void Sample4(){
cout<<"====4. 3次元ベクトルのスケール変更====="<<endl;
//変換したい点を作成
VectorXf point_in_3d(3), point_out_3d(3);
point_in_3d<<1,1,2; //[x y z]
PRINT_MAT(point_in_3d);
point_out_3d=2.0*point_in_3d; //スケールを変える時には定数をかければOK
//point_out_3d=Scaling(2.0)*point_in_3d; //これでもOK
PRINT_MAT(point_out_3d);
}
void Sample5(){
cout<<"====5. 逆変換====="<<endl;
//変換したい点を作成
VectorXf point_in_3d(3), point_out_3d(3);
point_in_3d<<1,1,2; //[x y z]
PRINT_MAT(point_in_3d);
//ある軸に対する回転行列を作成
Matrix3f AxisAngle;
Vector3f axis;
axis<<1,0,0; //x軸を指定
AxisAngle=AngleAxisf(M_PI,axis); //x軸周りに90度反時計回りに回転
PRINT_MAT(AxisAngle);
//回転行列をかけて回転
point_out_3d=AxisAngle*point_in_3d;
PRINT_MAT(point_out_3d);
//回転行列の逆行列からもとに戻す
point_out_3d=AxisAngle.inverse()*point_out_3d;
PRINT_MAT(point_out_3d);
}
int main()
{
cout<<"Eigen Geometry Sample Code"<<endl;
Sample1();
Sample2();
Sample3();
Sample4();
Sample5();
}
@s059ff

This comment has been minimized.

Copy link

@s059ff s059ff commented Jan 27, 2018

90°回転じゃなくて、180°回転の間違いじゃないですか?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment