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;
}
}
}
}
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;
}