0
$\begingroup$

Rosanswers logo

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.


Originally posted by lukeb88 on ROS Answers with karma: 33 on 2014-05-23

Post score: 2

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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::PointCloudpcl::PointXYZRGBA::Ptr cloud (new pcl::PointCloudpcl::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 :)


Originally posted by Sudeep with karma: 460 on 2014-05-24

This answer was ACCEPTED on the original site

Post score: 5


Original comments

Comment by lukeb88 on 2014-05-26:
for first thanks for your answer... In Ros i'm subscribed to this: ros::Subscriber sub = nh.subscribe< pcl::PointCloudpcl::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...

Comment by Sudeep on 2014-05-26:
if you subscribe to pointcloud topic by "ros::Subscriber sub = nh.subscribepcl::PointCloud<pcl::PointXYZRGB >("/points", 1, callback);"

then in callback function call, use pointcloud ptr, void callback(const pcl::PointCloudpcl::PointXYZRGB::ConstPtr& input) this will work :)

Comment by lukeb88 on 2014-05-26:
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...

Comment by Sudeep on 2014-05-26:
This should work, can you attach your function to get depth! Is RGB image part working?

Comment by lukeb88 on 2014-05-27:
the rgb part work well! but i can't take the depth... i updated my question with my code!

Comment by Sudeep on 2014-05-27:
luke, code is bit unclear, first if depth is a vector (std::vactor depth) then use

depth.push_back((float)depth_value). and also update the value of call_count while printing. It should work

Comment by lukeb88 on 2014-05-27:
hi sudeep, now all work! thanks for your answer, it was really useful!

Comment by PKumars on 2016-01-19:
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

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.