pcl点云数据 转化为 Eigen::Map

pcl点云数据转化为Eigen::Map

需求：了解getMatrixXfMap()函数参数与相关用法，点云程序进行CUDA转写时，可能用到。

参考：
pcl点云和Eigen::Map转换

1.函数说明：

1.1 优势说明

点云数据点格式转换为矩阵格式进行计算，如果直接进行赋值，会重新开辟内存空间，对大数据点云来说并不现实。
因此使用Eigen::Map进行内存映射，节省内存空间，加快处理速度。

1.2 函数原型

// pcl/point_cloud.h文件中
inline Eigen::Map<Eigen::MatrixXf, Eigen::Aligned, Eigen::OuterStride<> > 
      getMatrixXfMap (int dim, int stride, int offset)

注意：
从 pcl1.4.0 开始，Eigen 矩阵被强制为行优先（Row Major） 以提高 PCL 中算法的效率。
原文中：通过正确地 1比1方式 映射 PointCloud 结构到Eigen::Map中，即：点云中的点数 = 矩阵中的行数，点维度 = 矩阵中的列数。

本文得到结论与原文行列是相反的，点云中的点数 = 矩阵中的列，点维度 = 矩阵中的行数，如getMatrixXfMap(4,4,0)可以将xyzi点云数据转化为 4*size 的点云矩阵。

1.3 参数说明

• 参数1： dim ，每一个点需要顾及到的维度
• 参数2： stride， 每一个点中所包含的值的数目
• 参数3： offset，从每一个点起始位置，跳过的数目

1.4 具体实例

1. 新建一个点云，点云的x，y，z，i分别按照索引值进行赋值，方便打印查看。

pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_test(new pcl::PointCloud<pcl::PointXYZI>);
for (int i = 0; i < 10; i++) {
    pcl::PointXYZI p;
    p.x = i;
    p.y = i;
    p.z = i;
    p.intensity = i;
    cloud_test->points.push_back(p);
}

2）使用转换函数提取xyz坐标值

auto pc_matrix = cloud_test->getMatrixXfMap(3,8,0);

说明：为何是(3,8,0)?
因为每一个点取xyz三个维度，故第一个dim取3。第三个偏移值从首位x开始，故第三个参数offset=0。

第二个值为何取8？
开始可能会存在疑问，因为每一个点都是xyzi ，包含4个浮点数，按说应该是4才对，但是查看pcl::PointXYZI结构的组织形式如下，我们发现data[3]并非intensity强度值，而是赋值1.0f。
进一步，查看EIGEN_ALIGN16，发现采用16字节对齐方式，一个浮点数float占据4个字节。即这8个浮点数分别是x,y,z,data[3]=1.0f, data[4]=intensity，data[5] ,data[6] ,data[7]。(其中data[5] ,data[6] ,data[7]并未赋值)

// PointXYZI结构体
  struct PointXYZI : public _PointXYZI
  {
    inline PointXYZI (const _PointXYZI &p)
    {
      x = p.x; y = p.y; z = p.z; data[3] = 1.0f;
      intensity = p.intensity;
    }
  }
  
// EIGEN_ALIGN16 16字节对齐
  struct EIGEN_ALIGN16 _PointXYZI
  {
    PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4])
    union
    {
      struct
      {
        float intensity;
      };
      float data_c[4];
    };
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };

3. 使用转换函数提取intensity矩阵

auto trans_cloudi = cloud_test->getMatrixXfMap(1,8,4);

4）打印pc_matrix和trans_cloudi值，得到4*10的矩阵
结果图：

同样可以通过属性，查看矩阵维度，如下：

auto col = pc_matrix.row(0).size(); // 10
auto row = pc_matrix.col(0).size(); // 3 
auto intensity_col = trans_cloudi.row(0).size(); // 10