使用环境:ubuntu16.04、Kdevelop4.0
话不多说,我们先直接上CMakeLists.txt and main.cpp的代码
CMakeLists.txt的代码如下:
cmake_minimum_required(VERSION 2.6)
project(eigen_geometry)
# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )
set(CMAKE_BUILD_TYPE "Debug")
add_executable(eigen_geometry main.cpp)
install(TARGETS eigen_geometry RUNTIME DESTINATION bin)
main.cpp的代码如下:
#include <iostream>
#include <cmath>
#include <ctime>
using namespace std;
#include <Eigen/Core>
// Eigen 几何模块
#include <Eigen/Geometry>
#define MATRIX_SIZE 50
/****************************
* 本程序演示了 Eigen 几何模块的使用方法
****************************/
int main ( int argc, char** argv )
{
// Eigen/Geometry 模块提供了各种旋转和平移的表示
// 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
// 旋转向量使用 AngleAxis, 它底层不直接是Matrix,但运算可以当作矩阵(因为重载了运算符)
Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转 45 度
cout .precision(3);
cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl; //用matrix()转换成矩阵
// 也可以直接赋值
rotation_matrix = rotation_vector.toRotationMatrix();
// 用 AngleAxis 可以进行坐标变换
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 ); // ZYX顺序,即roll pitch yaw顺序
cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;
// 欧氏变换矩阵使用 Eigen::Isometry
Eigen::Isometry3d T=Eigen::Isometry3d::Identity(); // 虽然称为3d,实质上是4*4的矩阵
T.rotate ( rotation_vector ); // 按照rotation_vector进行旋转
T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) ); // 把平移向量设成(1,3,4)
cout << "Transform matrix = \n" << T.matrix() <<endl;
// 用变换矩阵进行坐标变换
Eigen::Vector3d v_transformed = T*v; // 相当于R*v+t
cout<<"v tranformed = "<<v_transformed.transpose()<<endl;
// 对于仿射和射影变换,使用 Eigen::Affine3d 和 Eigen::Projective3d 即可,略
// 四元数
// 可以直接把AngleAxis赋值给四元数,反之亦然
Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
cout<<"quaternion = \n"<<q.coeffs() <<endl; // 请注意coeffs的顺序是(x,y,z,w),w为实部,前三者为虚部
// 也可以把旋转矩阵赋给它
q = Eigen::Quaterniond ( rotation_matrix );
cout<<"quaternion = \n"<<q.coeffs() <<endl;
// 使用四元数旋转一个向量,使用重载的乘法即可
v_rotated = q*v; // 注意数学上是qvq^{-1}
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
//下面为自己的测试程序
clock_t time_stt=clock();
Eigen::Vector3d p (0.5,0,0.2);
Eigen::Vector3d p2 (0,0,0);
Eigen::Quaterniond q1 (0.2,0.3,0.1,0.35);
Eigen::Quaterniond q2 (0.4,-0.1,0.2,-0.5);
Eigen::Vector3d t1 (0.3,0.1,0.1);
Eigen::Vector3d t2 (-0.1,0.5,0.3);
Eigen::Isometry3d T1=Eigen::Isometry3d::Identity();
Eigen::Isometry3d T2=Eigen::Isometry3d::Identity(); // 虽然称为3d,实质上是4*4的矩阵
T1.rotate ( q1.toRotationMatrix() ); // 按照rotation_vector进行旋转
T1.pretranslate ( t1 );
T2.rotate (q2.toRotationMatrix() );
T2.pretranslate (t2);
//对T1矩阵进行求逆的操作,使用方法为使用矩阵分解,这样的运行速度较快
Eigen::Matrix< double, MATRIX_SIZE,MATRIX_SIZE> matrix_NN;
matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);
Eigen::Matrix< double,4, 1> v_Nd;
v_Nd= Eigen::MatrixXd::Random(4,1);
T1=T1.inverse();
p2=T2*T1*p;
cout<<"p2's coordinate in camara2 is "<<p2.transpose()<<endl;
cout << "Transform matrix = \n" << T1.matrix() <<endl;
cout<<"time use is :"<<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl;
return 0;
}
Tips1:这里有一个语句是cout .precision(3),表示输出的浮点数的精度(保留的小数点后的位数),下面的另外一张截图表示cout .precision(4)
Tips2:Eigen 中对各种形式的表达方式总结如下。请注意每种类型都有单精度和双精度两种
数据类型,而且和之前一样,不能由编译器自动转换。下面以双精度为例,你可以把最后
的 d 改成 f,即得到单精度的数据结构。
• 旋转矩阵(3 × 3):Eigen::Matrix3d。
• 旋转向量(3 × 1):Eigen::AngleAxisd。
• 欧拉角(3 × 1):Eigen::Vector3d。
• 四元数(4 × 1):Eigen::Quaterniond。
• 欧氏变换矩阵(4 × 4):Eigen::Isometry3d。
• 仿射变换(4 × 4):Eigen::Affine3d。
• 射影变换(4 × 4):Eigen::Projective3d。