我的目标是将我的点云(源自TLS)划分为体素,其中用户必须定义体素的边长。一旦创建了体素,我必须仅选择位于体素中的满足特定条件的一个点。到目前为止,我编写了这个目标,但我遇到了一个小问题。在点云的某些部分,点密度变得更高,如下图所示(用红色多边形标记的密集区域):
我认为这是Z方向上垂直于XY平面的相邻体素的结果。你能帮我解决这个问题吗?
这是我的代码:
int main(int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
//pcd'ye dönüştürülen dosyanın okunması
pcl::io::loadPCDFile<pcl::PointXYZ>("input/yeni_proje_V2_ENTIRE_CLOUD_with_SEMI_DIAGONAL - Cloud.pcd", *cloud);
//voxel boyutu (metre)
float resolution = 0.02f;
//voxel index merkez ve içindeki noktalar
FILE *Dosya = fopen("output/yeni_proje_V2_ENTIRE_VOXELS_INDEXES_2cm_SEMI_DIAGONAL_ROTATED.xyz", "w+");
//hata elipsoidlerini içeren dosya
FILE* Dosya2 = fopen("input/yeni_proje_V2_ENTIRE_CLOUD_with_SEMI_DIAGONAL - ROTATED.xyz", "r");
//elipsoidi en küçük olan noktalar
FILE *Dosya3 = fopen("output/yeni_proje_V2_2cm_selected_ONLY_COORDS_SEMI_DIAGONAL_ROTATED.xyz", "w+");
FILE *Dosya4 = fopen("output/yeni_proje_V2_2cm_selected_SEMI_DIAGONAL_ROTATED.xyz", "w+");
//FILE *Dosya5 = fopen("input/FARO_salon010203_COORDINATES_NORMALS.xyz", "r");
//FILE *Dosya6 = fopen("output/FARO_salon010203_5cm_selected_COORDINATES_and_NORMALS_SEMI_DIAGONAL.xyz", "w+");
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
int index;
double a, b, c, deer;
double* r_deer = new double[cloud->points.size()];// hata parametri çekme
double* normal_x = new double[cloud->points.size()];
double* normal_y = new double[cloud->points.size()];
double* normal_z = new double[cloud->points.size()];
for (index = 0; index < cloud->points.size(); index++)// hata parametri çekme
{
fscanf(Dosya2, "%lf %lf %lf %lf", &a, &b, &c, &deer);//4 lü dosya
r_deer[index] = deer;
//fscanf(Dosya5, "%lf %lf %lf %lf %lf %lf", &a, &b, &c, &normal_x[index], &normal_y[index],&normal_z[index]);
}
std::cout << normal_x[4] <<" "<<normal_y[4]<<" "<< normal_z[4] << std::endl;
octree.setInputCloud(cloud);
octree.addPointsFromInputCloud();
pcl::PointXYZ searchPoint;
std::cout << "Voxel sayisi: " << octree.getLeafCount() << std::endl;
// Neighbors within voxel search
std::vector<pcl::PointXYZ, Eigen::aligned_allocator<pcl::PointXYZ>> pointGrid;
octree.getOccupiedVoxelCenters(pointGrid);
int k = 0;
int kıyas = 0;
/*
int kontrol;
std::cout << "kontrol satiri girin:" << std::endl;
std::cin >> kontrol;//kontrol
kontrol = kontrol - 1;
*/
for (k = 0; k < octree.getLeafCount(); k++)
{
// if (k == kontrol) std::cout << "Secili merkez" << "(" << pointGrid[k].x << " " << pointGrid[k].y << " " << pointGrid[k].z << ")" << std::endl;//kontrol
fprintf(Dosya, "%i %f %f %f", k + 1, pointGrid[k].x, pointGrid[k].y, pointGrid[k].z);
std::vector<int> pointIdxVec;
double limit = sqrt(3)*resolution;
limit = limit / 2;
if (octree.voxelSearch(pointGrid[k], pointIdxVec))
{
kıyas = pointIdxVec[0];
for (size_t i = 0; i < pointIdxVec.size(); ++i)
{
if (pointIdxVec.size() - (i + 1) != 0)//Hata elipsoidlerini kıyasla ve en küçüğü ver
{
if (r_deer[kıyas] > r_deer[pointIdxVec[i + 1]])kıyas = pointIdxVec[i + 1];
}
//if (kontrol == k) std::cout << pointIdxVec[i] + 1 << "(" << r_deer[pointIdxVec[i]] << ")" << std::endl; //kontrol
fprintf(Dosya, " %i", pointIdxVec[i] + 1);
if (pcl::euclideanDistance(cloud->points[pointIdxVec[i]], pointGrid[k]) >= limit)
{
std::cout << pointIdxVec[i] << " " << cloud->points[pointIdxVec[i]] << " Nokta voxelin icinde degil!!! " << std::endl;
std::cout << pcl::euclideanDistance(cloud->points[pointIdxVec[i]], pointGrid[k]) << " Merkezle Mesefe" << std::endl;
std::cout << limit << " Olması gereken maksimum mesafe" << std::endl;
}
}
//if (kontrol == k) std::cout << "Minimum:" << kıyas + 1 << "(" << r_deer[kıyas] << ")" << std::endl; //kontrol
fprintf(Dosya3, "%f %f %f\n", cloud->points[kıyas].x, cloud->points[kıyas].y, cloud->points[kıyas].z);
//fprintf(Dosya6, "%f %f %f %f %f %f\n", cloud->points[kıyas].x, cloud->points[kıyas].y, cloud->points[kıyas].z, normal_x[kıyas], normal_y[kıyas], normal_z[kıyas]);
fprintf(Dosya4, "%f %f %f %f\n", cloud->points[kıyas].x, cloud->points[kıyas].y, cloud->points[kıyas].z,r_deer[kıyas]);
fprintf(Dosya, "\n");
}
}
fclose(Dosya);
fclose(Dosya2);
fclose(Dosya3);
fclose(Dosya4);
//fclose(Dosya5);
//fclose(Dosya6);
}
我期待着您的回复
穆斯塔法
答案 0 :(得分:0)
也许您正在尝试“体素化”您的数据,并且您希望在每个体素中存储自定义数据结构以满足您的自定义要求。这是一个通用的3D Mapping Framework,可让您在Voxel中存储自定义数据:
https://github.com/m4nh/skimap_ros
您必须为您创建一个自定义数据,能够存储一组点,以实现自定义方法:
voxel.getSatisfactoryPoint(..)
隐藏您的业务逻辑,并为每个体素返回一个点。