Rotate point cloud around it's origin?

classic Classic list List threaded Threaded
5 messages Options
Reply | Threaded
Open this post in threaded view
|

Rotate point cloud around it's origin?

john24
Hello, I wanted to ask how I do rotate a cloud around it's origin. Before I used the sensor_orientation attribute but it proved troublesome when computing and displaying normals, keypoints and the rest.
So now I just attach orientation sensor data to point cloud at the start by rotating them. But it seems to rotate around an origin that is far away from the point cloud and gives me this affect which impacts registration negatively:

(as you can see the green point cloud rotates around an origin that is far away from it)

The code i used as a test:

        //Load point clouds here

        Eigen::Affine3f transform = Eigen::Affine3f::Identity();
        transform.translation() << 0.0, 0.0, 0.0;
        transform.rotate(Eigen::AngleAxisf((0.0*M_PI) / 180, Eigen::Vector3f::UnitX()));
        transform.rotate(Eigen::AngleAxisf((60.0*M_PI) / 180, Eigen::Vector3f::UnitY()));
        transform.rotate(Eigen::AngleAxisf((0.0*M_PI) / 180, Eigen::Vector3f::UnitZ()));
        pcl::transformPointCloud(*vox_tgt, *vox_tgt, transform); //Only rotate target cloud


I did translate them to 0 0 0 origin before rotating but that doesn't help, is there a way to rotate the point cloud around it's own origin?

Regards
Reply | Threaded
Open this post in threaded view
|

Re: Rotate point cloud around it's origin?

VictorLamoine
Administrator
Hello,

You are doing exactly what you say, you are rotating the point cloud around it's origin (the coordinate system displayed on your image is the origin of the point cloud).
But I guess that's not what you want to do, you probably want to rotate the point cloud around it's centroid!

My solution to do that is to apply a transformation to the centroid and get the vector between the non-rotated centroid and the rotated centroid, by withdrawing that translation you rotate around the centroid.

Here is a code example:
#include <pcl/console/parse.h>
#include <pcl/point_types.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/common/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

int main(int argc, char *argv[])
{
  std::vector<int> ply_file_indices = pcl::console::parse_file_extension_argument(argc, argv, ".ply");
  if (ply_file_indices.size() != 1)
  {
    PCL_ERROR("Wrong usage: %s input.ply\n", argv[0]);
  }

  // Load mesh
  pcl::PolygonMesh cad;
  if (pcl::io::loadPolygonFilePLY(argv[ply_file_indices[0]], cad) == 0)
  {
    PCL_ERROR("Failed to read PLY file %s\n", argv[ply_file_indices[0]]);
    return -1;
  }

  PointCloudT::Ptr cloud (new PointCloudT),
                   cloud_tf_1 (new PointCloudT),
                   cloud_tf_2 (new PointCloudT);
  pcl::fromPCLPointCloud2(cad.cloud, *cloud);

  pcl::visualization::PCLVisualizer viewer;
  viewer.addCoordinateSystem(0.5);
  viewer.addPointCloud(cloud);

  Eigen::Affine3f transform (Eigen::Affine3f::Identity());
  Eigen::Matrix3f rotation (Eigen::AngleAxisf((60.0*M_PI) / 180, Eigen::Vector3f::UnitZ()));
  transform.rotate(rotation);
  pcl::transformPointCloud(*cloud, *cloud_tf_1, transform);
  std::cout << transform.matrix() << std::endl << std::endl;

  Eigen::Vector4f centroid (Eigen::Vector4f::Zero());
  pcl::compute3DCentroid(*cloud, centroid);
  Eigen::Vector4f centroid_new (Eigen::Vector4f::Zero());
  centroid_new.head<3>() = rotation * centroid.head<3>();
  transform.translation() = centroid.head<3>() - centroid_new.head<3>();
  pcl::transformPointCloud(*cloud, *cloud_tf_2, transform);
  std::cout << transform.matrix() << std::endl << std::endl;

  pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tf_1_color_handler (cloud_tf_1, 25, 200, 25);
  viewer.addPointCloud (cloud_tf_1, cloud_tf_1_color_handler, "cloud_tf_1");

  pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tf_2_color_handler (cloud_tf_2, 200, 25, 25);
  viewer.addPointCloud (cloud_tf_2, cloud_tf_2_color_handler, "cloud_tf_2");
  viewer.spin();
  return 0;
}
The white cloud is the original cloud, the green one rotated around the origin, the red one rotated around the white cloud centroid.

Bye
Reply | Threaded
Open this post in threaded view
|

Re: Rotate point cloud around it's origin?

john24
This post was updated on .
Hi Victor,

Really big thanks for the code :) Works like a charm.

Just wanted to ask how would I apply rotation on all 3 axis?

The way I am thinking does not make a for a graceful solution.

The code I got:

                Eigen::Affine3f transform(Eigen::Affine3f::Identity());

                transform.rotate(Eigen::AngleAxisf((orient->y*M_PI) / 180, Eigen::Vector3f::UnitZ()));
                transform.rotate(Eigen::AngleAxisf((orient->x*M_PI) / 180, Eigen::Vector3f::UnitY()));
                transform.rotate(Eigen::AngleAxisf((orient->z*M_PI) / 180, Eigen::Vector3f::UnitX()));

                Eigen::Vector4f centroid(Eigen::Vector4f::Zero());
                pcl::compute3DCentroid(*outCloud, centroid);
                Eigen::Vector4f centroid_new(Eigen::Vector4f::Zero());
                centroid_new.head<3>() = transform.rotation() * centroid.head<3>();
                transform.translation() = centroid.head<3>() - centroid_new.head<3>();
                                           
                pcl::transformPointCloud(*outCloud, *outCloud, transform);


Thanks
Reply | Threaded
Open this post in threaded view
|

Re: Rotate point cloud around it's origin?

VictorLamoine
Administrator
It's already quite good, here is what I use:

  transform.linear() = (Eigen::Matrix3f) Eigen::AngleAxisf(DEG2RAD(25), Eigen::Vector3f::UnitX())
                                                     * Eigen::AngleAxisf(DEG2RAD(45), Eigen::Vector3f::UnitY())
                                                     * Eigen::AngleAxisf(DEG2RAD(-60), Eigen::Vector3f::UnitZ());
Be careful, if you invert X, Y, Z lines, the result is not the same because rotations are NOT commutative!
Reply | Threaded
Open this post in threaded view
|

Re: Rotate point cloud around it's origin?

john24
Once again big thank you for your help:)