Нерегулярное облако точек от вокселей

Question

Моя цель - разделить мое облако точек (полученное из 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);

}

Я с нетерпением жду ответа от вас

Мустафа

Answer 1

Может быть, вы пытаетесь "оканчивать" ваши данные, и вы хотите сохранить в каждом вокселе собственную структуру данных, соответствующую вашим требованиям. Вот общая структура 3D Mapping Framework, которая позволяет вам хранить в Voxel ваши пользовательские данные:

https://github.com/m4nh/skimap_ros

Вам нужно создать пользовательские данные для Voxel, которые могут хранить набор точек, реализующих пользовательский метод:

voxel.getSatisfactoryPoint(..)

это скрывает вашу бизнес-логику и возвращает только одно очко за каждый воксель.