How to understand the depth of a pixel with the kinect

asked 2014-05-23 03:55:13 -0600

lukeb88
33 ●2 ●3 ●6

updated 2014-05-26 23:49:55 -0600

Hi!

I'm new of ROS and for an university project i have to grab a cup with a robot. For first i have to understand where the cup is. I have implemented an algorithm that segment the cup from the rest of the scene, and then it calculate the kaypoints of the new image. At this point i can determinate the "central" of my keypoints, that is broadly the centre of my cup.

The problem now is that till here i have worked with openCV and with the sensor_msgs/image, but to understand the depth of my pixel (the cup position) i need the registered depth topic.

What i couldn't understand it's: How i can subscribe the /camera/depth_registered/points (sensor_msgs/PointCloud2) to extract the RGB image (for the procedure that till here i did with sensor_msgs/image) and the depth image to know the depth of my task's pixel?

thanks

Luca

UPDATE

this is my callback

void callback(const sensor_msgs::PointCloud2::ConstPtr& msg) {
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::fromROSMsg (*msg, *cloud);

    cv::Mat imageFrame;
    if (cloud->isOrganized()) {
        imageFrame = cv::Mat(cloud->height, cloud->width, CV_8UC3); 

        for (int h=0; h<imageFrame.rows; h++) {
            for (int w=0; w<imageFrame.cols; w++) {

                pcl::PointXYZRGB point = cloud->at(w, h);

                Eigen::Vector3i rgb = point.getRGBVector3i();

                imageFrame.at<cv::Vec3b>(h,w)[0] = rgb[2];
                imageFrame.at<cv::Vec3b>(h,w)[1] = rgb[1];
                imageFrame.at<cv::Vec3b>(h,w)[2] = rgb[0];

                int i = centre_x + centre_y*cloud->width;
                depth[call_count] = (float)cloud->points[i].z;

            }
        }
}

}//end callback

like you suggested to me. i tried to put the depth of my pixel in a vector, so i thought that i could see at least some values different to 0 (for the noise of the kinect i know that it's possible that some depth values in a point could be 0 or near 0 sometimes). But in my case all values still 0.

edit retag flag offensive close merge delete

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2014-05-24 01:39:05 -0600

Sudeep

460 ●4 ●5 ●14 http://web.iiit.ac.in/...

Hi luke,

To get RGB image from cloud data ,

   cv::Mat imageFrame;
if (cloud->isOrganized()) 
{
    imageFrame = cv::Mat(cloud->height, cloud->width, CV_8UC3); 
    {

        for (int h=0; h<imageFrame.rows; h++) 
        {
            for (int w=0; w<imageFrame.cols; w++) 
            {
                pcl::PointXYZRGBA point = cloud->at(w, h);

                Eigen::Vector3i rgb = point.getRGBVector3i();

                imageFrame.at<cv::Vec3b>(h,w)[0] = rgb[2];
                imageFrame.at<cv::Vec3b>(h,w)[1] = rgb[1];
                imageFrame.at<cv::Vec3b>(h,w)[2] = rgb[0];
            }
        }
    }
}

But first you might need to convert cloud data into pcl supported form, for this you can use,

pcl::PointCloud<pcl::pointxyzrgba>::Ptr cloud (new pcl::PointCloud<pcl::pointxyzrgba>);

pcl::fromROSMsg (*input, *cloud);

Once you get the image, you can apply your detection algorithm using opencv libs,

To get depth of point x,y in image

int i = x + y*cloud->width; float depth = (float)cloud->points[i].z;

Hope this helped :)

edit flag offensive delete link

Comments

for first thanks for your answer... In Ros i'm subscribed to this: ros::Subscriber sub = nh.subscribe< pcl::PointCloud<pcl::pointxyzrgb> >("/camera/depth_registered/points", 1, callback); because i discovered that i can directly subscribe a pcl::PointCloud insted of sensor_msgs::PointCloud, So if I understand correctly i have not to convert cloud data in pcl...

lukeb88 ( 2014-05-26 00:00:42 -0600 )edit

if you subscribe to pointcloud topic by "ros::Subscriber sub = nh.subscribe<pcl::pointcloud<pcl::pointxyzrgb> >("/points", 1, callback);" then in callback function call, use pointcloud ptr, void callback(const pcl::PointCloud<pcl::pointxyzrgb>::ConstPtr& input) this will work :)

Sudeep ( 2014-05-26 01:16:02 -0600 )edit

i solved the last question using a subscriber that receive a sensor_msgs::PointCloud2 and then using pcl::fromROSMsg (*input, *cloud); But now my problem is that the depth it's always 0, i can't understand why...

lukeb88 ( 2014-05-26 04:08:40 -0600 )edit

This should work, can you attach your function to get depth! Is RGB image part working?

Sudeep ( 2014-05-26 06:32:28 -0600 )edit

the rgb part work well! but i can't take the depth... i updated my question with my code!

lukeb88 ( 2014-05-27 01:01:17 -0600 )edit

luke, code is bit unclear, first if depth is a vector (std::vactor<float> depth) then use > depth.push_back((float)depth_value). and also update the value of call_count while printing. It should work

Sudeep ( 2014-05-27 05:13:12 -0600 )edit

hi sudeep, now all work! thanks for your answer, it was really useful!

lukeb88 ( 2014-05-27 22:21:00 -0600 )edit

Hi luke can you please tell how you are able to detect cup from pointcloud2? I'm using kinect and now I can detect a ball using Opencv. I want to add depth information to it. I think you can help me in extracting RGB and Depth information using the topic /camera/depth_registered/points.

Thanks