Ejemplo n.º 1
0
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;
			}

		}

	}

}
Ejemplo n.º 2
0
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);
}
Ejemplo n.º 3
0
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);
}
Ejemplo n.º 4
0
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);
}
Ejemplo n.º 5
0
 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);
 }
Ejemplo n.º 6
0
void MyPointCloud::getLastFrameNormals(DeviceArray2D<pcl::PointXYZ>& normals) {
	
	normals.create (rows_, cols_);
	DeviceArray2D<float4>& n = (DeviceArray2D<float4>&)normals;
	device::convert<float4>(nmaps_[0], n);

}
Ejemplo n.º 7
0
void MyPointCloud::getLastFrameCloud(DeviceArray2D<pcl::PointXYZ>& cloud) {
	
	cloud.create (rows_, cols_);
	DeviceArray2D<float4>& c = (DeviceArray2D<float4>&)cloud;
	device::convert (vmaps_[0], c);

}
Ejemplo n.º 8
0
void MyPointCloud::getHostErrorInRGB(DeviceArray2D<pcl::PointXYZI>& errorInRGB) {

	errorInRGB.create(rows_ , cols_);
	DeviceArray2D<float4>& c = (DeviceArray2D<float4>&)errorInRGB;
	device::convertXYZI(error_, c);

}
Ejemplo n.º 9
0
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);
}
Ejemplo n.º 10
0
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;

}
Ejemplo n.º 11
0
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);
}
Ejemplo n.º 12
0
void MulticamFusion::getLastFrameCloud (DeviceArray2D<PointXYZ>& cloud) const{
	cloud.create (rows, cols);
	DeviceArray2D<float4>& c = (DeviceArray2D<float4>&)cloud;
	device::convert (vmaps_g_prev_, c);
}