已知圆的坐标(x,y),和半径,画圆,以下代码为画半径不同的同心圆,其中需要确定二维点云的法向量,可以自己设置,也可根据已有的平面点云的法向量设置,本代码为平面已有的点云法向量
// 构造三维圆点云,cloud为生成的点云,R为圆的半径
bool Imgdeal::Generate3DCircle(pcl_ptr &circle3DCloud, pcl::PointXYZ &circleCenter, pcl_nml &cloud_normals, double R)
{
Eigen::Vector3f c = circleCenter.getVector3fMap();
float cx = c[0], cy = c[1], cz = c[2]; // 圆-中心
float r = R; // 圆-半径
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);
double nm_x = cloud_normals->points[0].normal_x;
double nm_y = cloud_normals->points[0].normal_y;
double nm_z = cloud_normals->points[0].normal_z;
Eigen::Vector3f n(nm_x, nm_y, nm_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 x1i, y1i, z1i;
float x2i, y2i, z2i;
float x3i, y3i, z3i;
float x4i, y4i, z4i;
float x5i, y5i, z5i;
float x6i, y6i, z6i;
float x7i, y7i, z7i;
float x8i, y8i, z8i;
float t = -180.0;
float angle = pcl::deg2rad(t);
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));
x1i = cx + r/1.5*(a[0]*cos(angle) + b[0]*sin(angle));
y1i = cy + r/1.5*(a[1]*cos(angle) + b[1]*sin(angle));
z1i = cz + r/1.5*(a[2]*cos(angle) + b[2]*sin(angle));
x2i = cx + r/2*(a[0]*cos(angle) + b[0]*sin(angle));
y2i = cy + r/2*(a[1]*cos(angle) + b[1]*sin(angle));
z2i = cz + r/2*(a[2]*cos(angle) + b[2]*sin(angle));
x3i = cx + r/2.5*(a[0]*cos(angle) + b[0]*sin(angle));
y3i = cy + r/2.5*(a[1]*cos(angle) + b[1]*sin(angle));
z3i = cz + r/2.5*(a[2]*cos(angle) + b[2]*sin(angle));
x4i = cx + r/3*(a[0]*cos(angle) + b[0]*sin(angle));
y4i = cy + r/3*(a[1]*cos(angle) + b[1]*sin(angle));
z4i = cz + r/3*(a[2]*cos(angle) + b[2]*sin(angle));
x5i = cx + r/4*(a[0]*cos(angle) + b[0]*sin(angle));
y5i = cy + r/4*(a[1]*cos(angle) + b[1]*sin(angle));
z5i = cz + r/4*(a[2]*cos(angle) + b[2]*sin(angle));
x6i = cx + r/5*(a[0]*cos(angle) + b[0]*sin(angle));
y6i = cy + r/5*(a[1]*cos(angle) + b[1]*sin(angle));
z6i = cz + r/5*(a[2]*cos(angle) + b[2]*sin(angle));
x7i = cx + r/6*(a[0]*cos(angle) + b[0]*sin(angle));
y7i = cy + r/6*(a[1]*cos(angle) + b[1]*sin(angle));
z7i = cz + r/6*(a[2]*cos(angle) + b[2]*sin(angle));
x8i = cx + r/7*(a[0]*cos(angle) + b[0]*sin(angle));
y8i = cy + r/7*(a[1]*cos(angle) + b[1]*sin(angle));
z8i = cz + r/7*(a[2]*cos(angle) + b[2]*sin(angle));
x.push_back(xi);
y.push_back(yi);
z.push_back(zi);
x.push_back(x1i);
y.push_back(y1i);
z.push_back(z1i);
x.push_back(x2i);
y.push_back(y2i);
z.push_back(z2i);
x.push_back(x3i);
y.push_back(y3i);
z.push_back(z3i);
x.push_back(x4i);
y.push_back(y4i);
z.push_back(z4i);
x.push_back(x5i);
y.push_back(y5i);
z.push_back(z5i);
x.push_back(x6i);
y.push_back(y6i);
z.push_back(z6i);
x.push_back(x7i);
y.push_back(y7i);
z.push_back(z7i);
x.push_back(x8i);
y.push_back(y8i);
z.push_back(z8i);
t = t + 1;
angle = pcl::deg2rad(t);
}
// for(int i=1; i++; i<8)
// {
// while (-180.0 <= t & t <= 180.0)
// {
// xi = cx + (r/i)*(a[0]*cos(angle) + b[0]*sin(angle));
// yi = cy + (r/i)*(a[1]*cos(angle) + b[1]*sin(angle));
// zi = cz + (r/i)*(a[2]*cos(angle) + b[2]*sin(angle));
// x.push_back(xi);
// y.push_back(yi);
// z.push_back(zi);
// t = t + 1;
// angle = pcl::deg2rad(t);
// }
// } //算法时间慢
//定义cloudPoints大小,无序点云
circle3DCloud->resize(x.size());
for (int i = 0; i < x.size(); i++){
//将三维坐标赋值给PCL点云坐标
(*circle3DCloud)[i].x = x[i];
(*circle3DCloud)[i].y = y[i];
(*circle3DCloud)[i].z = z[i];
}
// writer.write("../PointsCloud/circle3DCloud.pcd", *circle3DCloud);
return true;
}
使用方法为:
pcl::PointXYZ lylord_Center{(float)x, (float)y, 2.0};
Generate3DCircle(generate_cloud, lylord_Center, cloud_normals, lylord_devlate/(2+number));
自己设置的方法为:
pcl::Normal N{0.5,0.5,1.0};
PCL::PointXYZ center{0.0,0.0,0.0};