空间平面旋转与xoy平行

空间平面旋转与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=0
		Eigen::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));
	
	// 生成单位旋转矩阵3x3
	Eigen::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.0
	view->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;
}