点云置平:基于点云法向量

将点云置平(法向量),方便做一些其他处理,与XOY平面平行

normal为法向量,z_normal为置平后的法向量,通过这两个向量计算转换矩阵

Eigen::Affine3f transform_Plane_To_XOYPlane(CP& cloudIn, vector<float> normal_vec) {
    pcl::console::TicToc time; time.tic();
    int cloud_size = cloudIn->size();
    CP cloudAfter(new CloudT);

    Vector3f normal(normal_vec[0], normal_vec[1], normal_vec[2]);
    Vector3f z_normal = Vector3f::UnitZ();                    //(0.0, 0.0, 1.0);


    Vector3f rotation = (z_normal.cross(normal)).normalize();
    float theta = -acos(z_normal.dot(normal));
    Eigen::Affine3f transformXOY = Eigen::Affine3f::Identity();
    transformXOY.rotate(Eigen::AngleAxisf(theta, rotation));

    pcl::transformPointCloud(*cloudIn, *cloudAfter, transformXOY);
    cout << "旋转时间:" << time.toc() << "ms" << endl;

    cloudIn = cloudAfter;
    return transformXOY;
}