空间平面旋转与xoy平行

法向量

空间平面ax+by+cz+d=0的其中一个法向量(a,b,c),法向量垂直于空间平面。目标平面平行于xoy的平面为0x+0y+cz+d=0;其中一个法向量为(0,0,c),c可以为不为0的任意值，取(0,0,1)，目标平面的的法向量垂直于xoy平面

向量叉乘点乘

两个向量的点乘叉乘的区别
点乘计算两向量的投影关系并返回标量,反映两向量方向相似性
叉乘则生成垂直于原向量平面的新向量并反映空间结构关系
点乘获取旋转角度
叉乘获取旋转轴

// 平面生成
void generatePlanePointCloud(float a, float b, float c, float d,pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud) 
{for (int i = 0; i < 10000; i++){double x = rand() % 500;double y = rand() % 500;double z = (a * x + b * y + d) / (0 - c);//假设平面方程类型 ax+by+cz+d=0Eigen::Vector3f point(x, y, z);cloud->points.emplace_back(point.x(), point.y(), point.z());}
}
int main() 
{pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());// 定义平面 ax + by + cz + d = 0 的参数float a = -2.0f, b = -9.0f, c = 8.0f, d = 100.0f;// 调用函数生成平面点云generatePlanePointCloud(a, b, c, d, cloud);//点云几何中心 可平移点云到坐标原点Eigen::Vector4f centroid;					pcl::compute3DCentroid(*cloud, centroid);Eigen::Matrix4f transform_matrix = Eigen::Matrix4f::Identity();Eigen::Vector3f translationxyz(-centroid(0), -centroid(1), -centroid(2));transform_matrix.block<3, 1>(0, 3) = translationxyz;pcl::PointCloud<pcl::PointXYZ>::Ptr ocloud(new pcl::PointCloud<pcl::PointXYZ>);pcl::transformPointCloud(*cloud, *ocloud, transform_matrix);// 平面法向量和目标平面法向量Eigen::Vector3f v1(a, b, c), v2(0, 0, 1);// 点乘获取旋转夹角float RotateRad = pcl::getAngle3D(v1, v2);// 叉乘获取旋转轴 旋转轴需要单位化Eigen::Vector3f RotateAxis = v1.cross(v2).normalized();// 点乘获取旋转夹角float RotateRad1 = RotateRad;float angle = acos(v1.normalized().dot(v2.normalized()));// 生成放射变换单位矩阵4x4 Affine3f不是matrix但是有matrix函数Eigen::Affine3f rotation = Eigen::Affine3f::Identity();// 将旋转轴和旋转角添加到仿射矩阵rotation.rotate(Eigen::AngleAxisf(RotateRad1, RotateAxis));// 生成单位旋转矩阵3x3Eigen::Matrix3f rotation_matrix = Eigen::Matrix3f::Identity();// 生成旋转向量Eigen::AngleAxisf rotation_vector(RotateRad, RotateAxis); // 注意：旋转轴必须为单位向量rotation_matrix = rotation_vector.toRotationMatrix();// 使用罗德里格斯公式得到旋转矩阵// 初始化平移向量（此处为0）Eigen::Vector3f translation(0, 0, 0);// 创建4x4变换矩阵Eigen::Matrix4f transform_matrix1 = Eigen::Matrix4f::Identity();transform_matrix1.block<3, 3>(0, 0) = rotation_matrix; // 设置旋转部分transform_matrix1.block<3, 1>(0, 3) = translation;     // 设置平移部分pcl::PointCloud<pcl::PointXYZ>::Ptr xcloud(new pcl::PointCloud<pcl::PointXYZ>);// 平面点云进行仿射变换pcl::transformPointCloud(*ocloud, *xcloud, transform_matrix1);pcl::PointCloud<pcl::PointXYZ>::Ptr ycloud(new pcl::PointCloud<pcl::PointXYZ>);// 平面点云进行仿射变换pcl::transformPointCloud(*cloud, *ycloud, rotation);boost::shared_ptr<pcl::visualization::PCLVisualizer> view(new pcl::visualization::PCLVisualizer("3D Viewer"));pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(xcloud, 0, 255, 0);//定义颜色 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h1(ycloud, 0, 0, 255);//定义颜色 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h2(ocloud, 0, 255, 255);//定义颜色 view->addCoordinateSystem(500.0,0,0,0); // 添加坐标轴，大小为500.0view->addPointCloud<pcl::PointXYZ>(cloud, "cloud_in");view->addPointCloud<pcl::PointXYZ>(xcloud, src_h, "clound_xuanzhuan");view->addPointCloud<pcl::PointXYZ>(ycloud, src_h1, "xuanzhuan1");//view->addPointCloud<pcl::PointXYZ>(ocloud, src_h2, "xuanzhuan2");view->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "cloud_in");while (!view->wasStopped()){view->spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(100000));}return 0;
}