Rotation and Transformation of a plane to XY plane and origin in PointCloud

Question

I have a point cloud that is known to contain the floor. This is oriented in some unknown direction and is not at the origin (0,0,0). I have to

move the floor_plane to XY plane, so that the floor_plane lies on XY plane
move the centroid of floor_plane to the origin (0,0,0).

My approach for the above is:

Get the plane coefficients of floor_plane from RANSAC. First three coefficients correspond to the floor_plane's normal.
Generate the normal vector for XY plane. This would be x=0,y=0 and z=1.
Calculate cross product of normal of ground plane and normal of xy plane to get rotation vector (axis) which is a unit vector.
Calculate the rotation angle. Angle between the planes is equal to angle between the normals. From the definition of the dot product, we can extract the angle between two normal vectors. In case of XY plane, it is equal to theta=acos(C/sqrt(A^2+B^2+C^2) where A, B, C are first three coefficients of floor_plane.
Generate the rotation matrix (3x3) or quaternion. Look for the formula in Wikipedia.
Find the centroid of floor_plane. Negate them to generate the translation
Apply the transformation simply with transformPointCloud(cloud,transformed,transformationMatrix)

My code using PointCloud Library goes as follows. It is unable to perform the required transformation, and I am not sure why. Any clues?

      // Find the planar coefficients for floor plane
      pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
      pcl::PointIndices::Ptr floor_inliers (new pcl::PointIndices);
      pcl::SACSegmentation<pcl::PointXYZRGB> seg;
      seg.setOptimizeCoefficients (true);
      seg.setModelType (pcl::SACMODEL_PLANE);
      seg.setMethodType (pcl::SAC_RANSAC);
      seg.setDistanceThreshold (0.01);
      seg.setInputCloud (floor_plane);
      seg.segment (*floor_inliers, *coefficients);
      std::cerr << "Floor Plane Model coefficients: " << coefficients->values[0] << " "
                                        << coefficients->values[1] << " "
                                        << coefficients->values[2] << " "
                                        << coefficients->values[3] << std::endl;

      Eigen::Matrix<float, 1, 3> floor_plane_normal_vector, xy_plane_normal_vector, rotation_vector;

      floor_plane_normal_vector[0] = coefficients->values[0];
      floor_plane_normal_vector[1] = coefficients->values[1];
      floor_plane_normal_vector[2] = coefficients->values[2];

      std::cout << floor_plane_normal_vector << std::endl;

      xy_plane_normal_vector[0] = 0.0;
      xy_plane_normal_vector[1] = 0.0;
      xy_plane_normal_vector[2] = 1.0;

      std::cout << xy_plane_normal_vector << std::endl;

      rotation_vector = xy_plane_normal_vector.cross (floor_plane_normal_vector);
      std::cout << "Rotation Vector: "<< rotation_vector << std::endl;

      float theta = acos(floor_plane_normal_vector.dot(xy_plane_normal_vector)/sqrt( pow(coefficients->values[0],2)+ pow(coefficients->values[1],2) + pow(coefficients->values[2],2)));


  Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
  transform_2.translation() << 0, 0, 30;
  transform_2.rotate (Eigen::AngleAxisf (theta, rotation_vector));
  std::cout << "Transformation matrix: " << std::endl << transform_2.matrix() << std::endl;
  pcl::transformPointCloud (*cloud, *centeredCloud, transform_2);

I had the same problem and your step of approach helped me a lot. Thanks. — Sunil Singh
Did you achieve what you wanted with this code? I was unable to do so and later changed it to something else, which worked. — pr4n
If I want to get the rotation to another axis (for example y axis) I have to change the xy_plane_normal_vector[1] to 1.0 and xy_plane_normal_vector[2] to 0.0? Do I have to change anything else? — alltooconfusingthereforesleep
Hi @PranavPrakash, what was the other solution you got that worked? Can you share the details ? Thanks! — ManJan
@ManJan I tried the approach mentioned by otna_ecnav below and it worked, — pr4n

Bruzzlee Bruzzlee · Accepted Answer · 2015-09-18T15:12:02

Do the translation first, then do the rotation.

Check the sign of theta

Eigen::Vector3f rotation_vector = xy_plane_normal_vector.cross(floor_plane_normal_vector);
float theta = -atan2(rotation_vector.norm(), xy_plane_normal_vector.dot(floor_plane_normal_vector));

Rotation and Transformation of a plane to XY plane and origin in PointCloud

2 Answers