5 Replies Latest reply on Jun 3, 2017 8:18 AM by Thomas Kirven

    Locate nearest object with R200

    Thomas Kirven

      Hello,

       

      I am hoping to use the R200 camera to determine the position of the nearest object in the camera frame. In theory this should be easy using the "points" data, and this is what I've tried;

       

      float dist,min_dist;

      min_dist = 5;

       

      float px,py,pz;

       

      auto points = reinterpret_cast<const rs::float3 *>(dev.get_frame_data(rs::stream::points));

       

      for(int y=0; y<depth_intrin.height; ++y)

              {

                  for(int x=0; x<depth_intrin.width; ++x)

                  {

                      if(points->z) //if(uint16_t d = *depth++)

                      {

                          dist = sqrt(points->x*points->x+points->y*points->y+points->z*points->z);

                          if (dist<min_dist) {

                               min_dist = dist;

                               px = points->x;

                               py = points->y;

                               pz = points->z;

                          }

                      }

                      ++points;

                  }

              }

          }

      printf("object position = %f, %f, %f \n",px,py,pz);

       

      However plotting px vs time, py vs time, pz vs time shows very noisy results even when the camera FOV is empty except for a single visible objects at a fixed position.

      For example if I put an object 1 meter in from of the camera .2 meters up and .2 meters to the left and left it stationary, I would expect the time vs px,py,pz graphs to reflect this clearly, but they do not.

       

      Is there something obvious I am doing wrong or expecting the camera to do something it's not capable of?

       

      Thanks,

       

      Thomas