Slow implementation of tranformPointCloud

[UditSinghParihar]

Hi, @TixiaoShan and Lego-Loam team,

Thanks for your wonderful work. This function from the lego loam is used to transform Pointcloud from one frame to another. It is taking a single point from the source point cloud and then applying the transform operation on the source point and then filling the target Pointcloud in for a loop. I want to ask if we can convert the transform operations into an Eigen matrix and then apply the transformation matrix on the whole source PointCloud in a single-shot manner (matrix multiplication) than the function should be quite fast. Transformation matrix, T(4x4) and Source PointCloud in Homogenous format, sourcePC(4XN), then, target PointCloud, targetPC is:

targetPC = T x sourcePointCloud

What's your thought on this, what am I missing here?

pcl::PointCloud<PointType>::Ptr MapOptimization::transformPointCloud(
    pcl::PointCloud<PointType>::Ptr cloudIn) {
  // !!! DO NOT use pcl for point cloud transformation, results are not
  // accurate Reason: unkown
  pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());

  PointType *pointFrom;
  PointType pointTo;

  int cloudSize = cloudIn->points.size();
  cloudOut->resize(cloudSize);

  for (int i = 0; i < cloudSize; ++i) {
    pointFrom = &cloudIn->points[i];
    float x1 = ctYaw * pointFrom->x - stYaw * pointFrom->y;
    float y1 = stYaw * pointFrom->x + ctYaw * pointFrom->y;
    float z1 = pointFrom->z;

    float x2 = x1;
    float y2 = ctRoll * y1 - stRoll * z1;
    float z2 = stRoll * y1 + ctRoll * z1;

    pointTo.x = ctPitch * x2 + stPitch * z2 + tInX;
    pointTo.y = y2 + tInY;
    pointTo.z = -stPitch * x2 + ctPitch * z2 + tInZ;
    pointTo.intensity = pointFrom->intensity;

    cloudOut->points[i] = pointTo;
  }
  return cloudOut;
}

Regards, Udit