void create(int query_number, int max_elements)
{
max_elems = max_elements;
data.create (query_number * max_elems);
if (max_elems != 1)
sizes.create(query_number);
}
void
pcl::gpu::TsdfVolume::fetchNormals (const DeviceArray<PointType>& cloud, DeviceArray<NormalType>& normals) const
{
normals.create (cloud.size ());
const float3 device_volume_size = device_cast<const float3> (size_);
device::extractNormals (volume_, device_volume_size, cloud, (device::float8*)normals.ptr ());
}
DeviceArray<Point>
kfusion::cuda::TsdfVolume::fetchSliceAsCloud (DeviceArray<Point>& cloud_buffer, const kfusion::tsdf_buffer* buffer, const Vec3i minBounds, const Vec3i maxBounds, const Vec3i globalShift ) const
{
enum { DEFAULT_CLOUD_BUFFER_SIZE = 10 * 1000 * 1000 };
if (cloud_buffer.empty ())
cloud_buffer.create (DEFAULT_CLOUD_BUFFER_SIZE/2);
DeviceArray<device::Point>& b = (DeviceArray<device::Point>&)cloud_buffer;
device::Vec3i dims = device_cast<device::Vec3i>(dims_);
device::Vec3i deviceGlobalShift;
deviceGlobalShift.x = globalShift[0];
deviceGlobalShift.y = globalShift[1];
deviceGlobalShift.z = globalShift[2];
device::Vec3i minBounds_c;
minBounds_c.x = minBounds[0];
minBounds_c.y = minBounds[1];
minBounds_c.z = minBounds[2];
device::Vec3i maxBounds_c;
maxBounds_c.x = maxBounds[0];
maxBounds_c.y = maxBounds[1];
maxBounds_c.z = maxBounds[2];
device::Vec3f vsz = device_cast<device::Vec3f>(getVoxelSize());
device::Aff3f aff = device_cast<device::Aff3f>(pose_);
device::TsdfVolume volume((ushort2*)data_.ptr<ushort2>(), dims, vsz, trunc_dist_, max_weight_);
size_t size = extractSliceAsCloud (volume, buffer, minBounds_c, maxBounds_c, deviceGlobalShift, aff, b);
return DeviceArray<Point>((Point*)cloud_buffer.ptr(), size);
}
DeviceArray<pcl::gpu::MarchingCubes::PointType>
pcl::gpu::MarchingCubes::run(const TsdfVolume& tsdf, DeviceArray<PointType>& triangles_buffer)
{
if (triangles_buffer.empty())
triangles_buffer.create(DEFAULT_TRIANGLES_BUFFER_SIZE);
occupied_voxels_buffer_.create(3, triangles_buffer.size() / 3);
device::bindTextures(edgeTable_, triTable_, numVertsTable_);
int active_voxels = device::getOccupiedVoxels(tsdf.data(), occupied_voxels_buffer_);
if(!active_voxels)
{
device::unbindTextures();
return DeviceArray<PointType>();
}
DeviceArray2D<int> occupied_voxels(3, active_voxels, occupied_voxels_buffer_.ptr(), occupied_voxels_buffer_.step());
int total_vertexes = device::computeOffsetsAndTotalVertexes(occupied_voxels);
float3 volume_size = device_cast<const float3>(tsdf.getSize());
device::generateTriangles(tsdf.data(), occupied_voxels, volume_size, (DeviceArray<device::PointType>&)triangles_buffer);
device::unbindTextures();
return DeviceArray<PointType>(triangles_buffer.ptr(), total_vertexes);
}
pcl::gpu::DeviceArray<pcl::gpu::TsdfVolume::PointType>
pcl::gpu::TsdfVolume::fetchCloud (DeviceArray<PointType>& cloud_buffer) const
{
if (cloud_buffer.empty ())
cloud_buffer.create (DEFAULT_CLOUD_BUFFER_SIZE);
float3 device_volume_size = device_cast<const float3> (size_);
size_t size = device::extractCloud (volume_, device_volume_size, cloud_buffer);
return (DeviceArray<PointType> (cloud_buffer.ptr (), size));
}
PCL_EXPORTS void
mergePointNormal(const DeviceArray<PointXYZ>& cloud, const DeviceArray<Normal>& normals, DeviceArray<PointNormal>& output)
{
const size_t size = min(cloud.size(), normals.size());
output.create(size);
const DeviceArray<float4>& c = (const DeviceArray<float4>&)cloud;
const DeviceArray<float8>& n = (const DeviceArray<float8>&)normals;
const DeviceArray<float12>& o = (const DeviceArray<float12>&)output;
device::mergePointNormal(c, n, o);
}
void kfusion::cuda::TsdfVolume::fetchNormals(const DeviceArray<Point>& cloud, const tsdf_buffer& buffer, DeviceArray<Normal>& normals) const
{
normals.create(cloud.size());
DeviceArray<device::Point>& c = (DeviceArray<device::Point>&)cloud;
device::Vec3i dims = device_cast<device::Vec3i>(dims_);
device::Vec3f vsz = device_cast<device::Vec3f>(getVoxelSize());
device::Aff3f aff = device_cast<device::Aff3f>(pose_);
device::Mat3f Rinv = device_cast<device::Mat3f>(pose_.rotation().inv(cv::DECOMP_SVD));
device::TsdfVolume volume((ushort2*)data_.ptr<ushort2>(), dims, vsz, trunc_dist_, max_weight_);
device::extractNormals(volume, buffer, c, aff, Rinv, gradient_delta_factor_, (float4*)normals.ptr());
}
DeviceArray<Point> kfusion::cuda::TsdfVolume::fetchCloud(DeviceArray<Point>& cloud_buffer, const tsdf_buffer& buffer) const
{
enum { DEFAULT_CLOUD_BUFFER_SIZE = 10 * 1000 * 1000 };
if (cloud_buffer.empty ())
cloud_buffer.create (DEFAULT_CLOUD_BUFFER_SIZE);
DeviceArray<device::Point>& b = (DeviceArray<device::Point>&)cloud_buffer;
device::Vec3i dims = device_cast<device::Vec3i>(dims_);
device::Vec3f vsz = device_cast<device::Vec3f>(getVoxelSize());
device::Aff3f aff = device_cast<device::Aff3f>(pose_);
device::TsdfVolume volume((ushort2*)data_.ptr<ushort2>(), dims, vsz, trunc_dist_, max_weight_);
size_t size = extractCloud(volume, buffer, aff, b);
return DeviceArray<Point>((Point*)cloud_buffer.ptr(), size);
}
void copyFieldsEx(const DeviceArray<PointIn>& src, DeviceArray<PointOut>& dst, int rule1, int rule2 = NoCP, int rule3 = NoCP, int rule4 = NoCP)
{
int rules[4] = { rule1, rule2, rule3, rule4 };
dst.create(src.size());
copyFieldsImpl(sizeof(PointIn)/sizeof(int), sizeof(PointOut)/sizeof(int), rules, (int)src.size(), src.ptr(), dst.ptr());
}