Problems with kinect depth information (ROS V-REP)

[gendibal784]

Hey there!

I'm trying to get and use depth information through ROS using a V-REP scene and your last ROS plugin, ROS interface.

At the beginning, I tried to follow this post (viewtopic.php?f=9&t=5916) because I had similar wrongs but it seems that the codes of the post doesn't work for me and I can't view depth image through RVIZ or other ROS tools. It also doesn't work to generate pointcloud and laser scan topic with this depth information.

This is the code I'm using:

Code: Select all

function sysCall_init() 
    activeVisionSensor=sim.getObjectHandle('kinect_depth')
    depthView=sim.floatingViewAdd(0.9,0.9,0.2,0.2,0)
    sim.adjustView(depthView,activeVisionSensor,64)

    colorCam=sim.getObjectHandle('kinect_rgb')
    colorView=sim.floatingViewAdd(0.69,0.9,0.2,0.2,0)
    sim.adjustView(colorView,colorCam,64)
    pub=simROS.advertise('/image', 'sensor_msgs/Image')
    simROS.publisherTreatUInt8ArrayAsString(pub)
end

if (sim_call_type==sim_childscriptcall_sensing) then
	
    transform={getTransformStamped(activeVisionSensor,'camera_depth_frame', -1,'map')}
    simROS.sendTransforms(transform)
    -- Publish the image of the active vision sensor:
    local data=simGetVisionSensorDepthBuffer(activeVisionSensor+sim_handleflag_codedstring)
    [[local res,nearClippingPlane=simGetObjectFloatParameter(activeVisionSensor,sim_visionfloatparam_near_clipping)
    local res,farClippingPlane=simGetObjectFloatParameter(activeVisionSensor,sim_visionfloatparam_far_clipping)
    nearClippingPlane=nearClippingPlane*1000 -- we want mm
    farClippingPlane=farClippingPlane*1000 -- we want mm
    data=sim.transformBuffer(data,sim_buffer_float,farClippingPlane-nearClippingPlane,nearClippingPlane,sim_buffer_uint16)]]
    local res=sim.getVisionSensorResolution(activeVisionSensor)
    d={}
    d_depth['header']={seq=0,stamp=simGetSystemTime(), frame_id="camera_depth_frame"}
    d['height']=res[2]
    d['width']=res[1]
    d['encoding']='16UC1' 
    d['is_bigendian']=1
    d['step']=res[1]*2
    d['data']=data
    simROS.publish(pub,d)
end
function getTransformStamped(obj,n,relTo,relN)

    t=sim.getSimulationTime()
    --t=simROS.getTime()
    --t=sim.getSystemTime()    
    p=sim.getObjectPosition(obj,relTo)
    o=sim.getObjectQuaternion(obj,relTo)
    return {
        header={
            stamp=t,
            frame_id=relN
        },
        child_frame_id=n,
        transform={
            translation={x=p[1],y=p[2],z=p[3]},
            rotation={x=o[1],y=o[2],z=o[3],w=o[4]}
        }
    }
end

On the other hand, as I couldn't publish the depth information by this way, I've started to try to publish it through a pointcloud2 following this another post (viewtopic.php?t=7450). As they said, i created and built a ROS node to combine RGB information with depth information into a pointcloud2. This time i had better results but they weren't enough.
this is the result: https://ibb.co/HTkRRjX
this is the scene: https://ibb.co/K9mmdmW

As it can be seen on the image, the pointcloud is strange. It seems like nearest object are detected as the farest and the gain of the detector isn't arithmetic.

the code:

Code: Select all

if (sim_call_type==sim.syscb_init) then
    
    depth_pub = simROS.advertise('/vrep/depth', 'std_msgs/Float32MultiArray')
    rgb_pub = simROS.advertise('/vrep/rgb', 'sensor_msgs/Image')
    simROS.publisherTreatUInt8ArrayAsString(rgb_pub)
    depthCam=sim.getObjectHandle('kinect_depth')
    colorCam=sim.getObjectHandle('kinect_rgb')
    colorView=sim.floatingViewAdd(0.69,0.9,0.2,0.2,0)
    sim.adjustView(colorView,colorCam,64)
end

if (sim_call_type==sim.syscb_actuation) then
    sim.handleVisionSensor(depthCam)
    transform={getTransformStamped(depthCam,'camera_depth_frame', -1,'map')}
    simROS.sendTransforms(transform)
    

end

if (sim_call_type==sim.syscb_sensing) then
    sim.handleVisionSensor(depthCam)
    tb_depth={}
    tb_depth['data']=sim.getVisionSensorDepthBuffer(depthCam)
    simROS.publish(depth_pub, tb_depth)

    t=simROS.getTime()
    image=sim.getVisionSensorImage(colorCam,0,0,0,0,1)
    tb_rgb={}
    tb_rgb['header']={stamp=t, frame_id="camera_depth_frame"}
    tb_rgb['height']=480
    tb_rgb['width']=640
    tb_rgb['encoding']='rgb8'
    tb_rgb['step']=640 * 3
    tb_rgb['data']=image
    simROS.publish(rgb_pub, tb_rgb)
end


if (sim_call_type==sim.syscb_cleanup) then
    simROS.shutdownPublisher(depth_pub)
    simROS.shutdownPublisher(rgb_pub)
end

function getTransformStamped(obj,n,relTo,relN)

    t=sim.getSimulationTime()
    --t=simROS.getTime()
    --t=sim.getSystemTime()    
    p=sim.getObjectPosition(obj,relTo)
    o=sim.getObjectQuaternion(obj,relTo)
    return {
        header={
            stamp=t,
            frame_id=relN
        },
        child_frame_id=n,
        transform={
            translation={x=p[1],y=p[2],z=p[3]},
            rotation={x=o[1],y=o[2],z=o[3],w=o[4]}
        }
    }
end

and ROS converter:

Code: Select all

const float v_res=640;
const float u_res=480;
const float far_clip=3.5;
const float near_clip=0.1;
const float focal_length=69.40*3.141516/180;


Cloud::Ptr img2cloud(cv::Mat rgb, MultiArray depth_msg)
{
  std::vector<float> depth = depth_msg.data;

  Point punto;
  if (rgb.cols != v_res || rgb.rows != u_res || depth.size() != u_res * v_res) {
    std::cout << rgb.cols << ", " << rgb.rows <<std::endl;
    std::cout << depth.size() << std::endl;
    std::cout << "rgb and depth image size mismatch" << '\n';
    exit(0);
  }
  Cloud::Ptr cloud(new Cloud);
  for (size_t u = 0; u < u_res; u++) {
    for (size_t v = 0; v < v_res; v++) {
      auto rgb_ints = rgb.at<cv::Vec3b>(u, v);
      int w = v_res * u + v;
      float z = far_clip* depth[w] + near_clip;
      float y = z*sin((v-v_res/2)*focal_length/v_res);
      float x = z*sin((u-u_res/2)*focal_length/u_res);
      Point p;
      p.x = x, p.y = y, p.z = z;
      p.r = (int) rgb_ints[0]; p.g = (int) rgb_ints[1]; p.b = (int) rgb_ints[2];
      cloud->points.push_back(p);
    }
  }
  cloud->header.frame_id = "camera_depth_frame";
  cloud->width = cloud->points.size();
  cloud->height = 1;

  return cloud;
}

Thanks!

[coppelia]

Hello,

sim.getVisionSensorDepthBuffer returns values between 0 (on the near clipping plane), and 1 (on the far clipping plane).It doesn't return a distance.

If you want a distance, then do:

Code: Select all

distance=nearClippingPlane+depthValue*(farClippingPlane-nearClippingPlane)

You can get the clipping planes with:

Code: Select all

local res,nearClippingPlane=sim.getObjectFloatParameter(visionSensorHandle,sim.visionfloatparam_near_clipping)
local res,farClippingPlane=sim.getObjectFloatParameter(visionSensorHandle,sim.visionfloatparam_far_clipping)

Cheers

[gendibal784]

Thanks for the reply!

I know, I saw how it works in other post and that's why on my ROS code i used:

Code: Select all

float z = far_clip*depth[w] + near_clip;

I mean, i translate this info into distance on the ROS code, not in V-REP

By the way, I was testing this scenes, trying to find the reason of why it's working as it does, and I finally got several decent results but they had some weird behaviors.

• Firstly, I realize when I put the camera far from a wall that Depth buffer returns lower values than when the camera is nearer (I'm talking about the center point of the image). It seems that, in my case, Depth buffer returns values between 1 (on the near clipping plane), and 0 (on the far clipping plane) and that's why my pointcloud was inverted, like on one of the images i posted before. It seems fixed with this code:

Code: Select all

float z = far_clip*(1 - depth[w]) + near_clip;

• Besides, depth info progression works better with the code above but it seems less arithmetical when objects are nearer so when you watch the pointcloud from one of their sides, you can see that the floor isn't plane and its progresion is a bit parabolic.

Thanks!

extra: Following your post, I modify how I get the distance on my code and now I use this code, getting more reliable distances (thanks!!), but the two problems above still happen

Code: Select all

float z = (far_clip-near_clip)*(1 - depth[w]) + near_clip;

[coppelia]

That is very strange. If I add a vision sensor to a scene with following code:

Code: Select all

function sysCall_init()
    h=sim.getObjectAssociatedWithScript(sim.handle_self)
end

function sysCall_sensing()
    local d=sim.getVisionSensorDepthBuffer(h,15,15,1,1)
    print(d[1])
end

I get values closer to 0, when close to an object, and values closer to 1, when far from that object. Can you confirm that?

[gendibal784]

Well, I've just tested this behavior using your code and I finally realize why it's working as it does. The reason is that I was using the filter work image to output depth image on the depth camera instead of using the general filter work image to output image. So, it makes me think and I have some questions, How does work image to output depth image work? Why does it have this behavior?

When I was testing with work image to output depth image, i got that the function never gave me depth values between 0 and 1, for example, my camera info was that my near clipping plane was 0.8 m and my far clipping plane 4 m, but the depth value was between 0.33 more or less (4 m) and 0.66 (0.8 m). It seems like depth info comes from 1, when an object is on the origin of the camera, to 0, which is the infinite, (my first problem),and, besides, the gain of the depth info mustn't be arithmetical, which is the second problem I saw before. it's just my hypothesis but I could try more things and post my results.

On the other hand, with the another filter, everything works fine except one thing and it's that I couldn't flip horizontally my depth image. I use this filters:

1. original depth image to work image
2. Flip work image horizontally
3. Intensity scale work image
4. work image to output image

I think I could flip by c++ programming easily but I would like to do that on the V-REP environment. Maybe, should I do this?

1. original image to work image
2. Flip work image horizontally
3. work image to output image
4. original depth image to work image
5. Intensity scale work image
6. work image to output image

Thanks for your reply!

[coppelia]

The work image is a RGB image. The vision sensor image is also a RGB image. The vision sensor depth map is however an array of floats. So you will always lose some information when sending the depth map to the work image, or when sending the work image to the depth map.
You will have to flip the depth buffer on the client side.

Cheers

[gendibal784]

OK thanks!!! I'll test that.

By the way, about the filter work image to output depth image, How does it really work? Where can I find information about that? Is my hypothesis of my last post right or wrong? I was searching something about that on your user manual but I couldn't find anything. I suppose It should be used to get depth info but my experience with that makes me think I could be wrong.