该代码源于《视觉SLAM十四讲》 Eigen 中Geometry (几何模块)
主要操作有旋转向量和旋转矩阵之间的转换;欧拉角(旋转矩阵变欧拉角);欧式变换(旋转+平移);四元数
该页代码用于日后查找方便 (添加了部分笔记)
#include <iostream>
#include <cmath>
using namespace std;
#include <Eigen/Core>
#include <Eigen/Geometry>
int main ( int argc, char** argv )
{
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );
cout .precision(3);
cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl;
rotation_matrix = rotation_vector.toRotationMatrix();
Eigen::Vector3d v ( 1,0,0 );
Eigen::Vector3d v_rotated = rotation_vector * v;
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
v_rotated = rotation_matrix * v;
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 );
cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;
Eigen::Isometry3d T=Eigen::Isometry3d::Identity();
T.rotate ( rotation_vector );
T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) );
cout << "Transform matrix = \n" << T.matrix() <<endl;
Eigen::Vector3d v_transformed = T*v;
cout<<"v tranformed = "<<v_transformed.transpose()<<endl;
Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
cout<<"quaternion = \n"<<q.coeffs() <<endl;
q = Eigen::Quaterniond ( rotation_matrix );
cout<<"quaternion = \n"<<q.coeffs() <<endl;
v_rotated = q*v;
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
return 0;
}
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)