Get aligned color and depth with python

[kjacks21]

There are multiple issues on this but I haven't found one that resolves my issue. I originally posted this to #4306 but realized it was a thread about the matlab wrapper.

When I align the color and depth frames and try to access both, I always get the following error as a result of running the code below (assuming the function is properly called):

Error

unnecessary trace...
depth_image = np.asanyarray(aligned_depth_frame.get_data())
RuntimeError: null pointer passed for argument "frame_ref"

Code

import argparse
import pyrealsense2 as rs
import numpy as np
import cv2
import os


def bag2mp4(bag_fname, color_mp4_fname, depth_mp4_fname):

    config = rs.config()
    pipeline = rs.pipeline()

    # make it so the stream does not continue looping
    rs.config.enable_device_from_file(config, bag_fname, repeat_playback=False)
    config.enable_stream(rs.stream.color)
    profile = pipeline.start(config)
    # this makes it so no frames are dropped while writing video
    playback = profile.get_device().as_playback()
    playback.set_real_time(False)

    align = rs.align(rs.stream.color)

    i = 0
    while True:

        # when stream is finished, RuntimeError is raised, hence this
        # exception block to capture this
        try:
            frameset = pipeline.wait_for_frames(timeout_ms=100)
        except (RuntimeError):
            print('frame count', i-1)
            break

        frameset = align.process(frameset)

        aligned_color_frame = frameset.get_color_frame()
        color_image = np.asanyarray(aligned_color_frame.get_data())

        # convert color image to BGR for OpenCV
        r, g, b = cv2.split(color_image)
        color_image = cv2.merge((b, g, r))

        # Update color and depth frames:
        aligned_depth_frame = frameset.get_depth_frame()
        depth_image = np.asanyarray(aligned_depth_frame.get_data())

        break

return None

The example here only has one frame and other issues about extracting different types of frames using conditionals didn't resolve my issue.

[ev-mp]

@kjacks21 hello, several remarks:

1. It seem you're missing a request to playback depth along with color. Only color request is specified

rs.config.enable_device_from_file(config, bag_fname, repeat_playback=False)
config.enable_stream(rs.stream.color)
config.enable_stream(rs.stream.depth) ?
profile = pipeline.start(config)

2. When looking further into the segment the following code is not correct

align = rs.align(rs.stream.color)
..
aligned_color_frame = frameset.get_color_frame() ?? This will return the original color frame. the color is not being aligned here.

First decide which direction alignment will be applied - Depth->Color or Color->Depth.
Setting align = rs.align(rs.stream.color) instructs the block to align Depth->Color, i.e. generate a modified depth frame. so when applied correctly to get the aligned_depth frame use

aligned_depth_frame = frameset.get_depth_frame()

3. Make sure you have both depth and color frame to run alignment to avoid de-referencing null pointer:

frameset = pipeline.wait_for_frames(timeout_ms=100)
if frameset.size() <2:
    #Inputs are not ready yet
    break

[kjacks21]

@ev-mp thank you! Issue resolved. In case others want to see how I did this, I saved the color stream as .png and the depth stream as an array of scaled matrices saved as a .npy file. If there is a better way to do this please let me know, and if you'd like me to make a PR for python examples I'd be happy to since I didn't see anything on how to save the color and depth stream in the current examples. Code below:

import argparse
import pyrealsense2 as rs
import numpy as np
import cv2
import os


def extract_from_bag(bag_fname, color_fname, depth_fname):

    config = rs.config()
    pipeline = rs.pipeline()

    # make it so the stream does not continue looping
    config.enable_stream(rs.stream.color)
    config.enable_stream(rs.stream.depth)
    rs.config.enable_device_from_file(config, bag_fname, repeat_playback=False)
    profile = pipeline.start(config)
    # this makes it so no frames are dropped while writing video
    playback = profile.get_device().as_playback()
    playback.set_real_time(False)

    colorizer = rs.colorizer()

    align_to = rs.stream.color
    align = rs.align(align_to)

    depth_sensor = profile.get_device().first_depth_sensor()
    depth_scale = depth_sensor.get_depth_scale()

    depth_matrices = []

    i = 0
    while True:

        # when stream is finished, RuntimeError is raised, hence this
        # exception block to capture this
        try:
            # frames = pipeline.wait_for_frames()
            frames = pipeline.wait_for_frames(timeout_ms=100)
            if frames.size() <2:
                # Inputs are not ready yet
                continue
        except (RuntimeError):
            print('frame count', i-1)
            pipeline.stop()
            break

        # align the deph to color frame
        aligned_frames = align.process(frames)

        # get aligned frames
        aligned_depth_frame = aligned_frames.get_depth_frame()
        color_frame = aligned_frames.get_color_frame()

        # validate that both frames are valid
        if not aligned_depth_frame or not color_frame:
            continue

        depth_image = np.asanyarray(aligned_depth_frame.get_data())
        scaled_depth_image = depth_image * depth_scale
        color_image = np.asanyarray(color_frame.get_data())

        # convert color image to BGR for OpenCV
        r, g, b = cv2.split(color_image)
        color_image = cv2.merge((b, g, r))

        depth_colormap = np.asanyarray(colorizer.colorize(aligned_depth_frame).get_data())

        images = np.hstack((color_image, depth_colormap))
        cv2.namedWindow('Aligned Example', cv2.WINDOW_AUTOSIZE)
        cv2.imshow('Aligned Example', images)

        fname = "frame{:06d}".format(i) + ".png"
        cv2.imwrite(color_fname + fname, color_image)

        depth_matrices.append(scaled_depth_image)

        # color_out.write(color_image)
        if (cv2.waitKey(1) & 0xFF) == ord('q'):
            break

        i += 1

    # release everything now that job finished
    np.save(depth_fname, np.array(depth_matrices))
    print("Size of depth matrices:", len(depth_matrices))
    cv2.destroyAllWindows()

if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    parser.add_argument("-i", "--input", type=str, help=".bag file to read")
    parser.add_argument("-c", "--rgbfilename", type=str, help=".mp4 file to save RGB stream")
    parser.add_argument("-d", "--depthfilename", type=str, help=".npy file to save depth stream")
    args = parser.parse_args()

    extract_from_bag(bag_fname=args.input, color_fname=args.rgbfilename, depth_fname=args.depthfilename)

[NguyenKhanh27]

@ev-mp thank you! Issue resolved. In case others want to see how I did this, I saved the color stream as .png and the depth stream as an array of scaled matrices saved as a .npy file. If there is a better way to do this please let me know, and if you'd like me to make a PR for python examples I'd be happy to since I didn't see anything on how to save the color and depth stream in the current examples. Code below:

import argparse
import pyrealsense2 as rs
import numpy as np
import cv2
import os


def extract_from_bag(bag_fname, color_fname, depth_fname):

    config = rs.config()
    pipeline = rs.pipeline()

    # make it so the stream does not continue looping
    config.enable_stream(rs.stream.color)
    config.enable_stream(rs.stream.depth)
    rs.config.enable_device_from_file(config, bag_fname, repeat_playback=False)
    profile = pipeline.start(config)
    # this makes it so no frames are dropped while writing video
    playback = profile.get_device().as_playback()
    playback.set_real_time(False)

    colorizer = rs.colorizer()

    align_to = rs.stream.color
    align = rs.align(align_to)

    depth_sensor = profile.get_device().first_depth_sensor()
    depth_scale = depth_sensor.get_depth_scale()

    depth_matrices = []

    i = 0
    while True:

        # when stream is finished, RuntimeError is raised, hence this
        # exception block to capture this
        try:
            # frames = pipeline.wait_for_frames()
            frames = pipeline.wait_for_frames(timeout_ms=100)
            if frames.size() <2:
                # Inputs are not ready yet
                continue
        except (RuntimeError):
            print('frame count', i-1)
            pipeline.stop()
            break

        # align the deph to color frame
        aligned_frames = align.process(frames)

        # get aligned frames
        aligned_depth_frame = aligned_frames.get_depth_frame()
        color_frame = aligned_frames.get_color_frame()

        # validate that both frames are valid
        if not aligned_depth_frame or not color_frame:
            continue

        depth_image = np.asanyarray(aligned_depth_frame.get_data())
        scaled_depth_image = depth_image * depth_scale
        color_image = np.asanyarray(color_frame.get_data())

        # convert color image to BGR for OpenCV
        r, g, b = cv2.split(color_image)
        color_image = cv2.merge((b, g, r))

        depth_colormap = np.asanyarray(colorizer.colorize(aligned_depth_frame).get_data())

        images = np.hstack((color_image, depth_colormap))
        cv2.namedWindow('Aligned Example', cv2.WINDOW_AUTOSIZE)
        cv2.imshow('Aligned Example', images)

        fname = "frame{:06d}".format(i) + ".png"
        cv2.imwrite(color_fname + fname, color_image)

        depth_matrices.append(scaled_depth_image)

        # color_out.write(color_image)
        if (cv2.waitKey(1) & 0xFF) == ord('q'):
            break

        i += 1

    # release everything now that job finished
    np.save(depth_fname, np.array(depth_matrices))
    print("Size of depth matrices:", len(depth_matrices))
    cv2.destroyAllWindows()

if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    parser.add_argument("-i", "--input", type=str, help=".bag file to read")
    parser.add_argument("-c", "--rgbfilename", type=str, help=".mp4 file to save RGB stream")
    parser.add_argument("-d", "--depthfilename", type=str, help=".npy file to save depth stream")
    args = parser.parse_args()

    extract_from_bag(bag_fname=args.input, color_fname=args.rgbfilename, depth_fname=args.depthfilename)

Hi kjacks21,
I'm using the method to save depth data as numpy, it is a 3x3 matrices when saved to npy, may I ask how did you load and collect data(depth value) from npy? I appreciate your helps!

[kjacks21]

@NguyenKhanh27 Try using numpy.load(): https://numpy.org/doc/stable/reference/generated/numpy.load.html

I'm also not sure I follow why it saves as a 3x3 matrix.

[666tua]

After running, an error will be reported and how to solve it.
error：Unable to allocate 32.7 GiB for an array with shape (1586, 720, 1280, 3) and data type float64.

[kjacks21]

After running, an error will be reported and how to solve it. error：Unable to allocate 32.7 GiB for an array with shape (1586, 720, 1280, 3) and data type float64.

@L-xn you're running out of memory. You may need write chunks of the array to disk at specific intervals to avoid exceeding your memory.

[666tua]

After running, an error will be reported and how to solve it. error：Unable to allocate 32.7 GiB for an array with shape (1586, 720, 1280, 3) and data type float64.

@L-xn you're running out of memory. You may need write chunks of the array to disk at specific intervals to avoid exceeding your memory.
I saved the depth stream for each frame as npy.I'm doing keypoints detection on the color images.I now have two-dimensional coordinates for the 10 key points of each frame.I want to look in the npy file for the corresponding depth information. What should I do?

[kjacks21]

After running, an error will be reported and how to solve it. error：Unable to allocate 32.7 GiB for an array with shape (1586, 720, 1280, 3) and data type float64.

@L-xn you're running out of memory. You may need write chunks of the array to disk at specific intervals to avoid exceeding your memory.

I saved the depth stream for each frame as npy.I'm doing keypoints detection on the color images.I now have two-dimensional coordinates for the 10 key points of each frame.I want to look in the npy file for the corresponding depth information. What should I do?

@666tua See the answer here for loading the data: #4934 (comment)

The depth image should have the same x,y dimensions as the color stream. To get the corresponding depth values for each key point for a given frame, use the key point coordinates to index the depth image.