两点云坐标点的转化

两点云坐标点的转化

两点云坐标点的转化，就是把一个点云从自己的坐标系变换到另一个坐标系，配准，拼接都用得到。有点类似相机和激光外参的标定（将激光坐标系转换到相机的坐标系。都是Ｒt变换）。

（１）基本知识

R为旋转矩阵，X为原始点，t为平移量，X'为变换后的点（目标点）

R*X+t=X'  （Ｒt*X=X'）

但是所求Ｒt矩阵用一个矩阵表示如下：

 /* Reminder: how transformation matrices work :
           |-------> This column is the translation
    | 1 0 0 x |  \
    | 0 1 0 y |   }-> The identity 3x3 matrix (no rotation) on the left
    | 0 0 1 z |  /
    | 0 0 0 1 |    -> We do not use this line (and it has to stay 0,0,0,1)

（２）举例说明

将一个点（坐标系）绕自身ｚ轴旋转（正）M_PI/4，然后再沿着旋转后得到的ｘ轴方向（正）平移2.5。求Rt矩阵？

方法１：

直接数学方法得到并赋值：

    METHOD #1: Using a Matrix4f
    This is the "manual" method, perfect to understand but error prone !
  */
  Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();

  // Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
  float theta = M_PI/4; // The angle of rotation in radians
  transform_1 (0,0) = cos (theta);　//第１行第１列个元素
  transform_1 (0,1) = -sin(theta);　//第１行第 2 列个元素
  transform_1 (1,0) = sin (theta);
  transform_1 (1,1) = cos (theta);
  //    (row, column)

  // Define a translation of 2.5 meters on the x axis.
  // transform_1 (0,3) = 2.5;
  transform_1 (0,3) = 0;
  // Print the transformation
  printf ("Method #1: using a Matrix4f\n");
  std::cout << transform_1 << std::endl;

方法２：

通过程序计算矩阵：

  /*  METHOD #2: Using a Affine3f
    This method is easier and less error prone
  */
  Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();

  // Define a translation of 2.5 meters on the x axis.
  transform_2.translation() << 2.5, 0.0, 0.0;
  // The same rotation matrix as before; theta radians arround Z axis
  transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitZ()));

  // Print the transformation
  printf ("\nMethod #2: using an Affine3f\n");
  std::cout << transform_2.matrix() << std::endl;

最后转化：

pcl::transformPointCloud (*source_cloud, *transformed_cloud, transform_2);

方法３：（轴对应的整数变换）

参考loam_velodyne代码通过赋值：

  PointType point;
  std::vector<pcl::PointCloud<PointType> > laserCloudScans(N_SCANS);
  for (int i = 0; i < cloudSize; i++) {
    point.x = laserCloudIn.points[i].y; // y轴换到x轴
    point.y = laserCloudIn.points[i].z; // z轴换到y轴
    point.z = laserCloudIn.points[i].x; // x轴换到z轴
    float angle = atan(point.y / sqrt(point.x * point.x + point.z * point.z)) * 180 / M_PI;
    int scanID;
    int roundedAngle = int(angle + (angle<0.0?-0.5:+0.5)); 
    if (roundedAngle > 0){
      scanID = roundedAngle;
    }
    else {
      scanID = roundedAngle + (N_SCANS - 1);
    }
    if (scanID > (N_SCANS - 1) || scanID < 0 ){
      count--;
      continue;
    }