Ask Your Question
0

Using a depth map

asked 2018-08-07 07:51:55 -0500

Welsy gravatar image

Hi, I have had issues finding any information on how to use a depth map/image to gain the distance to an obstacle.

I have an Intel Realsense D415 camera, I have installed the SDK, the ROS wrapper, I have a topic with a depth map published. (/camera/depth/image_rect_raw)

Next, I've written a little program in C++ that converts the image using cv_bridge to an OpenCV image and I display it in a window, so I know it's working. (image: https://i.imgur.com/QyKWp2J.png )

Now I need to get the distance from it, which I have no idea how to do and have been unsuccessful in finding help for. I imagine I'm only going to want to use the top half or 2/3 of the image, because of the way the camera will be mounted, so the bottom third/half will only contain of floor/ground.

I feel like I am missing something big and simple in order to make this work, but I literally dont even know what to do now.

The code:

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "sensor_msgs/Image.h"
#include <iostream>
#include "image_transport/image_transport.h"
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>



void handleTwist(const geometry_msgs::Twist& msg);
void handleCamera(const sensor_msgs::ImageConstPtr& msg);

std::string OPENCV_WINDOW = "Camera";

int main(int argc, char** argv)
{
    ros::init(argc,argv,"coldetect_node");

    ros::NodeHandle nh;
    image_transport::ImageTransport it(nh);
    ros::Subscriber twist = nh.subscribe("cmd", 1000, handleTwist);
    image_transport::Subscriber camera = it.subscribe("camera/depth/image_rect_raw", 1, handleCamera);
    cv::namedWindow(OPENCV_WINDOW, cv::WINDOW_AUTOSIZE);

    ros::spin();

    cv::destroyAllWindows();
    return 0;

}

void handleTwist(const geometry_msgs::Twist& msg){
    ROS_INFO("\nlinear:{%.1f,%.1f,%.1f}\nangular:{%.1f,%.1f,%.1f}", msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.x, msg.angular.y, msg.angular.z);    
}

void handleCamera(const sensor_msgs::ImageConstPtr& msg){
    //ROS_INFO("Image received");
    cv_bridge::CvImageConstPtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_16UC1);
      ROS_INFO("copied image");
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
    cv::imshow(OPENCV_WINDOW, cv_ptr->image);
    cv::waitKey(100);
}
edit retag flag offensive close merge delete

Comments

What do you mean by getting the distance from it? The depth image contains the depth information for any pixel. As Welsy noted in his answer answer you can get the depth of a specific pixel.

pavel92 gravatar imagepavel92 ( 2018-08-08 01:43:11 -0500 )edit

Well I wasn't sure it contained it, because I once tried to just get the value of pixel (x,y) and it was some sort of nonsense. Now I know I was using the wrong integer type, but it confused me before. I also wasn't sure whether it was in mm or what units.

Welsy gravatar imageWelsy ( 2018-08-08 03:27:04 -0500 )edit

uint16 depth image is pretty convenient to use.

pavel92 gravatar imagepavel92 ( 2018-08-08 05:59:47 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-08-07 10:40:57 -0500

Welsy gravatar image

updated 2018-08-08 03:27:44 -0500

I have been able to obtain the distance with a very simple line of code...

double distance;
distance = 0.001*cv_ptr->image.at<u_int16_t>(cv_ptr->image.rows/2, cv_ptr->image.cols/2);

This gives me the distance of the object in the center of the image. In meters.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2018-08-07 07:27:54 -0500

Seen: 97 times

Last updated: Aug 08 '18