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

i have point cloud known contain floor. oriented in unknown direction , not @ origin (0,0,0). have to

• move floor_plane xy plane, floor_plane lies on xy plane
• move centroid of floor_plane origin (0,0,0).

my approach above is:

• get plane coefficients of floor_plane ransac. first 3 coefficients correspond floor_plane's normal.
• generate normal vector xy plane. x=0,y=0 , z=1.
• calculate cross product of normal of ground plane , normal of xy plane rotation vector (axis) unit vector.
• calculate rotation angle. angle between planes equal angle between normals. definition of dot product, can extract angle between 2 normal vectors. in case of xy plane, equal theta=acos(c/sqrt(a^2+b^2+c^2) a, b, c first 3 coefficients of floor_plane.
• generate rotation matrix (3x3) or quaternion. formula in wikipedia.
• find centroid of floor_plane. negate them generate translation
• apply transformation transformpointcloud(cloud,transformed,transformationmatrix)

my code using pointcloud library goes follows. unable perform required transformation, , not sure why. clues?

      // find planar coefficients 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); 

in case interested

there 2 problems in code

1. rotation vector need normalized (just call rotation_vector.normalized())
2. angle need negated (as suggested previous answer).

thank posting code, helped me finish quickly.