void MyPointCloud::getDepthMap(unsigned short *depthMap) { for(int pixel = 0; pixel < (640 * 480); pixel++) depthMap[pixel] = 0; DeviceArray2D<pcl::PointXYZ> cloudDevice; pcl::PointCloud<PointXYZ>::Ptr hostFrameCloud; this->getLastFrameCloud(cloudDevice); int c; hostFrameCloud = PointCloud<PointXYZ>::Ptr (new PointCloud<PointXYZ>); cloudDevice.download (hostFrameCloud->points, c); hostFrameCloud->width = cloudDevice.cols (); hostFrameCloud->height = cloudDevice.rows (); int count = 0; int x, y, pixel; for(int point = 0; point < hostFrameCloud->points.size(); point++) { if(hostFrameCloud->points[point].z == hostFrameCloud->points[point].z) { if(hostFrameCloud->points[point].z != 0) { x = (hostFrameCloud->points[point].x * 525.f / hostFrameCloud->points[point].z) + 320; y = (hostFrameCloud->points[point].y * 525.f / hostFrameCloud->points[point].z) + 240; pixel = y * 640 + x; if(pixel >= 0 && pixel < (640 * 480)) depthMap[pixel] = hostFrameCloud->points[point].z; } } } }
void pcl::gpu::people::colorizeLabels(const DeviceArray<pcl::RGB>& color_map, const DeviceArray2D<unsigned char>& labels, DeviceArray2D<pcl::RGB>& color_labels) { color_labels.create(labels.rows(), labels.cols()); const DeviceArray<uchar4>& map = (const DeviceArray<uchar4>&)color_map; device::Image& img = (device::Image&)color_labels; device::colorLMap(labels, map, img); }
void pcl::gpu::KinfuTracker::getLastFrameNormals (DeviceArray2D<NormalType>& normals) const { normals.create (rows_, cols_); DeviceArray2D<float8>& n = (DeviceArray2D<float8>&)normals; device::convert (nmaps_g_prev_[0], n); }
void pcl::gpu::KinfuTracker::getLastFrameCloud (DeviceArray2D<PointType>& cloud) const { cloud.create (rows_, cols_); DeviceArray2D<float4>& c = (DeviceArray2D<float4>&)cloud; device::convert (vmaps_g_prev_[0], c); }
template<> PCL_EXPORTS void convertMapToOranizedCloud<pcl::Normal> (const RayCaster::MapArr& map, DeviceArray2D<pcl::Normal>& cloud) { cloud.create (map.rows()/3, map.cols()); DeviceArray2D<float8>& n = (DeviceArray2D<float8>&)cloud; device::convert(map, n); }
void MyPointCloud::getLastFrameNormals(DeviceArray2D<pcl::PointXYZ>& normals) { normals.create (rows_, cols_); DeviceArray2D<float4>& n = (DeviceArray2D<float4>&)normals; device::convert<float4>(nmaps_[0], n); }
void MyPointCloud::getLastFrameCloud(DeviceArray2D<pcl::PointXYZ>& cloud) { cloud.create (rows_, cols_); DeviceArray2D<float4>& c = (DeviceArray2D<float4>&)cloud; device::convert (vmaps_[0], c); }
void MyPointCloud::getHostErrorInRGB(DeviceArray2D<pcl::PointXYZI>& errorInRGB) { errorInRGB.create(rows_ , cols_); DeviceArray2D<float4>& c = (DeviceArray2D<float4>&)errorInRGB; device::convertXYZI(error_, c); }
void pcl::gpu::PseudoConvexHull3D::reconstruct (const Cloud &cloud, DeviceArray2D<int>& vertexes) { const device::Cloud& c = (const device::Cloud&)cloud; device::FacetStream& fs = impl_->fs; device::PointStream ps(c); ps.computeInitalSimplex(); device::InitalSimplex simplex = ps.simplex; fs.setInitialFacets(ps.simplex); ps.initalClassify(); for(;;) { //new external points number ps.cloud_size = ps.searchFacetHeads(fs.facet_count, fs.head_points); if (ps.cloud_size == 0) break; fs.compactFacets(); ps.classify(fs); if (!fs.canSplit()) throw PCLException("Can't split facets, please enlarge default buffer", __FILE__, "", __LINE__); fs.splitFacets(); } int ecount; int fcount = fs.facet_count; fs.empty_count.download(&ecount); vertexes.create(3, fcount + ecount); DeviceArray2D<int> subf(3, fcount, vertexes.ptr(), vertexes.step()); DeviceArray2D<int> sube(3, ecount, vertexes.ptr()+fcount, vertexes.step()); DeviceArray2D<int>(3, fcount, fs.verts_inds.ptr(), fs.verts_inds.step()).copyTo(subf); DeviceArray2D<int>(3, ecount, fs.empty_facets.ptr(), fs.empty_facets.step()).copyTo(sube); }
Eigen::Vector3f& MyPointCloud::getCentroid() { DeviceArray2D<pcl::PointXYZ> cloudDevice; pcl::PointCloud<PointXYZ>::Ptr hostFrameCloud; this->getLastFrameCloud(cloudDevice); int c; hostFrameCloud = PointCloud<PointXYZ>::Ptr (new PointCloud<PointXYZ>); cloudDevice.download (hostFrameCloud->points, c); hostFrameCloud->width = cloudDevice.cols (); hostFrameCloud->height = cloudDevice.rows (); Eigen::Vector3f centroid; centroid.setZero(); int count = 0; for(int point = 0; point < hostFrameCloud->points.size(); point++) { if(hostFrameCloud->points[point].z == hostFrameCloud->points[point].z) { if(hostFrameCloud->points[point].z != 0) { centroid(0) += hostFrameCloud->points[point].x; centroid(1) += hostFrameCloud->points[point].y; centroid(2) += hostFrameCloud->points[point].z; count++; } } } centroid /= count; return centroid; }
void pcl::gpu::PseudoConvexHull3D::reconstruct (const Cloud &points, Cloud &output) { DeviceArray2D<int> vertexes; reconstruct(points, vertexes); DeviceArray<int> cont(vertexes.cols() * vertexes.rows()); DeviceArray2D<int> buf(3, vertexes.cols(), cont.ptr(), vertexes.cols() * sizeof(int)); vertexes.copyTo(buf); size_t new_size = device::remove_duplicates(cont); DeviceArray<int> new_cont(cont.ptr(), new_size); output.create(new_size); const device::Cloud& c = (const device::Cloud&)points; device::Cloud& o = (device::Cloud&)output; device::pack_hull(c, new_cont, o); }
void MulticamFusion::getLastFrameCloud (DeviceArray2D<PointXYZ>& cloud) const{ cloud.create (rows, cols); DeviceArray2D<float4>& c = (DeviceArray2D<float4>&)cloud; device::convert (vmaps_g_prev_, c); }