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