diff --git a/.gitignore b/.gitignore index 7db5920..05cadb2 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ +.idea/ /build/ /cmake-build-debug/ diff --git a/pcl_wrapper.cpp b/pcl_wrapper.cpp index 7f5883a..80623dd 100644 --- a/pcl_wrapper.cpp +++ b/pcl_wrapper.cpp @@ -1,24 +1,65 @@ -// pcl_wrapper.cpp #include #include #include +using PointType = pcl::PointXYZ; + +class PclPointCloud { +public: + pcl::PointCloud::Ptr cloud; + + PclPointCloud() : cloud(new pcl::PointCloud) {} +}; + +class PclVoxelGrid { +public: + pcl::VoxelGrid filter; +}; + extern "C" { -int filter_pcd(const char* input_path, const char* output_path, float leaf_size) { - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr cloud_filtered (new pcl::PointCloud); - - if (pcl::io::loadPCDFile(input_path, *cloud) == -1) - return -1; - - pcl::VoxelGrid sor; - sor.setInputCloud(cloud); - sor.setLeafSize(leaf_size, leaf_size, leaf_size); - sor.filter(*cloud_filtered); - - pcl::io::savePCDFile(output_path, *cloud_filtered); - return 0; +void* create_point_cloud() { + return new PclPointCloud(); } -} \ No newline at end of file +void delete_point_cloud(void* ptr) { + delete static_cast(ptr); +} + +int load_point_cloud(void* ptr, const char* path) { + auto* cloud = static_cast(ptr); + return pcl::io::loadPCDFile(path, *cloud->cloud); +} + +int save_point_cloud(void* ptr, const char* path) { + auto* cloud = static_cast(ptr); + return pcl::io::savePCDFileBinary(path, *cloud->cloud); +} + +void* create_voxel_filter() { + return new PclVoxelGrid(); +} + +void delete_voxel_filter(void* ptr) { + delete static_cast(ptr); +} + +void set_voxel_leaf_size(void* ptr, float lx, float ly, float lz) { + auto* vf = static_cast(ptr); + vf->filter.setLeafSize(lx, ly, lz); +} + +void set_voxel_input_cloud(void* vf_ptr, void* cloud_ptr) { + auto* vf = static_cast(vf_ptr); + auto* cloud = static_cast(cloud_ptr); + vf->filter.setInputCloud(cloud->cloud); +} + +void* apply_voxel_filter(void* vf_ptr) { + auto* vf = static_cast(vf_ptr); + auto* result = new PclPointCloud(); + vf->filter.filter(*result->cloud); + return result; +} + +}