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); }
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"; }
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); }
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; }
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 ()); }
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); }
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()); }
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; }
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 pcl::gpu::people::uploadColorMap(DeviceArray<pcl::RGB>& color_map) { // Copy the list of label colors into the devices vector<pcl::RGB> rgba(LUT_COLOR_LABEL_LENGTH); for(int i = 0; i < LUT_COLOR_LABEL_LENGTH; ++i) { // !!!! generate in RGB format, not BGR rgba[i].r = LUT_COLOR_LABEL[i*3 + 2]; rgba[i].g = LUT_COLOR_LABEL[i*3 + 1]; rgba[i].b = LUT_COLOR_LABEL[i*3 + 0]; rgba[i].a = 255; } color_map.upload(rgba); }
operator PtrStep<int>() const { return PtrStep<int>((int*)data.ptr(), max_elems * sizeof(int)); }
void pcl::gpu::kinfuLS::CyclicalBuffer::performShift (const TsdfVolume::Ptr volume, const pcl::PointXYZ &target_point, const bool last_shift) { // compute new origin and offsets int offset_x, offset_y, offset_z; computeAndSetNewCubeMetricOrigin (target_point, offset_x, offset_y, offset_z); // extract current slice from the TSDF volume (coordinates are in indices! (see fetchSliceAsCloud() ) DeviceArray<PointXYZ> points; DeviceArray<float> intensities; int size; if(!last_shift) { size = volume->fetchSliceAsCloud (cloud_buffer_device_xyz_, cloud_buffer_device_intensities_, &buffer_, offset_x, offset_y, offset_z); } else { size = volume->fetchSliceAsCloud (cloud_buffer_device_xyz_, cloud_buffer_device_intensities_, &buffer_, buffer_.voxels_size.x - 1, buffer_.voxels_size.y - 1, buffer_.voxels_size.z - 1); } points = DeviceArray<PointXYZ> (cloud_buffer_device_xyz_.ptr (), size); intensities = DeviceArray<float> (cloud_buffer_device_intensities_.ptr(), size); PointCloud<PointXYZI>::Ptr current_slice (new PointCloud<PointXYZI>); PointCloud<PointXYZ>::Ptr current_slice_xyz (new PointCloud<PointXYZ>); PointCloud<PointIntensity>::Ptr current_slice_intensities (new PointCloud<PointIntensity>); // Retrieving XYZ points.download (current_slice_xyz->points); current_slice_xyz->width = (int) current_slice_xyz->points.size (); current_slice_xyz->height = 1; // Retrieving intensities // TODO change this mechanism by using PointIntensity directly (in spite of float) // when tried, this lead to wrong intenisty values being extracted by fetchSliceAsCloud () (padding pbls?) std::vector<float , Eigen::aligned_allocator<float> > intensities_vector; intensities.download (intensities_vector); current_slice_intensities->points.resize (current_slice_xyz->points.size ()); for(int i = 0 ; i < current_slice_intensities->points.size () ; ++i) current_slice_intensities->points[i].intensity = intensities_vector[i]; current_slice_intensities->width = (int) current_slice_intensities->points.size (); current_slice_intensities->height = 1; // Concatenating XYZ and Intensities pcl::concatenateFields (*current_slice_xyz, *current_slice_intensities, *current_slice); current_slice->width = (int) current_slice->points.size (); current_slice->height = 1; // transform the slice from local to global coordinates Eigen::Affine3f global_cloud_transformation; global_cloud_transformation.translation ()[0] = buffer_.origin_GRID_global.x; global_cloud_transformation.translation ()[1] = buffer_.origin_GRID_global.y; global_cloud_transformation.translation ()[2] = buffer_.origin_GRID_global.z; global_cloud_transformation.linear () = Eigen::Matrix3f::Identity (); transformPointCloud (*current_slice, *current_slice, global_cloud_transformation); // retrieve existing data from the world model PointCloud<PointXYZI>::Ptr previously_existing_slice (new PointCloud<PointXYZI>); world_model_.getExistingData (buffer_.origin_GRID_global.x, buffer_.origin_GRID_global.y, buffer_.origin_GRID_global.z, offset_x, offset_y, offset_z, buffer_.voxels_size.x - 1, buffer_.voxels_size.y - 1, buffer_.voxels_size.z - 1, *previously_existing_slice); //replace world model data with values extracted from the TSDF buffer slice world_model_.setSliceAsNans (buffer_.origin_GRID_global.x, buffer_.origin_GRID_global.y, buffer_.origin_GRID_global.z, offset_x, offset_y, offset_z, buffer_.voxels_size.x, buffer_.voxels_size.y, buffer_.voxels_size.z); PCL_INFO ("world contains %d points after update\n", world_model_.getWorldSize ()); world_model_.cleanWorldFromNans (); PCL_INFO ("world contains %d points after cleaning\n", world_model_.getWorldSize ()); // clear buffer slice and update the world model pcl::device::kinfuLS::clearTSDFSlice (volume->data (), &buffer_, offset_x, offset_y, offset_z); // insert current slice in the world if it contains any points if (current_slice->points.size () != 0) { world_model_.addSlice(current_slice); } // shift buffer addresses shiftOrigin (volume, offset_x, offset_y, offset_z); // push existing data in the TSDF buffer if (previously_existing_slice->points.size () != 0 ) { volume->pushSlice(previously_existing_slice, getBuffer () ); } }
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()); }
DeviceMatrix<float> RadialBasisFunction::Test(DeviceMatrix<float> &Input,DeviceMatrix<float> &Centers,DeviceMatrix<float> &Weights, DeviceArray<float> &Widths){ DeviceMatrix<float> device_activ_matrix(Input.Columns(),Centers.Rows(),ColumnMajor); KernelActivationMatrix(device_activ_matrix.Pointer(),Input.Pointer(),Centers.Pointer(),Input.Columns(),Centers.Columns(),device_activ_matrix.Columns(),device_activ_matrix.Rows(),scaling_factor,Widths.Pointer()); DeviceMatrix<float> d_Output(device_activ_matrix.Rows(),Weights.Columns(),ColumnMajor); device_activ_matrix.Multiply(device_activ_matrix,Weights,d_Output); return d_Output; }
void show (KinfuTracker& kinfu, bool integrate_colors) { viewer_pose_ = kinfu.getCameraPose(); ScopeTimeT time ("PointCloud Extraction"); cout << "\nGetting cloud... " << flush; valid_combined_ = 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 (compute_normals_) { kinfu.volume().fetchNormals (extracted, normals_device_); pcl::gpu::mergePointNormal (extracted, normals_device_, combined_device_); combined_device_.download (combined_ptr_->points); combined_ptr_->width = (int)combined_ptr_->points.size (); combined_ptr_->height = 1; valid_combined_ = true; } else { 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; } else point_colors_ptr_->points.clear(); } size_t points_size = valid_combined_ ? combined_ptr_->points.size () : cloud_ptr_->points.size (); cout << "Done. Cloud size: " << points_size / 1000 << "K" << endl; cloud_viewer_.removeAllPointClouds (); if (valid_combined_) { visualization::PointCloudColorHandlerRGBHack<PointNormal> rgb(combined_ptr_, point_colors_ptr_); cloud_viewer_.addPointCloud<PointNormal> (combined_ptr_, rgb, "Cloud"); cloud_viewer_.addPointCloudNormals<PointNormal>(combined_ptr_, 50); } else { visualization::PointCloudColorHandlerRGBHack<PointXYZ> rgb(cloud_ptr_, point_colors_ptr_); cloud_viewer_.addPointCloud<PointXYZ> (cloud_ptr_, rgb); } }
bool validate(size_t cloud_size) const { return (sizes.size() == cloud_size) && (cloud_size * max_elems == data.size()); }
size_t neighboors_size() const { return data.size()/max_elems; }