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

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