Eigen使用小技巧

前言

  不得不说pcl功能确实很强大，强大到可以通过调用函数实现样点的操作，但是对于独立的小功能而言，很多时候并不一定需要调用pcl库，而是可以通过简单的数学运算搞定。本文章主要记录一些简单功能的原理与Eigen的实现。

一、矩阵转换

  在点云处理过程中，需要经常性的对点云进行空间姿态的转换，在pcl中，通过调用pcl::transformPointCloudWithNormals()与pcl::transformPointCloud()可实现对点云的坐标转换，但是，此函数需要特定的数据类型作为输入，在使用中，如果只是对个别样点进行坐标转换，采用调函数的方式就略显臃肿了，所以，现将该函数重构成Eigen库的实现。
  旋转矩阵与样点及其属性的转换主要有两个：其一为空间坐标的转换，其二为空间方向的转换。
  基本步骤：

1. 将3维坐标扩展为齐次坐标：旋转矩阵表示空间坐标的组合变换，即包含旋转变换，又包含平移变换，从变换关系分析，需要先进行旋转运动，然后再进行平移运动，旋转的计算用*完成，平移的计算用+完成；从计算形式上分析，是一个4*4的矩阵，所以要保持行列一致才能进行计算，具体齐次坐标转化为：

空间坐标：(100, 200, 300) -> (100, 200, 300, 1)
空间方向：(1.0, 0.0, 0.0) -> (1.0, 0.0, 0.0, 0)

1. 使用齐次坐标表示矩阵相乘：

1. 测试代码如下：

    pcl::PointNormal tmp;
    tmp.x = 100;
    tmp.y = 200;
    tmp.z = 300;
    tmp.normal_x = 1.0;
    tmp.normal_y = 0.0;
    tmp.normal_z = 0.0;
    Eigen::Vector4f point(tmp.x, tmp.y, tmp.z, 1);
    Eigen::Vector4f normal(tmp.normal_x, tmp.normal_y, tmp.normal_z, 0);
    cout << "point = " << (mat * point).transpose() << endl;
    cout << "normal = " << (mat * normal).transpose() << endl;

二、旋转矩阵与欧拉角的转换

  欧拉角是用来唯一地确定定点转动刚体位置的三个一组独立角参量，决定刚体最终姿态的属性有两个：旋转顺序与旋转角度，只要两者有任意不同量，则刚体的最终形态也是不一样的。根据旋转顺序的不同，一共有6中方式：x-y-z,x-z-y,y-x-z,y-z-x,z-x-y,z-y-x。其中，x-y-z和z-y-x是用的最多的方式。

1、欧拉角转旋转矩阵

  根据欧拉角生成旋转矩阵相对简单点，

rotateMat = R(x)*R(y)*R(z)       

  其中，R(x),R(y),R(z)分别表示独立按照x,y,z旋转一定角度的矩阵（x,y,z即表示旋转轴又表示旋转角度，网上资料较多，可百度），三者相乘即可得到组合旋转矩阵。

2、旋转矩阵转欧拉角

  三位空间的旋转矩阵可以表示为3*3的矩阵，假设欧拉角分别为alpha,beta,gamma,则旋转矩阵的计算如下（按照z-y-x的旋转顺序）：

  可根据最终的旋转矩阵反求出alpha,beta,gamma
  测试代码如下：

  【C/C++计算】
    //将旋转矩阵a转换成欧拉角
    double sin_beta = a(0, 2);
    double beta = 180 * asin(sin_beta) / 3.1415926;

    double z_tan = -1 * a(0, 1) / a(0, 0);
    double gamma = 180 * atan(z_tan) / 3.1415926;
    double x_tan = -1 * a(1, 2) / a(2, 2);
    double alpha = 180 * atan(x_tan) / 3.1415926;
    
  【Eigen计算】
        Eigen::Vector3d  eu = a.eulerAngles(0, 1, 2);
        eu = eu * 180 / 3.141592654;