Пример #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;
			}

		}

	}

}
Пример #2
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;

}