Eigen几何模块

最新推荐文章于 2024-04-25 11:27:18 发布

25zhixun

最新推荐文章于 2024-04-25 11:27:18 发布

阅读量296

点赞数

分类专栏： Ubuntu 文章标签： Eigen几何模块

本文链接：https://blog.csdn.net/ljl1015ljl/article/details/95447738

版权

Ubuntu 专栏收录该内容

39 篇文章 2 订阅

订阅专栏

useGeometry.cpp

#include <iostream>
#include <cmath>
using namespace std;

#include <Eigen/Core>
// Eigen 几何模块
#include <Eigen/Geometry>

/****************************
* 本程序演示了 Eigen 几何模块的使用方法
****************************/

int main ( int argc, char** argv )
{
    // Eigen/Geometry 模块提供了各种旋转和平移的表示
    // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
    //Eigen::Matrix3d::Identity() ==单位矩阵
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    
    // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
    //M_PI=180度    （0,0,1）沿 Z 轴旋转45度  
    Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );     
    //保留3位小数
    cout .precision(3);
    //旋转向量用matrix()转换成旋转矩阵
    cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl;
    //0.707    -0.707    0
    //0.707     0.707    0
    //    0         0    1
    //单位矩阵绕z轴旋转45度，解析
	//[1 0 0]     [cos45 -sin45 0]
	//[0 1 0] *   [sin45  cos45 0]
	//[0 0 1]     [    0      0 1]
    // 也可以直接赋值 旋转向量--》旋转矩阵
    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;
    //3*3的向量 与 3*1 相乘
    //0.707 0.707 0
    
    // 或者用旋转矩阵
    v_rotated = rotation_matrix * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
    //0.707 0.707 0
	    
    // 欧拉角: 可以将旋转矩阵直接转换成欧拉角( euler Angles )
      // ZYX顺序，即yaw pitch roll 顺序
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 );
    cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;
    //0.785 -0 0

    // 欧氏变换矩阵 使用 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;
    //0.707 -0.707 0 1
    //0.707  0.707 0 3
    //    0      0 1 4
    //    0      0 0 1
    
    // 用变换矩阵进行坐标变换
    Eigen::Vector3d v_transformed = T*v;        // 相当于R*v+t
    cout<<"v tranformed = "<<v_transformed.transpose()<<endl;
    //1.71 3.71 4 

    // 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略

    // 四元数
    // 可以直接把AngleAxis赋值给四元数，反之亦然
    Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
    //0 0 0.383 0.924

    // 也可以把旋转矩阵赋给它
    q = Eigen::Quaterniond ( rotation_matrix );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;
    //0 0 0.383 0.924
    
    // 使用四元数旋转一个向量，使用重载的乘法即可
    v_rotated = q*v; // 注意数学上是qvq^{-1}
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
    //0.707 0.707 0
    return 0;
}

CMakeLists.txt

//声明cmake要求的最低版本
cmake_minimum_required(VERSION 2.8)
//设置编译模式
set(CMAKE_BUILD_TYPE "Debug")
//添加头文件
include_directories("/usr/include/eigen3")
//添加了头文件就不需要链接到库了
//target_link_libraries(程序名 库名）
//声明一个cmake工程
project(UseGeometry)
//添加可执行程序 （程序名 源代码文件）
add_executable(useGeometry useGeometry.cpp)