boost::shared_ptr<pcl::PolygonMesh> convertToMesh(const DeviceArray<PointXYZ>& triangles)
{
if (triangles.empty())
return boost::shared_ptr<pcl::PolygonMesh>();
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.width = (int)triangles.size();
cloud.height = 1;
triangles.download(cloud.points);
boost::shared_ptr<pcl::PolygonMesh> mesh_ptr( new pcl::PolygonMesh() );
pcl::toPCLPointCloud2(cloud, mesh_ptr->cloud);
mesh_ptr->polygons.resize (triangles.size() / 3);
for (size_t i = 0; i < mesh_ptr->polygons.size (); ++i)
{
pcl::Vertices v;
v.vertices.push_back(i*3+0);
v.vertices.push_back(i*3+2);
v.vertices.push_back(i*3+1);
mesh_ptr->polygons[i] = v;
}
return mesh_ptr;
cout << mesh_ptr->polygons.size () << " plys\n";
}
boost::shared_ptr<pcl::PolygonMesh> convertToMesh(const DeviceArray<PointXYZ>& triangles)
{
if (triangles.empty())
{
std::cerr << "kinfu_util::convertToMesh(): triangles empty...returning null..." << std::endl;
return boost::shared_ptr<pcl::PolygonMesh>();
}
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.width = (int)triangles.size();
cloud.height = 1;
triangles.download(cloud.points);
boost::shared_ptr<pcl::PolygonMesh> mesh_ptr( new pcl::PolygonMesh() );
pcl::toROSMsg(cloud, mesh_ptr->cloud);
mesh_ptr->polygons.resize (triangles.size() / 3);
for (size_t i = 0; i < mesh_ptr->polygons.size (); ++i)
{
pcl::Vertices v;
v.vertices.push_back(i*3+0);
v.vertices.push_back(i*3+2);
v.vertices.push_back(i*3+1);
mesh_ptr->polygons[i] = v;
}
return mesh_ptr;
}
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
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<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);
}
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());
}
void SceneCloudView::generateCloud(KinfuTracker& kinfu, bool integrate_colors)
{
viewer_pose_ = kinfu.getCameraPose();
ScopeTimeT time ("PointCloud Extraction");
cout << "\nGetting cloud... " << flush;
valid_combined_ = false;
bool valid_extracted_ = false;
if (extraction_mode_ != GPU_Connected6) // So use CPU
{
kinfu.volume().fetchCloudHost (*cloud_ptr_, extraction_mode_ == CPU_Connected26);
}
else
{
DeviceArray<PointXYZ> extracted = kinfu.volume().fetchCloud (cloud_buffer_device_);
if(extracted.size() > 0){
valid_extracted_ = true;
extracted.download (cloud_ptr_->points);
cloud_ptr_->width = (int)cloud_ptr_->points.size ();
cloud_ptr_->height = 1;
if (integrate_colors)
{
kinfu.colorVolume().fetchColors(extracted, point_colors_device_);
point_colors_device_.download(point_colors_ptr_->points);
point_colors_ptr_->width = (int)point_colors_ptr_->points.size ();
point_colors_ptr_->height = 1;
//pcl::gpu::mergePointRGB(extracted, point_colors_device_, combined_color_device_);
//combined_color_device_.download (combined_color_ptr_->points);
}
else
point_colors_ptr_->points.clear();
combined_color_ptr_->clear();
generateXYZRGB(cloud_ptr_, point_colors_ptr_, combined_color_ptr_);
}else{
valid_extracted_ = false;
cout << "Failed to Extract Cloud " << endl;
}
}
cout << "Done. Cloud size: " << cloud_ptr_->points.size () / 1000 << "K" << endl;
}
size_t neighboors_size() const { return data.size()/max_elems; }
bool validate(size_t cloud_size) const
{
return (sizes.size() == cloud_size) && (cloud_size * max_elems == data.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());
}
