空間平面旋轉與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;
}