Manually align the depth image with the color image.

[shiyt0313]

Required Info
Camera Model	D435
Operating System & Version	Raspbian
Platform	Raspberry Pi
Language	python

I tried to use RealSense to obtain depth and RGB images on a Raspberry Pi 4B. When I use align() in librealsense, the frame rate drops to 3 fps, but I need at least 10 fps of data. Therefore, I want to manually save the unaligned depth and RGB images.
How can I manually align them (If there is any interface I can use in librealsense)?
Alternatively, is there any other effective way to obtain aligned data with at least 10 fps besides manual alignment?

[MartyG-RealSense]

Hi @shiyt0313 A RealSense user at #11758 with a similar development board called Orange Pi also experienced almost 3 FPS when aligning with Python code.

The approach that the user ended up applying - described at #11758 (comment) - was to use the instruction rs2_project_color_pixel_to_depth_pixel to convert a single coordinate from a color pixel to a depth pixel. This is more processing-efficient than aligning the entire image if the reason that you are aligning is to obtain a 3D real-world coordinate.

If you require an aligned image and do not mind generating a depth-color aligned 3D pointcloud image then pc.calculate() is an alternative to align_to. A Python example of this instruction is at #4612 (comment)

It is worth bearing in mind that when RealSense cameras are used with Raspberry Pi, you can obtain individual depth and color streams but more processing intensive operations such as depth-color alignment or pointclouds may have problems.

[shiyt0313]

@MartyG-RealSense Thanks for your advice! However, neither of these two methods apply to my situation.

• For method one, I need the depth of each pixel in the image for further processing, so I cannot only use rs2_project_color_pixel_to_depth_pixel on specific pixels.

• For method two, after testing, I found that the method of generating point clouds still has a low frame rate issue (only 3 fps).

Is there any other possible solution?

[MartyG-RealSense]

If you use pc.calculate with map_to() to map depth and color together then you could obtain depth for all the pixels by storing the data as vertices in a numpy array and then retrieve the vertices as x, y and z (depth) values. An example of a script for doing this in Python is at #4612 (comment)

[shiyt0313]

As I mentioned above, my frame rate drops to 3fps when using map_to() and storing the point cloud, which is same as align_to().

Are there any other efficient alignment methods, or manual alignment solutions?

[MartyG-RealSense]

map_to() and align_to() are the two main methods of alignment. Other than rs2_project_color_pixel_to_depth_pixel, there are no alternatives unfortunately.

Below I modified a depth to color Python align_to script to align to the monochrome infrared image instead of the RGB color image. As depth and infrared originate from the same sensor, if FPS improves when this script is run then this would indicate that it is the RGB color that is likely causing the slowdown.

import numpy as np
import cv2
import pyrealsense2 as rs

#Colorizer for the depth frame
colorizer = rs.colorizer()

#Initialize pipeline
pipe = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 848, 480,, rs.format.z16, 15)
config.enable_stream(rs.stream.infrared, 848, 480, rs.format.y8, 15)
align_to = rs.stream.infrared
align = rs.align(align_to)
       
#Start pipeline
profile = pipe.start(config)

while True:
    frames = pipe.wait_for_frames()
    
    #alignment
    frames = align.process(frames)
    
    #Get infrared and depth frames for display
    infrared_frame = frames.get_infrared_frame()
    depth_frame = frames.get_depth_frame()
    
    infrared_frame = np.asanyarray(infrared_frame.get_data())
    depth_frame = np.asanyarray(colorizer.colorize(depth_frame).get_data())
    
    camera_images = np.vstack((infrared_frame, depth_frame))
    
    #Display images
    cv2.imshow("RealSense", camera_images)            
    key = cv2.waitKey(1)
    if key & 0xFF == ord('q'): 
        cv2.destroyWindow("RealSense")
        pipe.stop()  
        break

[shiyt0313]

I'm really appreciate for your suggestions. I tried the method you proposed and found that it is impossible to ensure a frame rate of 10 fps while aligning RGB and depth in real time. Therefore, I am planning to try two other approaches.

• Use different frame rates for color_frame and depth_frame. (backup plan)
  Considering that objects in the RGB images are usually static for short periods, I used different sampling rates for the color_frame(15 fps) and depth frame(3 fps) and then tracked the object in the RGB image to calculate depth.

• Manual align depth_frame to color_frame
  I wanna record the intrinsic and extrinsic parameters of the color_frame and depth_frame, and then manually align the RGB and depth images. Is there any implementation in librealsense that I can refer to for this? Is there a way to directly save the original frame data so I can offline call align_to() align it.

[MartyG-RealSense]

Something else you could try is to disable an RGB option called auto-exposure priority. When depth and RGB are both enabled, if auto-exposure is enabled and auto-exposure priority is disabled then the SDK will try to force both streams to maintain a constant FPS rate instead of permitting FPS to vary. A line of Python code that can be placed after the pipe start line to disable auto-exposure priority is below.

profile = pipe.start(config)
p.get_device().query_sensors()[1].set_option(rs.option.auto_exposure_priority, 0.0)

[shiyt0313]

@MartyG-RealSense We tried to set auto-exposure priority, but it didn't work on Raspi. So we finally used different frame rates. Thank you very much for your suggestions and methods. If you have no other recommendations, I will close this issue in the next comment.

To make it easier for someone else who encounters the same problem with me in the future, I have put all possible solutions here:

1. If not considering the depth of each pixel, use rs2_project_color_pixel_to_depth_pixel to get the depth corresponding to a single RGB pixel. [Question] How to use rs2_project_color_pixel_to_depth_pixel in python to convert 1 point from color coordinate into depth coordinate #5603 (comment)

# There values are needed to calculate the mapping
depth_scale = profile.get_device().first_depth_sensor().get_depth_scale()
depth_min = 0.11 #meter
depth_max = 1.0 #meter

depth_intrin = profile.get_stream(rs.stream.depth).as_video_stream_profile().get_intrinsics()
color_intrin = profile.get_stream(rs.stream.color).as_video_stream_profile().get_intrinsics()

depth_to_color_extrin =  profile.get_stream(rs.stream.depth).as_video_stream_profile().get_extrinsics_to( profile.get_stream(rs.stream.color))
color_to_depth_extrin =  profile.get_stream(rs.stream.color).as_video_stream_profile().get_extrinsics_to( profile.get_stream(rs.stream.depth))

color_points = [
    [400.0, 150.0],
    [560.0, 150.0],
    [560.0, 260.0],
    [400.0, 260.0]
]
for color_point in color_points:
   depth_point_ = rs.rs2_project_color_pixel_to_depth_pixel(
                depth_frame.get_data(), depth_scale,
                depth_min, depth_max,
                depth_intrin, color_intrin, depth_to_color_extrin, color_to_depth_extrin, color_point)

2. Obtain the depth corresponding to pixels using point clouds. Calculate objects volume with realsense D435i python #4612 (comment)

import pyrealsense2 as rs
import numpy as np
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
pc = rs.pointcloud()
pipeline.start(config)

i = 0
while i<5:

    frames = pipeline.wait_for_frames()
    depth_frame = frames.get_depth_frame()
    print(depth_frame)
    color_frame = frames.get_color_frame()
    print(color_frame)
    i = i+1
    pc.map_to(color_frame)
    points = pc.calculate(depth_frame)
    vtx = np.asanyarray(points.get_vertices())
    tex = np.asanyarray(points.get_texture_coordinates())
    print(type(points), points)
    print(type(vtx), vtx.shape, vtx)
    print(type(tex), tex.shape, tex)
pipeline.stop()

3. Adjust auto-exposure priority to fix the frame rate. Manually align the depth image with the color image. #12928 (comment)

profile = pipe.start(config)
p.get_device().query_sensors()[1].set_option(rs.option.auto_exposure_priority, 0.0)

4. Use different frame rates for color_frame and depth_frame.
   For example:

count = 4
i = 0
while i < 100:
    count  = count + 1
    frames = pipeline.wait_for_frames()
    color_frame = frames.get_color_frame()
    print(color_frame)
    if count == 5:
        count = 0
        aligen_to = rs.stream.color
        align = rs.align(aligen_to)
        aligned_frames = align.process(frames)
        depth_frame = aligned_frames.get_depth_frame()
        print(depth_frame)

    i = i+1

[MartyG-RealSense]

Thanks so much for sharing your solutions. I do not have further recommendations.