65 lines
1.6 KiB
C++
65 lines
1.6 KiB
C++
|
|
// pcl_wrapper/src/wrapper.cpp
|
|
#include "filter.hpp"
|
|
#include "point_types.hpp"
|
|
#include "pcl_wrapper_api.hpp"
|
|
#include <vector>
|
|
#include <cstring>
|
|
|
|
using namespace pcl_wrapper;
|
|
|
|
using CloudPtr = pcl::PointCloud<PointXYZ>::Ptr;
|
|
|
|
class VoxelGridXYZBridge {
|
|
public:
|
|
VoxelGridXYZBridge(float leaf) {
|
|
filter.setLeafSize(leaf);
|
|
}
|
|
|
|
void setInput(const float* points, int count) {
|
|
CloudPtr cloud(new pcl::PointCloud<PointXYZ>());
|
|
cloud->resize(count);
|
|
for (int i = 0; i < count; ++i) {
|
|
(*cloud)[i].x = points[3 * i];
|
|
(*cloud)[i].y = points[3 * i + 1];
|
|
(*cloud)[i].z = points[3 * i + 2];
|
|
}
|
|
filter.setInputCloud(cloud);
|
|
}
|
|
|
|
void getFiltered(float** outPts, int* outCount) {
|
|
auto cloud = filter.getFilteredCloud();
|
|
*outCount = cloud->size();
|
|
*outPts = new float[*outCount * 3];
|
|
for (int i = 0; i < *outCount; ++i) {
|
|
(*outPts)[3 * i + 0] = (*cloud)[i].x;
|
|
(*outPts)[3 * i + 1] = (*cloud)[i].y;
|
|
(*outPts)[3 * i + 2] = (*cloud)[i].z;
|
|
}
|
|
}
|
|
|
|
private:
|
|
VoxelGridFilterImpl<PointXYZ> filter;
|
|
};
|
|
|
|
|
|
extern "C" {
|
|
|
|
void* CreateVoxelGridXYZ(float leaf) {
|
|
return new VoxelGridXYZBridge(leaf);
|
|
}
|
|
|
|
void SetInputCloudXYZ(void* obj, const float* points, int count) {
|
|
static_cast<VoxelGridXYZBridge*>(obj)->setInput(points, count);
|
|
}
|
|
|
|
void ApplyVoxelGridXYZ(void* obj, float** outPoints, int* outCount) {
|
|
static_cast<VoxelGridXYZBridge*>(obj)->getFiltered(outPoints, outCount);
|
|
}
|
|
|
|
void FreeVoxelGridXYZ(void* obj) {
|
|
delete static_cast<VoxelGridXYZBridge*>(obj);
|
|
}
|
|
|
|
} // extern C
|