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);

Judaea

I had the same problem and your step of approach helped me a lot. Thanks. – Halfon

Did you achieve what you wanted with this code? I was unable to do so and later changed it to something else, which worked. – Judaea

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? – Fernandefernandel

Hi @PranavPrakash, what was the other solution you got that worked? Can you share the details ? Thanks! – Alexalexa

@Alexalexa I tried the approach mentioned by otna_ecnav below and it worked, – Judaea

@Judaea could you provide your final corrected code – Unrest

@Unrest - Unfortunately I don't have the source with me now, but I believe I tried the response from otna_ecnav below and it worked. – Judaea

Answer 1

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));

Christianachristiane

Answer 2

In case anyone is interested

There were 2 problems in the code

1. Rotation vector need to be normalized (just call rotation_vector.normalized() in the line transform_2.rotate (Eigen::AngleAxisf (theta, rotation_vector));)
2. angle need to be negated (as suggested by previous answer).

Thank you for posting the code, it helped me to finish this quickly.

Evident