三维空间圆形式如下:
三维空间圆的参数方程:
PCL绘制3D圆轨迹c++代码:
//center 为圆心,normal 为圆法向量, Radius为半径
pcl::visualization::PCLVisualizer::Ptr Plot3DCircle(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud,
pcl::PointXYZ center,
pcl::Normal normal,
float Radius)
{
float cx = center.x, cy = center.y, cz = center.z;
// float nx = normal.normal_x, ny = normal.normal_y, nz = normal.normal_z;
float r = Radius;
Eigen::Vector3f i(1.0, 0.0, 0.0);
Eigen::Vector3f j(0.0, 1.0, 0.0);
Eigen::Vector3f k(0.0, 0.0, 1.0);
Eigen::Vector3f n(normal.normal_x, normal.normal_y, normal.normal_z);
Eigen::Vector3f a(0.0, 0.0, 0.0);
Eigen::Vector3f b(0.0, 0.0, 0.0);
// 求向量a
a = n.cross(i);
if(a.norm() == 0.0)
{
a = n.cross(j);
}
if(a.norm() == 0.0)
{
a = n.cross(k);
}
// 求向量 b
b = n.cross(a);
// 归一化a,b(圆面两个互垂直向量)
a.normalize();
b.normalize();
//利用空间圆的参数方程生成圆
float xi, yi, zi;
float t = -180.0;
float angle = (t / 180.0)*PI;
std::vector<float> x, y, z;
while (-180.0 <= t & t <= 180.0)
{
xi = cx + r*(a[0]*cos(angle) + b[0]*sin(angle));
yi = cy + r*(a[1]*cos(angle) + b[1]*sin(angle));
zi = cz + r*(a[2]*cos(angle) + b[2]*sin(angle));
x.push_back(xi);
y.push_back(yi);
z.push_back(zi);
t = t + 1;
angle = (t / 180.0)*PI;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr CirclePointsCloud(new pcl::PointCloud<pcl::PointXYZ>);
//定义cloudPoints大小,无序点云
CirclePointsCloud->resize(x.size());
for (int i = 0; i < x.size(); i++){
//将三维坐标赋值给PCL点云坐标
(*CirclePointsCloud)[i].x = x[i];
(*CirclePointsCloud)[i].y = y[i];
(*CirclePointsCloud)[i].z = z[i];
}
//圆心点云设置
//设置点云颜色
pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer->setBackgroundColor(0, 0, 0.5);
viewer->addCoordinateSystem(0.1);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> CirclePointsCloud_color(CirclePointsCloud, 255, 0, 0);
//点云颜色渲染
viewer->addPointCloud(CirclePointsCloud, CirclePointsCloud_color, "CircleCloud");
//设置点云大小
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "CircleCloud");
// viewer->resetCamera();
viewer->initCameraParameters();
return (viewer);
}