Robotics StackExchange | Archived questions

How can i get object distance using camera/depth/image_raw

Hello all, I am using the ros topic /camera/depth/imageraw. It publish sensormsgs::image data with depth information. I can get the image but I do not exactly how can i get depth information. I need to what is the minimum distance of any pixel infront of the camera. I actually do not want to use the topic /camera/depth/points to measure the distance, as it is said /camera/depth/image_raw provides depth infromation. Can anyone please share some suggestion? I am using ros indigo, kinect, freenect.launch, opencv. Can anyone suggest me which library function is appropriate for me here

void callback(const sensor_msgs::Image::ConstPtr& msg){

 if(check_moving_obs)
 {
     if(img_previous.data.size() == 0)
     {
         img_previous = *msg;
         img_primary = *msg;
     }
     else
         {img_previous = img_current;}
    img_current = *msg;

     cout<<"------------------------header of msg :---------:"<< msg->header.seq <<"\n";
    cout<<"------------------------current image info :---------:"<<img_current.data.size()<<"\n";
    cout<<"------------------------Height :---------:"<<img_current.height<<"\n";
    cout<<"------------------------Width :---------:" <<img_current.width<<"\n";

    cout<<"------------------------previous image info :---------:"<<img_previous.data.size()<<"\n";
    cout<<"------------------------Height :---------:"<<img_previous.height<<"\n";
    cout<<"------------------------Width :---------:" <<img_previous.width<<"\n";


        //Convert from the ROS image message to a CvImage suitable for working with OpenCV for processing
            cv_bridge::CvImagePtr cv_ptr_prev, cv_ptr_cur;
            cv::Mat image_cur, image_pre, differenceImage;
            //thresholded difference image (for use in findContours() function)
            cv::Mat thresholdImage;

            try
            {

                //Always copy, returning a mutable CvImage
                //OpenCV expects color images to use BGR channel order.
                cv_ptr_cur = cv_bridge::toCvCopy(msg, enc::TYPE_32FC1);//cv_bridge::toCvCopy(img_current, enc::BGR8);
                cv_ptr_prev = cv_bridge::toCvCopy(img_previous, enc::TYPE_32FC1);//TYPE_16UC1


                image_cur = cv_ptr_cur->image;
                image_pre = cv_ptr_prev->image;
                int count_diff=0;
                bool motion_detected = false;

                //perform frame differencing with the sequential images.
                cv::absdiff(image_cur,image_pre,differenceImage);
                //threshold intensity image at a given sensitivity value
                cv::threshold(differenceImage,thresholdImage, 20,255,cv::THRESH_BINARY);
                cout<<"~~~~~~~~~~~~IMP~~~~~~~~~~difference image info :---------:"<<thresholdImage.rows<<"\n";
                cout<<"~~~~~~~~~~~~~IMP~~~~~~~~~difference image info :---------:"<<thresholdImage.cols<<"\n";
                for(int j=0; j< thresholdImage.rows; ++j)
                {
                    //const double* Mi = differenceImage.ptr<double>(j);
                    for(int i=0; i< thresholdImage.rows; ++i)
                    {
                        //cout<<"diff img pixel val : "<<Mi[i]<<"\n";
                        //double depth = thresholdImage.at<double>(cv::Point(j,i));
                        //cout<<"diff img depth val : "<<depth<<"\n";
                        cout<<"+++++++++++++++++++++++++++++++++++++++++++++\n";
                        cout<<"+++++ i: "<<i <<"------ j : "<< j<<"++++++++++++++++++++++++++++++++++++++++\n";
                        cout<<"+++++++++++++++++++++++++++++++++++++++++++++\n";
                        cout<<"current  image    : "<< image_cur.at<cv::Vec3b>(j,i)       << "cur img depth val : "<<image_cur.at<double>(cv::Point(j,i))<<"\n";
                        cout<<"previous  image   : "<< image_pre.at<cv::Vec3b>(j,i)       << "pre img depth val : "<<image_pre.at<double>(cv::Point(j,i))<<"\n";
                        cout<<"abs-diff  image   : "<< differenceImage.at<cv::Vec3b>(j,i) << "dif img depth val : "<<differenceImage.at<double>(cv::Point(j,i))<<"\n";
                        cout<<"threshholöd image : "<< thresholdImage.at<cv::Vec3b>(j,i ) << "thr img depth val : "<<thresholdImage.at<double>(cv::Point(j,i))<<"\n";
                        cout<<"+++++++++++++++++++++++++++++++++++++++++++++\n";

                    }

                }
                /*
                for(int j=0; j<image_cur.rows; ++j)
                {
                    if(count_diff > 150)
                    {
                        break;
                    }
                        for(int i=0; i<image_cur.cols; ++i)
                        {
                            int depth = image_cur.at<short int>(cv::Point(j,i));
                            if(depth < 0.6)
                            {
                                cout<<"##############something within the range infront of the camera...########################\n";
                                if(image_cur.at<cv::Vec3b>(j,i) != image_pre.at<cv::Vec3b>(j,i))
                                {
                                    cout<<"i : "<<i<<" ... j : "<<j<<"\n";
                                    cout<<"iamge curr : "<< image_cur.at<cv::Vec3b>(j,i)<<"\n";
                                    cout<<"iamge prev : "<< image_pre.at<cv::Vec3b>(j,i)<<"\n";
                                    count_diff++;
                                }

                                if(count_diff > 150)
                                {
                                    motion_detected = true;
                                    break;
                                }
                            }
                        }
                }
                */
                if(motion_detected)
                {
                    cout<<"------------------------something is moving stop and wait---------"<<"\n";
                }

                //threshold intensity image at a given sensitivity value
                //cv::threshold(differenceImage,thresholdImage, 20,255,cv::THRESH_BINARY);
                //show the difference image and threshold image
                //cv::imshow("Difference Image",differenceImage);
                //cv::imshow("Threshold Image", thresholdImage);

                //imshow("Frame1",image_pre);

            }
            catch (cv_bridge::Exception& e)
            {
                //if there is an error during conversion, display it
                ROS_ERROR("cv_bridge exception: %s", e.what());
                return;
            }
       //pub.publish( msg_decission);
    }
 sleep(1);//use sleep if you want to delay loop.

}


some portion of the output is as below:

+++++++++++++++++++++++++++++++++++++++++++++

+++++ i: 170------ j : 82++++++++++++++++++++++++++++++++++++++++ +++++++++++++++++++++++++++++++++++++++++++++ current image : [174, 68, 0]cur img depth val : 4.01401e+23 previous image : [173, 68, 0]pre img depth val : 3.63622e+23 abs-diff image : [192, 64, 0]dif img depth val : 3.35544e+07 threshholöd image : [0, 0, 0]thr img depth val : 0 +++++++++++++++++++++++++++++++++++++++++++++ +++++++++++++++++++++++++++++++++++++++++++++ +++++ i: 171------ j : 82++++++++++++++++++++++++++++++++++++++++ +++++++++++++++++++++++++++++++++++++++++++++ current image : [64, 175, 68]cur img depth val : 3.82512e+23 previous image : [128, 174, 68]pre img depth val : 3.82512e+23 abs-diff image : [0, 192, 64]dif img depth val : 0 threshholöd image : [0, 0, 0]thr img depth val : 0 +++++++++++++++++++++++++++++++++++++++++++++ +++++++++++++++++++++++++++++++++++++++++++++ +++++ i: 172------ j : 82++++++++++++++++++++++++++++++++++++++++ +++++++++++++++++++++++++++++++++++++++++++++ current image : [0, 224, 175]cur img depth val : 3.82512e+23 previous image : [0, 224, 175]pre img depth val : 3.82512e+23 abs-diff image : [0, 0, 0]dif img depth val : 5.39306e-315 threshholöd image : [0, 0, 0]thr img depth val : 0

Asked by rasoo on 2016-10-20 06:31:43 UTC

Comments

why absdiff method is giving some such wierd value?

Asked by rasoo on 2016-10-21 08:58:15 UTC

Answers

That depends on the image encoding. Check the value of "encoding" on the message being published.

If it's "32FC1" then the depth data for each pixel is a 32bit float where each value is the distance in meters from the camera. In c++ you can get the depth data from this encoding by casting to an array of floats.

//sensor_msgs::Image image;
if (image.encoding == sensor_msgs::image_encodings::TYPE_32FC1)
{
    const float * depth_array = reinterpret_cast<const float *>(&(image.data[0]));
    //depth_array[0], depth_array[1], ..., depth_aray[image.height * image.width - 1]
]

Asked by sloretz on 2016-10-20 10:42:10 UTC

Comments

image.data[0] -- does it means that the depth information for the zero-th pixel ?

Asked by rasoo on 2016-10-20 13:00:10 UTC

image.data[0] would give you the first byte (of 4 if encoding is 32FC1) of the zero-th pixel. depth_array[0] gives you the depth of the zero-th pixel. Pixels are in row-major order.

Asked by sloretz on 2016-10-20 13:15:25 UTC

Also, the package cv_bridge has some methods for converting between ros messages and opencv types. You may find these tutorials useful.

Asked by sloretz on 2016-10-20 13:27:26 UTC

Thanks for your response, but I am having some problem to understand. I added partial code in my original post. Can you please recheck it for me? I target is 1. movement detection by image compare 2. only detect when any object is within the range(0.6m).

Asked by rasoo on 2016-10-21 07:47:57 UTC

the Topic /camera/depth/image is of TYPE_32FC1 and the Topic /camera/depth/image_raw is of TYPE_16UC1. now if need to know the depth information of (i,j)th pixel then what should i do?

Asked by rasoo on 2016-10-23 21:23:03 UTC

My answer with complete code (c++) : https://answers.ros.org/question/141741/calculate-depth-from-point-cloud-xyz/?answer=381085#post-id-381085

Code funtionality:

  • Subscribed to /camera/depth/image_raw rostopic,
  • Convert ROS image to OpenCV image
  • Get image dimension
  • Get global max and min value,
  • Get depth value at specific point,

Asked by askkvn on 2021-06-25 01:31:57 UTC

Comments

Hey, how do I do in python ?

Asked by Siddarth09 on 2021-12-13 08:50:29 UTC