void MKinect::getKinectData(QImage &image) {
IMultiSourceFrame* frame = NULL;
while (!SUCCEEDED(reader->AcquireLatestFrame(&frame))) {
}
getDepthData(frame, depthP);
getColorData(frame, image);
}
void cloudcb(const sensor_msgs::PointCloud2Ptr &cloudptr) {
//convert the PointCloud2 into a pcl PointCloud
//which is a nice data type that we can work with
PointCloudXYZ cloud;
pcl::fromROSMsg(*cloudptr, cloud);
//The image we will output
sensor_msgs::Image outimage;
//get the width and height of the image
//from the CameraInfo topic
// const sensor_msgs::CameraInfoConstPtr infoptr =
// ros::topic::waitForMessage<sensor_msgs::CameraInfo>
// ("/camera/rgb/camera_info");
outimage.height = 480; //infoptr->height;
outimage.width = 640; //infoptr->width;
//now we need an array of inverse depths
unsigned int currindex = 0;
double maxz = 0, invdepth[outimage.height*outimage.width];
//note: the image is indexed (row, col)
//while the cloud is indexed (x,y)
//so the indexes have to be backwards
for (unsigned int i = 0; i < outimage.height; i++) {
for (unsigned int j = 0; j < outimage.width; j++) {
//the depth at the point (i, j)
//in the image
double zval = 0;
if (!isnan(zval)) {
//we have good depth data
zval = 1.0/cloud(j,i).z;
}
invdepth[currindex] = zval;
if (zval > maxz) {
maxz = zval;
}
currindex++;
}
}
//color image or not?
bool use_color;
ros::NodeHandle nh;
nh.param("kinect_use_color", use_color, false);
if (use_color) {
getColorData(invdepth, maxz, outimage);
} else {
getBWData(invdepth, maxz, outimage);
}
ROS_DEBUG("Publishing image");
//publish the message
image_pub.publish(outimage);
if (local_display) {
//show it in the window
cv_bridge::CvImagePtr cv_ptr;
try {
cv_ptr = cv_bridge::toCvCopy
(outimage, sensor_msgs::image_encodings::BGR8);
} catch (cv_bridge::Exception &e) {
ROS_WARN("Unable to convert from ROS to CV images: %s", e.what());
return;
}
cv::imshow("Depth-Color Image", cv_ptr->image);
cv::waitKey(3);
}
}
