Using rs2_project_color_pixel_to_depth_pixel with ROS wrapper

[jacklia]

I would like to the rs2_project_color_pixel_to_depth_pixel function from the librealsense library to convert specific points from the RGB frame to the depth frame instead of having to align the entire image like aligned_depth_to_color does. The problem is that rs2_project_color_pixel_to_depth_pixel requires a librealsense BufData object that cannot be instantiated since there's no constructor. This object is acquired using rs.pipeline().wait_for_frames().get_depth_frame().get_data() when using the python driver to access the camera.

Basically my question is – is there a way to register individual points between the RGB and the depth frame while using the ROS wrapper? Thanks!

[MartyG-RealSense]

Hi @jacklia There is a discussion about BufData and its lack of a default constructor at the link below. The RealSense user in that case had also wished to use rs2_project_color_pixel_to_depth_pixel and been unable to.

IntelRealSense/librealsense#8394

In another case though, someone did post a Python script for using rs2_project_color_pixel_to_depth_pixel

IntelRealSense/librealsense#5603 (comment)

In ROS you can publish additional topics that are aligned to the depth image - such as /camera/aligned_depth_to_color/image_raw - by adding the command align_depth:=true to the end of your roslaunch instruction. Off the top of my head, I can't think of an equivalent instruction to rs2_project_color_pixel_to_depth_pixel in ROS for getting details from a single specific aligned coordinate instead of aligning the whole image though.

[jacklia]

Thanks for the comment Marty, I've already checked out those two links. Would you know if there's a way to use rs2_deproject_pixel_to_point with /camera/aligned_depth_to_color/image_raw? From what i can see, the output if you directly put in the image from that topic to that function is incorrect, which I assume is because rs2_deproject_pixel_to_point requires camera intrinsics which only apply to the original depth stream and not the transformed one. I may be incorrect though.

[jacklia]

An additional question: rs.pipeline().wait_for_frames().get_depth_frame().get_data() seems to return data in 16-bit, but does "/camera/depth/image_rect_raw" return data in 8-bit or 16-bit? It seems that the Image message from sensor_msgs.msg returns 8-bit data, so is does the ROS topic return 8-bit instead of 16-bit?

[MartyG-RealSense]

In regard to the first question, I wonder if the two links below about a possible solution that uses /camera/aligned_depth_to_color/image_raw to get the 3D position of a pixel would be suitable for your needs.

#1086 (comment)

#1086 (comment)

For the second question about 16-bit / 8-bit, the discussion in the link below may be helpful.

#1524

[MartyG-RealSense]

Hi @jacklia Do you require further assistance with this case, please? Thanks!

[jacklia]

I do not for now – for anyone else who is curious, I ended up modifying rs2_project_color_pixel_to_depth_pixel (specifically line 213 of http://docs.ros.org/en/kinetic/api/librealsense2/html/rsutil_8h_source.html#l00186) to take in an actual depth from the CvBridged-ROS image as opposed to multiplying the depth scale by the raw depth value to get depth.

Thanks @MartyG-RealSense!

[MartyG-RealSense]

Thanks very much @jacklia for the update and for sharing your solution!

I will close the case as you do not require further assistance for now. Thanks again!

[joernnilsson]

For anyone stumbling over this thread looking for a solution, here's a python port of rs2_project_color_pixel_to_depth_pixel and a couple other util functions that were needed. The port assumes that data is a ROS sensor_msgs/Image (https://docs.ros2.org/latest/api/sensor_msgs/msg/Image.html), but can easily be converted to other formats.

I have't verified this thoroughly, but it works on my setup. Test it yourself 👍

    def ported_adjust_2D_point_to_boundary(self, p, width, height):
        if (p[0] < 0):
            p[0] = 0
        if (p[0] > width):
            p[0] = float(width)
        if (p[1] < 0):
            p[1] = 0
        if (p[1] > height):
            p[1] = float(height)
        return p

    def ported_next_pixel_in_line(self, curr, start, end_p):
        line_slope = (end_p[1] - start[1]) / (end_p[0] - start[0])
        if (abs(end_p[0] - curr[0]) > abs(end_p[1] - curr[1])):
            curr[0] = curr[0] + 1 if end_p[0] > curr[0] else curr[0] - 1
            curr[1] = end_p[1] - line_slope * (end_p[0] - curr[0])
        else:
            curr[1] = curr[1] + 1 if end_p[1] > curr[1] else curr[1] - 1
            curr[0] = end_p[0] - ((end_p[1] + curr[1]) / line_slope)
    
    def ported_is_pixel_in_line(self, curr, start, end_p):
        return ((end_p[0] >= start[0] and end_p[0] >= curr[0] and curr[0] >= start[0]) or (end_p[0] <= start[0] and end_p[0] <= curr[0] and curr[0] <= start[0])) and \
            ((end_p[1] >= start[1] and end_p[1] >= curr[1] and curr[1] >= start[1]) or (end_p[1] <= start[1] and end_p[1] <= curr[1] and curr[1] <= start[1]))

    def ported_rs2_project_color_pixel_to_depth_pixel(self,
                                                        data,
                                                        depth_scale,
                                                        depth_min,
                                                        depth_max,
                                                        depth_intrin,
                                                        color_intrin,
                                                        color_to_depth,
                                                        depth_to_color,
                                                        from_pixel
                                                    ):
        
        # Return value
        to_pixel = [0, 0]

        # Find line start pixel
        min_point = rs.rs2_deproject_pixel_to_point(color_intrin, from_pixel, depth_min)
        min_transformed_point = rs.rs2_transform_point_to_point(color_to_depth, min_point)
        start_pixel = rs.rs2_project_point_to_pixel(depth_intrin, min_transformed_point)

        # Find line end depth pixel
        max_point = rs.rs2_deproject_pixel_to_point(color_intrin, from_pixel, depth_max)
        max_transformed_point = rs.rs2_transform_point_to_point(color_to_depth, max_point)
        end_pixel = rs.rs2_project_point_to_pixel(depth_intrin, max_transformed_point)
        end_pixel = self.ported_adjust_2D_point_to_boundary(end_pixel, depth_intrin.width, depth_intrin.height)

        # search along line for the depth pixel that it's projected pixel is the closest to the input pixel
        min_dist = -1.0
        
        p = [start_pixel[0], start_pixel[1]]
        while self.ported_is_pixel_in_line(p, start_pixel, end_pixel):
        
            # This part assumes data is a ROS sensor_msgs/msg/Image
            x = int(p[0])
            y = int(p[1])
            step = data.step
            idx = y*step + x*2
            byte_values = data.data[idx:idx+2]
            int_value = int.from_bytes(byte_values, "big" if data.is_bigendian else "little")

            depth = depth_scale * int_value
            if (depth == 0):
                self.ported_next_pixel_in_line(p, start_pixel, end_pixel)
                continue

            point = rs.rs2_deproject_pixel_to_point(depth_intrin, p, depth)
            transformed_point = rs.rs2_transform_point_to_point(depth_to_color, point)
            projected_pixel = rs.rs2_project_point_to_pixel(color_intrin, transformed_point)

            new_dist = (float)(math.pow((projected_pixel[1] - from_pixel[1]), 2) + math.pow((projected_pixel[0] - from_pixel[0]), 2))
            if new_dist < min_dist or min_dist < 0:

                min_dist = new_dist
                to_pixel[0] = p[0]
                to_pixel[1] = p[1]
  
            self.ported_next_pixel_in_line(p, start_pixel, end_pixel)

        return to_pixel

[MartyG-RealSense]

Thanks so much @joernnilsson for sharing the solution that worked for you!