Geek Answers Handbook

Rotate and transform the plane into the XY plane and the origin in PointCloud

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

  • move the floor_plane to XY so that the floor_plane lies on the XY plane
  • move the center of gravity of floor_plane to the beginning (0,0,0) .

My approach to the above:

  • Get floor_plane coefficients from RANSAC. The first three coefficients correspond to the floor_plane normal.
  • We generate a normal vector for the XY plane. This will be x = 0, y = 0 and z = 1.
  • Calculate the transverse product of the normal to the ground plane and the normal to the xy plane to get the rotation vector (axis), which is a unit vector.
  • Calculate the angle of rotation. The angle between the planes is equal to the angle between the normals. From the definition of a point product, we can extract the angle between two normal vectors. In the case of the XY plane, it is equal to theta=acos(C/sqrt(A^2+B^2+C^2) , where A, B, C are the first three floor_plane coefficients.
  • Create a rotation matrix (3x3) or a quaternion. Look for the formula on Wikipedia.
  • Find the center of gravity of floor_plane . Cancel them to create a translation.
  • Transforming is easy with transformPointCloud (cloud, transform, transformMatrix)

My code using the PointCloud library is as follows. He cannot complete the necessary transformation, and I'm 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);
+5
source share
2 answers

Make a translation first, then make a turn.

Check out theta p> sign

 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));
+2
source

If anyone is interested

There were 2 problems in the code.

  • The rotation vector should be normalized (just call rotation_vector.normalized ())
  • the corner should be canceled (as suggested in the previous answer).

Thank you for submitting the code, it helped me finish quickly.

+3
source

More articles:


All Articles