Realsense-ros: Intrinsic Camera Parameters - D435

[jlysnantel]

Is there a topic that publish the Intrinsic Camera Parameters so I can use the depth data to calculate the coordinate (x,y,z) of a pixel. I use the D435. I want to calculate the coordinates as stated in https://github.com/IntelRealSense/librealsense/wiki/Projection-in-RealSense-SDK-2.0?fbclid=IwAR3gogVZe824YUps88Dzp02AN_XzEm1BDb0UbmzfoYvn1qDFb7KzbIz9twU#intrinsic-camera-parameters for the deprojection.

[jlysnantel]

I now understand that they are under /camera/nameOfYourTypeOfTopicCam/camera_info.

You can see http://docs.ros.org/melodic/api/sensor_msgs/html/msg/CameraInfo.html To have better information

[jinfagang]

@jlysnantel Do you solved your problem? How to subscribe topic? which topic do you using ? does there any snippets in c++ to do this?

[jlysnantel]

My tests have not been done yet. But what I have now is the rgb image in the topic /camera/color/image_raw. I have the depth of a pixel in the topic /camera/aligned_depth_to_color/image_raw and I have the intrinsic parameter of the camera in the topic /camera/aligned_depth_to_color/camera_info.

The topic camera_info return an object of type sensor_msgs/CameraInfo. As said in the documentation:

**Intrinsic camera matrix for the raw (distorted) images.**
     [fx  0 cx]
 K = [ 0 fy cy]
     [ 0  0  1]
 Projects 3D points in the camera coordinate frame to 2D pixel
 coordinates using the focal lengths (fx, fy) and principal point
(cx, cy).

Whit all these information we have everything to calculate the deprojection of a pixel.

/* Given pixel coordinates and depth in an image with no distortion or inverse distortion coefficients, compute the corresponding point in 3D space relative to the same camera */
static void rs2_deproject_pixel_to_point(float point[3], const struct rs2_intrinsics * intrin, const float pixel[2], float depth)
{
    assert(intrin->model != RS2_DISTORTION_MODIFIED_BROWN_CONRADY); // Cannot deproject from a forward-distorted image
    assert(intrin->model != RS2_DISTORTION_FTHETA); // Cannot deproject to an ftheta image
    //assert(intrin->model != RS2_DISTORTION_BROWN_CONRADY); // Cannot deproject to an brown conrady model

    float x = (pixel[0] - intrin->ppx) / intrin->fx;
    float y = (pixel[1] - intrin->ppy) / intrin->fy;
    if(intrin->model == RS2_DISTORTION_INVERSE_BROWN_CONRADY)
    {
        float r2  = x*x + y*y;
        float f = 1 + intrin->coeffs[0]*r2 + intrin->coeffs[1]*r2*r2 + intrin->coeffs[4]*r2*r2*r2;
        float ux = x*f + 2*intrin->coeffs[2]*x*y + intrin->coeffs[3]*(r2 + 2*x*x);
        float uy = y*f + 2*intrin->coeffs[3]*x*y + intrin->coeffs[2]*(r2 + 2*y*y);
        x = ux;
        y = uy;
    }
    point[0] = depth * x;
    point[1] = depth * y;
    point[2] = depth;
}

I find these information there : https://github.com/IntelRealSense/librealsense/wiki/Projection-in-RealSense-SDK-2.0

[ravijo]

Hi @jinfagang

I made a ROS package to do the same. Please check it out here.

Cheers!

[AndreV84]

is it possible to get the camera_intrinsic.json file for d435i from anywhere?

[doronhi]

I'm not sure what you mean by json file but the intrinsics values are available using the command:
rs-enumerate-devices -c

[AndreV84]

@doronhi Thank you for following up!
I am trying to install open3D
the latter seem to require camera file provided
https://support.intelrealsense.com/hc/en-us/community/posts/360039393953-Highlighting-the-JetScan-3D-scanner-build-project-for-Jetson-Nano
https://github.com/devshank3/JetScan#usage