目次
自分の
1. 回転ベクトル
1.1 初期化
回転角度: alpha alphaa lp ha (時計回り)、回転軸:( x , y , z ) (x,y,z)( x ,よ、z )
Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z))
Eigen::AngleAxisd yawAngle(alpha,Vector3d::UnitZ());
1.2 回転ベクトル <-> 回転行列
Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.matrix();
Eigen::Matrix3d rotation_matrix;
rotation_matrix=rotation_vector.toRotationMatrix();
1.3 回転ベクトル -> オイラー角
Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(0,1,2);
1.4 回転ベクトルをクォータニオンに変換する
Eigen::Quaterniond quaternion(rotation_vector);
Eigen::Quaterniond quaternion;
quaternion=rotation_vector;
2. 回転行列
2.1 初期化
Eigen::Matrix3d rotation_matrix;
rotation_matrix<<x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22;
2.2 回転行列 -> 回転ベクトル
Eigen::AngleAxisd rotation_vector(rotation_matrix);
Eigen::AngleAxisd rotation_vector;
rotation_vector=rotation_matrix;
Eigen::AngleAxisd rotation_vector;
rotation_vector.fromRotationMatrix(rotation_matrix);
2.3 回転行列 -> オイラー角
Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(0,1,2);
2.4 回転行列 -> クォータニオン
Eigen::Quaterniond quaternion(rotation_matrix);
Eigen::Quaterniond quaternion;
quaternion=rotation_matrix;
3. オイラー角
3.1 初期化
Eigen::Vector3d eulerAngle(roll,pitch,yaw);
3.2 オイラー角 -> 回転ベクトル
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));
Eigen::AngleAxisd rotation_vector;
rotation_vector=yawAngle*pitchAngle*rollAngle;
3.3 オイラー角 -> 回転行列
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));
Eigen::Matrix3d rotation_matrix;
rotation_matrix=yawAngle*pitchAngle*rollAngle;
3.4 オイラー角 -> クォータニオン
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));
Eigen::Quaterniond quaternion;
quaternion=yawAngle*pitchAngle*rollAngle;
4. クォータニオン
4.1 初期化
Eigen::Quaterniond quaternion(w,x,y,z);
4.2 クォータニオン -> 回転ベクトル
Eigen::AngleAxisd rotation_vector(quaternion);
Eigen::AngleAxisd rotation_vector;
rotation_vector=quaternion;
4.3 クォータニオン -> 回転行列
Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.matrix();
Eigen::Matrix3d rotation_matrix;
rotation_matrix=quaternion.toRotationMatrix();
4.4 クォータニオン -> オイラー角
Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(0,1,2);
5.アイソメトリ3D
5.1 初期化
- 各要素に値を割り当てます
Eigen::Isometry3d T1=Eigen::Isometry3d::Identity();
T1(0,0) = 1.000000e+00, T1(0,1) = 1.197624e-11, T1(0,2) = 1.704639e-10, T1(0,3) = 3.214096e-14;
T1(1,0) = 1.197625e-11, T1(1,1) = 1.197625e-11, T1(1,2) = 3.562503e-10, T1(1,3) = -1.998401e-15;
T1(2,0) = 1.704639e-10, T1(2,1) = 3.562503e-10, T1(2,2) = 1.000000e+00, T1(2,3) = -4.041212e-14;
T1(3,0) = 0, T1(3,1) = 0, T1(3,2) = 0, T1(3,3) = 1;
- 回転行列と平行移動ベクトルによる
Eigen::Isometry3d Tc1w = Eigen::Isometry3d::Identity();
Tc1w.rotate(rotation_matrix); // 按照rotation_matrix进行旋转
Tc1w.pretranslate(t); // 把平移向量设成t
- Eigen::Matrix4d は変換行列を構築します
Eigen::Matrix4d T2;
T2.setIdentity();
T2.block<3,3>(0,0) = rotation_matrix1;
T2.topRightCorner(3, 1) = t1;
5.2 回転行列を取得する
Eigen::Matrix3d rotation = Tc1w.rotation();
5.2 翻訳ベクトルの取得
Eigen::Vector3d position = Tc1w.translation();
6.Eigen::Affine3fとEigen::Matrix4f間の変換
Eigen::Affine3f A;
Eigen::Matrix4f M;
M = A.matrix();
A = M;
7. float 型と double 型の変換
Eigen::MatrixXd matrix_d;
Eigen::MatrixXf matrix_f;
matrix_f = matrix_d.cast<float>();
nav_msgs::オドメトリ
Header header
uint32 seq
time stamp
string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
geometry_msgs/TwistWithCovariance twist
geometry_msgs/Twist twist
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
float64[36] covariance
継続的改善…
参考記事:
Eigenポーズ表現は
Eigenを利用してクォータニオン、オイラー角、回転行列、回転ベクトル間の変換を実現