How can i estimate distance from object to my kinect camera based on YOLO?

[catalin15dcm]

Hi, I'm using your code and i find it very useful.

Yolo is providing just the bounding box coordinates and i'm looking forward to the object coordinates in 3d. I know that a kinect camera provide some depth images and i'm asking if there is any chance to obtain the object coordinates. So, How can i realize that? is there any idea?

[AlejoDiaz49]

Hi,

You could subscribe the YOLO node to the depth image topic of the Kinect, try to make it synchronizing the subscription of the color image that the YOLO use and the depth image, so that both are from the same sample time .

And then knowing the box of an object that the YOLO returns, go to the depth image and extract the depth value. Here you need to analyze your objects, maybe with just the depth value of the central point of the box is enough, but probably you will need to make something more elaborated, like calculate the median of all the values inside the box region. Next with the parameters of the camera you can just calculate the other 2 coordinates, for these use the equations of the pinhole model of a camera

Another option, is using the Point Cloud topic of the Kinect. With the coordinates of the color image (u,v) you can extract the real coordinates (x,y,z) from the Point Cloud

Hope this can help you

[catalin15dcm]

Can you tell me how to subscribe a node to a topic and how to extract the depth value from the central point of the object? I've done a lot of research about this info but i can't get the position of objects. I think that beacause I'm new into ROS and it sure is simple problem.

It really helped me your comment. Thanks!

[AlejoDiaz49]

There is probably a better way to do it than this one, but it works. Using OpenCV first you need to change the format so that OpenCV functions can work with it

DepthImage = cv_bridge::toCvCopy(msgdepth, sensor_msgs::image_encodings::TYPE_32FC1)

If you put 32FC1 the values of the pixel of the image should be already the distance in meters. Here you have a tutorial with more details. Then knowing the 2 points that define the box of the YOLO

• xmin
• ymin
• xmax
• ymax

You can calculate the middle point with

Xc = ((xmax-xmin)/2)+xmin Yc = ((ymax-ymin)/2)+ymin

And then to get the distane of that point just

Zreal=(float)DepthImage.at<float>(Yc,Xc);

This value you get is the Z coordinate, and to get the other two, as I told you, just use the parameters of the camera

Xreal=((Xc-Cx)*Z)/fx Yreal=((Yc-Cy)*Z)/fy

I know that there is a faster way to do it without OpenCV, accessing directly to the message. But I don't know exactly how to do it. If later you want to do something else to have a better depth value, you can use some filtering functions from OpenCV or anything else you want to use.

[catalin15dcm]

Is there any formula to get the Zreal using Xc and Yc? How can i get the Xreal and Yreal?

[AlejoDiaz49]

Yes, you can do it directly with the point cloud that the kinect generates. Here you have the structure of the message. And to get the coordinates you should do something like this

int ind = (x_center) + (y_center)*cloud->width;

Xreal = (float)cloud->points[ind].x; Yreal = (float)cloud->points[ind].y; Zreal = (float)cloud->points[ind].z;

[catalin15dcm]

Where should i put this?

[AlejoDiaz49]

After this line is a good place, so every time an object is detected you get the real coordinates. Of course you need to add at the begining the PointCloud topic as a subscription

[catalin15dcm]

I finally managed to get x_center and y_center correctly with your help. I'm trying to add PointCloud topic as a subscrition but i don't know how and where.

[catalin15dcm]

It's my last piece of project and i'm not able to finish it. Could you help me?

[AlejoDiaz49]

To subscribe to the topic use the same structure as in the package, but synchronizing (same sample time) the image for the YOLO and the PointCloud. I'll show you with the depth image instead of the Point Cloud beacuse is the way I did it and I dont have time right now to change it. You only have to change the topic but the structure is the same

YoloObjectDetector.cpp Around this line you have to add/change this

  // Initialize publisher and subscriber.
  std::string depthTopicName;        //For depth inclussion
  int depthQueueSize;                //For depth inclussion

  nodeHandle_.param("subscribers/camera_reading/topic", cameraTopicName, std::string("/camera/image_raw"));
  nodeHandle_.param("subscribers/camera_reading/queue_size", cameraQueueSize, 1);

  nodeHandle_.param("subscribers/camera_depth/topic", depthTopicName, std::string("/depth/image_raw"));   //For depth inclussion
  nodeHandle_.param("subscribers/camera_depth/queue_size", depthQueueSize, 1);                            //For depth inclussion

  sync_1.registerCallback(boost::bind(&YoloObjectDetector::cameraCallback,this,_1,_2));   //For depth inclussion

and also at the beggining for this to work add here the lines that has the comment "For depth inclussion"

  namespace darknet_ros 
  {
      char *cfg;
      char *weights;
      char *data;
      char **detectionNames;

      YoloObjectDetector::YoloObjectDetector(ros::NodeHandle nh)
          : nodeHandle_(nh),
            imageTransport_(nodeHandle_),
            numClasses_(0),
            classLabels_(0),
            rosBoxes_(0),
            rosBoxCounter_(0),
            imagergb_sub(imageTransport_,"/camera/rgb/image_raw",1),       //For depth inclussion
            imagedepth_sub(imageTransport_,"/camera/depth/image_raw",1),   //For depth inclussion
            sync_1(MySyncPolicy_1(5), imagergb_sub, imagedepth_sub)        //For depth inclussion

Then in the file YoloObjectDetector.hpp you have to add this includes

  #include <stdio.h>
  #include <std_msgs/String.h>
  #include <message_filters/subscriber.h>
  #include <message_filters/synchronizer.h>
  #include <message_filters/time_synchronizer.h>
  #include <message_filters/sync_policies/approximate_time.h>
  #include <image_transport/subscriber_filter.h>

and as public in the YoloObjectDetector class

  // Syncronizing Image messages
  typedef image_transport::SubscriberFilter ImageSubscriberFilter;
  ImageSubscriberFilter imagergb_sub;
  ImageSubscriberFilter imagedepth_sub;
  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> MySyncPolicy_1;
  message_filters::Synchronizer<MySyncPolicy_1> sync_1;

Finally to make it work, add in your config file .yaml in the suscriber the topic

  camera_depth:
     topic: /camera/depth/image_raw
     queue_size: 1

[catalin15dcm]

when i tried to catkin_make this is the error i get.

 In file included from /usr/include/boost/bind.hpp:22:0,
                 from /opt/ros/kinetic/include/ros/publisher.h:35,
                 from /opt/ros/kinetic/include/ros/node_handle.h:32,
                 from /opt/ros/kinetic/include/ros/ros.h:45,
                 from /home/cata/catkin_ws/src/darknet_ros/darknet_ros/include/darknet_ros/YoloObjectDetector.hpp:21,
                 from /home/cata/catkin_ws/src/darknet_ros/darknet_ros/src/YoloObjectDetector.cpp:10:
/usr/include/boost/bind/bind.hpp: In instantiation of ‘struct boost::_bi::result_traits<boost::_bi::unspecified, void (darknet_ros::YoloObjectDetector::*)(const boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >&)>’:
/usr/include/boost/bind/bind.hpp:883:48:   required from ‘class boost::_bi::bind_t<boost::_bi::unspecified, void (darknet_ros::YoloObjectDetector::*)(const boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >&), boost::_bi::list3<boost::_bi::value<darknet_ros::YoloObjectDetector*>, boost::arg<1>, boost::arg<2> > >’
/home/cata/catkin_ws/src/darknet_ros/darknet_ros/src/YoloObjectDetector.cpp:155:85:   required from here
/usr/include/boost/bind/bind.hpp:69:37: error: ‘void (darknet_ros::YoloObjectDetector::*)(const boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >&)’ is not a class, struct, or union type
     typedef typename F::result_type type;
                                     ^
In file included from /usr/include/boost/function/detail/maybe_include.hpp:58:0,
                 from /usr/include/boost/function/detail/function_iterate.hpp:14,
                 from /usr/include/boost/preprocessor/iteration/detail/iter/forward1.hpp:92,
                 from /usr/include/boost/function.hpp:64,
                 from /opt/ros/kinetic/include/ros/forwards.h:40,
                 from /opt/ros/kinetic/include/ros/common.h:37,
                 from /opt/ros/kinetic/include/ros/ros.h:43,
                 from /home/cata/catkin_ws/src/darknet_ros/darknet_ros/include/darknet_ros/YoloObjectDetector.hpp:21,
                 from /home/cata/catkin_ws/src/darknet_ros/darknet_ros/src/YoloObjectDetector.cpp:10:
/usr/include/boost/function/function_template.hpp: In instantiation of ‘static void boost::detail::function::void_function_obj_invoker9<FunctionObj, R, T0, T1, T2, T3, T4, T5, T6, T7, T8>::invoke(boost::detail::function::function_buffer&, T0, T1, T2, T3, T4, T5, T6, T7, T8) [with FunctionObj = boost::_bi::bind_t<boost::_bi::unspecified, boost::_bi::bind_t<boost::_bi::unspecified, void (darknet_ros::YoloObjectDetector::*)(const boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >&), boost::_bi::list3<boost::_bi::value<darknet_ros::YoloObjectDetector*>, boost::arg<1>, boost::arg<2> > >, boost::_bi::list9<boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::arg<4>, boost::arg<5>, boost::arg<6>, boost::arg<7>, boost::arg<8>, boost::arg<9> > >; R = void; T0 = const boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >&; T1 = const boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >&; T2 = const boost::shared_ptr<const message_filters::NullType>&; T3 = const boost::shared_ptr<const message_filters::NullType>&; T4 = const boost::shared_ptr<const message_filters::NullType>&; T5 = const boost::shared_ptr<const message_filters::NullType>&; T6 = const boost::shared_ptr<const message_filters::NullType>&; T7 = const boost::shared_ptr<const message_filters::NullType>&; T8 = const boost::shared_ptr<const message_filters::NullType>&]’:
/usr/include/boost/function/function_template.hpp:940:38:   required from ‘void boost::function9<R, T1, T2, T3, T4, T5, T6, T7, T8, T9>::assign_to(Functor) [with Functor = boost::_bi::bind_t<boost::_bi::unspecified, boost::_bi::bind_t<boost::_bi::unspecified, void (darknet_ros::YoloObjectDetector::*)(const boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >&), boost::_bi::list3<boost::_bi::value<darknet_ros::YoloObjectDetector*>, boost::arg<1>, boost::arg<2> > >, boost::_bi::list9<boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::arg<4>, boost::arg<5>, boost::arg<6>, boost::arg<7>, boost::arg<8>, boost::arg<9> > >; R = void; T0 = const boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >&; T1 = const boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >&; T2 = const boost::shared_ptr<const message_filters::NullType>&; T3 = const boost::shared_ptr<const message_filters::NullType>&; T4 = const boost::shared_ptr<const message_filters::NullType>&; T5 = const boost::shared_ptr<const message_filters::NullType>&; T6 = const boost::shared_ptr<const message_filters::NullType>&; T7 = const boost::shared_ptr<const message_filters::NullType>&; T8 = const boost::shared_ptr<const message_filters::NullType>&]’
/usr/include/boost/function/function_template.hpp:728:7:   required from ‘boost::function9<R, T1, T2, T3, T4, T5, T6, T7, T8, T9>::function9(Functor, typename boost::enable_if_c<boost::type_traits::ice_not<boost::is_integral<Functor>::value>::value, int>::type) [with Functor = boost::_bi::bind_t<boost::_bi::unspecified, boost::_bi::bind_t<boost::_bi::unspecified, void (darknet_ros::YoloObjectDetector::*)(const boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >&), boost::_bi::list3<boost::_bi::value<darknet_ros::YoloObjectDetector*>, boost::arg<1>, boost::arg<2> > >, boost::_bi::list9<boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::arg<4>, boost::arg<5>, boost::arg<6>, boost::arg<7>, boost::arg<8>, boost::arg<9> > >; R = void; T0 = const boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >&; T1 = const boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >&; T2 = const boost::shared_ptr<const message_filters::NullType>&; T3 = const boost::shared_ptr<const message_filters::NullType>&; T4 = const boost::shared_ptr<const message_filters::NullType>&; T5 = const boost::shared_ptr<const message_filters::NullType>&; T6 = const boost::shared_ptr<const message_filters::NullType>&; T7 = const boost::shared_ptr<const message_filters::NullType>&; T8 = const boost::shared_ptr<const message_filters::NullType>&; typename boost::enable_if_c<boost::type_traits::ice_not<boost::is_integral<Functor>::value>::value, int>::type = int]’
/usr/include/boost/function/function_template.hpp:1077:16:   required from ‘boost::function<R(T0, T1, T2, T3, T4, T5, T6, T7, T8)>::function(Functor, typename boost::enable_if_c<boost::type_traits::ice_not<boost::is_integral<Functor>::value>::value, int>::type) [with Functor = boost::_bi::bind_t<boost::_bi::unspecified, boost::_bi::bind_t<boost::_bi::unspecified, void (darknet_ros::YoloObjectDetector::*)(const boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >&), boost::_bi::list3<boost::_bi::value<darknet_ros::YoloObjectDetector*>, boost::arg<1>, boost::arg<2> > >, boost::_bi::list9<boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::arg<4>, boost::arg<5>, boost::arg<6>, boost::arg<7>, boost::arg<8>, boost::arg<9> > >; R = void; T0 = const boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >&; T1 = const boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >&; T2 = const boost::shared_ptr<const message_filters::NullType>&; T3 = const boost::shared_ptr<const message_filters::NullType>&; T4 = const boost::shared_ptr<const message_filters::NullType>&; T5 = const boost::shared_ptr<const message_filters::NullType>&; T6 = const boost::shared_ptr<const message_filters::NullType>&; T7 = const boost::shared_ptr<const message_filters::NullType>&; T8 = const boost::shared_ptr<const message_filters::NullType>&; typename boost::enable_if_c<boost::type_traits::ice_not<boost::is_integral<Functor>::value>::value, int>::type = int]’
/opt/ros/kinetic/include/message_filters/signal9.h:281:40:   required from ‘message_filters::Connection message_filters::Signal9<M0, M1, M2, M3, M4, M5, M6, M7, M8>::addCallback(C&) [with C = const boost::_bi::bind_t<boost::_bi::unspecified, void (darknet_ros::YoloObjectDetector::*)(const boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >&), boost::_bi::list3<boost::_bi::value<darknet_ros::YoloObjectDetector*>, boost::arg<1>, boost::arg<2> > >; M0 = sensor_msgs::Image_<std::allocator<void> >; M1 = sensor_msgs::Image_<std::allocator<void> >; M2 = message_filters::NullType; M3 = message_filters::NullType; M4 = message_filters::NullType; M5 = message_filters::NullType; M6 = message_filters::NullType; M7 = message_filters::NullType; M8 = message_filters::NullType]’
/opt/ros/kinetic/include/message_filters/synchronizer.h:310:40:   required from ‘message_filters::Connection message_filters::Synchronizer<Policy>::registerCallback(const C&) [with C = boost::_bi::bind_t<boost::_bi::unspecified, void (darknet_ros::YoloObjectDetector::*)(const boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >&), boost::_bi::list3<boost::_bi::value<darknet_ros::YoloObjectDetector*>, boost::arg<1>, boost::arg<2> > >; Policy = message_filters::sync_policies::ApproximateTime<sensor_msgs::Image_<std::allocator<void> >, sensor_msgs::Image_<std::allocator<void> > >]’
/home/cata/catkin_ws/src/darknet_ros/darknet_ros/src/YoloObjectDetector.cpp:155:86:   required from here
/usr/include/boost/function/function_template.hpp:159:11: error: no match for call to ‘(boost::_bi::bind_t<boost::_bi::unspecified, boost::_bi::bind_t<boost::_bi::unspecified, void (darknet_ros::YoloObjectDetector::*)(const boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >&), boost::_bi::list3<boost::_bi::value<darknet_ros::YoloObjectDetector*>, boost::arg<1>, boost::arg<2> > >, boost::_bi::list9<boost::arg<1>, boost::arg<2>, boost::arg<3>, boost::arg<4>, boost::arg<5>, boost::arg<6>, boost::arg<7>, boost::arg<8>, boost::arg<9> > >) (const boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >&, const boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >&, const boost::shared_ptr<const message_filters::NullType>&, const boost::shared_ptr<const message_filters::NullType>&, const boost::shared_ptr<const message_filters::NullType>&, const boost::shared_ptr<const message_filters::NullType>&, const boost::shared_ptr<const message_filters::NullType>&, const boost::shared_ptr<const message_filters::NullType>&, const boost::shared_ptr<const message_filters::NullType>&)’
           BOOST_FUNCTION_RETURN((*f)(BOOST_FUNCTION_ARGS));
           ^
darknet_ros/darknet_ros/CMakeFiles/darknet_ros_lib.dir/build.make:62: recipe for target 'darknet_ros/darknet_ros/CMakeFiles/darknet_ros_lib.dir/src/YoloObjectDetector.cpp.o' failed

[AlejoDiaz49]

It seems that is a Boost error, check that everything in the CmakeList.txt is fine, or maybe is something in this function that i dont really know where is written

void (darknet_ros::YoloObjectDetector::*)(const boost::shared_ptr<const sensor_msgs::Image_<std::allocator<void> > >&)

[catalin15dcm]

Do you get the 3d coordinates on your PC? I'm trying and still trying but got no results. Can you tell me how did you do it or show me a little of your code?

[AlejoDiaz49]

I upload my version to this repository. For me it works fine in gazebo simulation. I just did it the way I told you with the camera equations.

In the next hours I will describe everything with more details but it should be enough for you proyect

[catalin15dcm]

Firstly i would like to thank yo so much for the effort. Secondly i would like to ask you some questions about this error and that function in the last if. The yolo node will detect the object in one image and after that i got this error. What is wrong with that if and what does it do?

Thread 6 "darknet_ros" received signal SIGSEGV, Segmentation fault.
[Switching to Thread 0x7fffbb6cd700 (LWP 23490)]
0x00007ffff7ae33a9 in darknet_ros::YoloObjectDetector::Coordinates (
    this=this@entry=0x7fffffffc540, ObjID=ObjID@entry=0, xmin=xmin@entry=37, 
    ymin=ymin@entry=90, xmax=xmax@entry=498, ymax=ymax@entry=480)
---Type <return> to continue, or q <return> to quit---return
    at /home/cata/catkin_ws/src/darknet_ros/darknet_ros/src/YoloObjectDetector.cpp:799
799             if (Value==Value && ((int)Binaria1.at<uchar>(j,i))!=0)
(gdb) 

[AlejoDiaz49]

The code I put was thought for a specific problem so there are a few lines that you dont need. Since the objects I was detecting were only red, blue, green, yellow or black, I add a color filtering to not consider the depth values in the BoundingBox that are not from the object and check for false positive in the YOLO result. This is done from this line to this line. Then here you have a loop that goes through every pixel of the BoundingBox

for(int i=xmin; i<=xmax; i++)
    for(int j=ymin; j<=ymax; j++)
    {
        ...
    }

Inside the loop then I get the depth values from those pixels that are in the color image the color I want. Binaria is an image that has a 0 where the color isn't the one I want and 255 when is the color I want

Value=(float)DepthImageCopy_.at<float>(j,i); 
if (Value==Value && ((int)Binaria1.at<uchar>(j,i))!=0)
{
    GrayValue+=Value; 
    Ind++;
}
GrayValue=GrayValue/Ind;

So GrayValue is the mean value of all those depth values. The error you have is probably due to all this. You should erase it and just use

Value=(float)DepthImageCopy_.at<float>(Ycenter,Xcenter)

[Imraj]

@AlejoDiaz49 I couldn't get this to work I'm getting a -nan for X,Y,Z coordinate. My Coordinates function is

 void YoloObjectDetector::Coordinates(int ObjID, int xmin, int ymin, int xmax, int ymax)
  {
      int U = ((xmax-xmin)+xmin);
      int V = ((ymax-ymin)+ymin);
      int Xcenter = ((xmax-xmin)/2) + x_min;
      int Ycenter = ((ymax-ymin)/2) + y_min;

      float Value=(float)DepthImageCopy_.at<float>(Ycenter,Xcenter)

      Invalid= true; 
      if (Value!=0)
      {
         Invalid= false;
         Z= Value;                                //Metros
         X=((U-320.5)*Z)/554.254691191187;           //X=((U-Cx)*Z)/fx
         Y=((V-240.5)*Z)/554.254691191187;           //Y=((V-Cy)*Z)/fy
         ROS_INFO("X %f, Y %f, Z %f ,Invalid %d ", X, Y, Z, Invalid);
      }
   }`

[astronaut71]

Hi. I really like to get 3D Bounding Box from YOLO and use for Collision checking in Gazebo and Moveit. Can I use your code for that purpose?

[Sagid28]

Hi, I'm using your code and i find it very useful.

Yolo is providing just the bounding box coordinates and i'm looking forward to the object coordinates in 3d. I know that a kinect camera provide some depth images and i'm asking if there is any chance to obtain the object coordinates. So, How can i realize that? is there any idea?

can I get the code? I am new in this type of project. I want to detect object by using azure kinect and measure the distance. please help me.

[astronaut71]

Is it possible to get the orientation of the detected objects with this code or any help in modified the code in order to get the orientation?

[Algabri]

Another option, is using the Point Cloud topic of the Kinect. With the coordinates of the color image (u,v) you can extract the real coordinates (x,y,z) from the Point Cloud Could you put a reference paper for this method to show us mathematical equations?

[thejeshk]

Hi @AlejoDiaz49 @catalin15dcm

I am using YOLO for 2d object detection in a simulator (similar to Gazebo) It is Called AirSim. AirSim provides ROS wrapper so I am using darknet_ros.

Could you pease help me to get the distance from the obstacle infront.

Thank you TK