体素不规则点云

时间:2017-06-09 09:46:50

标签: c++ point-cloud-library point-clouds

我的目标是将我的点云(源自TLS)划分为体素,其中用户必须定义​​体素的边长。一旦创建了体素,我必须仅选择位于体素中的满足特定条件的一个点。到目前为止,我编写了这个目标,但我遇到了一个小问题。在点云的某些部分,点密度变得更高,如下图所示(用红色多边形标记的密集区域):

enter image description here

我认为这是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);

}

我期待着您的回复

穆斯塔法

1 个答案:

答案 0 :(得分:0)

也许您正在尝试“体素化”您的数据,并且您希望在每个体素中存储自定义数据结构以满足您的自定义要求。这是一个通用的3D Mapping Framework,可让您在Voxel中存储自定义数据:

https://github.com/m4nh/skimap_ros

您必须为您创建一个自定义数据,能够存储一组点,以实现自定义方法:

voxel.getSatisfactoryPoint(..)

隐藏您的业务逻辑,并为每个体素返回一个点。