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
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
Comments
why absdiff method is giving some such wierd value?
Asked by rasoo on 2016-10-21 08:58:15 UTC