How can I use registration.apply offline?

[RuWang15]

I want to get the depth frame warped into color frame. But I only have depth and color images. I notice there's an issue about creating Frame from numpy.ndarray, I tried but there's not a parameter called byte_data anymore and I didn't find anything helpful in the documentation about creating Frame from numpy.ndarray. So how can I get bigdepth from previously collected color and depth images without Kinect v2?

[sitzikbs]

You probably solved this already but adding the answer for future users.
The first step in the solution would be to convert the rgb image into bytes and then read them back from the buffer correctly and reshape to get dim=2.
So,
color_img = cv2.imread(image_file_name)
color_img = cv2cvtColor(color_img, cv2.COLOR_BGR2BGRA)
color_img = color_img.tobytes()
color_img = np.frombuffer(color_img, dtype=np.float)
color_img = color_img.reshape((1080, 1920))
color_img = color_img.astype(np.float32)

And then just create the frame
color_frame = Frame(1920, 1080, 4, FrameType.Color, color_img).

But there is still a problem.
When trying to use 'Registration.apply' I get an assertion error (because of rgb.take_ownership). There is no way to set it manually. For undistorting the depth it works fine.

@r9y9 , Is there a way to register the depth and rgb offline? (I want to generate an XYZRGB point cloud)

[manurare]

Hi,

Im trying to run the code @sitzikbs proposed but for me it does not seem to work. I need to convert an rgb image (BGR in opencv) to a Frame object but the execution gets stuck in np.frombuffer. If I change the dtype to float32 it decodes an array with nan in all positions. Do you know where the problem could be?

[AAcquier]

You probably solved this already but adding the answer for future users.
The first step in the solution would be to convert the rgb image into bytes and then read them back from the buffer correctly and reshape to get dim=2.
So,
color_img = cv2.imread(image_file_name)
color_img = cv2cvtColor(color_img, cv2.COLOR_BGR2BGRA)
color_img = color_img.tobytes()
color_img = np.frombuffer(color_img, dtype=np.float)
color_img = color_img.reshape((1080, 1920))
color_img = color_img.astype(np.float32)

And then just create the frame
color_frame = Frame(1920, 1080, 4, FrameType.Color, color_img).

But there is still a problem.
When trying to use 'Registration.apply' I get an assertion error (because of rgb.take_ownership). There is no way to set it manually. For undistorting the depth it works fine.

@r9y9 , Is there a way to register the depth and rgb offline? (I want to generate an XYZRGB point cloud)

I Have the same issue with the registration of the depth map and RGB offline, did you find a solution???

[sitzikbs]

@AAcquier I eventually reimplemented everything in python. I didn't document it properly at the time but I hope this helps:

def compute_point_clouds(depth_ins_params, rgb_ins_params, depth_frame, rgb_frame, point_cloud_dir_name):
    frame_filename, depth_frame_file_extension = os.path.splitext(os.path.basename(depth_frame))
    depth_img = cv2.imread(depth_frame, cv2.IMREAD_ANYDEPTH).astype(np.float32)
    depth_img = cv2.flip(depth_img, 1)  # images were saved flipped
    # relative_depth_frame = get_relative_depth(depth_img)
    color_img = cv2.imread(rgb_frame)
    color_img = cv2.flip(color_img, 1)  # images were saved flipped

    # # Compute point cloud from images
    point_cloud = []
    new_vertices = []
    registered = np.zeros([424, 512, 3], dtype=np.uint8)
    undistorted = np.zeros([424, 512, 3], dtype=np.float32)
    for y in range(424):
        for x in range(512):
            mx, my = distort(x, y, depth_ins_params)
            ix = int(mx + 0.5)
            iy = int(my + 0.5)
            point = getPointXYZ(depth_img, y, x, depth_ins_params)
            if (ix >= 0 and ix < 512 and iy >= 0 and iy < 424):  # check if pixel is within the image
                undistorted[iy, ix] = depth_img[y, x]
                z = depth_img[y, x]
                if z > 0 and not np.isnan(z):
                    # point_cloud.append(point)
                    cx, cy = depth_to_color(x, y, z, depth_ins_params, rgb_ins_params)
                    if (cx >= 0 and cx < 1920 and cy >= 0 and cy < 1080):
                        registered[y, x, :] = color_img[cy, cx].flatten()
                        registered[y, x, :] = cv2.cvtColor(registered[y, x].reshape([1, 1, 3]),
                                                           cv2.COLOR_BGR2RGB)

                        point_cloud.append([(point), registered[y, x, :]])
                        new_vertices.append((point[0], point[1], point[2], registered[y, x, 0],
                                             registered[y, x, 1], registered[y, x, 2]))

    pc_file_name = os.path.join(point_cloud_dir_name, frame_filename + '.ply')

    new_vertices = np.array(new_vertices, dtype=[('x', 'f4'), ('y', 'f4'), ('z', 'f4'),
                                                 ('red', 'u1'), ('green', 'u1'), ('blue', 'u1')])
    el = PlyElement.describe(new_vertices, 'vertex')
    PlyData([el]).write(pc_file_name)
    print('Saved Point cloud ' + frame_filename + '.ply to ' + point_cloud_dir_name)

def getPointXYZ(undistorted, r, c, depth_ins):
    depth_val = undistorted[r, c] #/ 1000.0  # map from mm to meters
    if np.isnan(depth_val) or depth_val <= 0.001:
        x = 0
        y = 0
        z = 0
    else:
        x = (c + 0.5 - depth_ins['cx']) * depth_val / depth_ins['fx']
        y = (r + 0.5 - depth_ins['cy']) * depth_val / depth_ins['fy']
        z = depth_val
    point = [x, y, z]
    return point

def distort(mx, my, depth_ins):
    # see http://en.wikipedia.org/wiki/Distortion_(optics) for description
    # based on the C++ implementation in libfreenect2
    dx = (float(mx) - depth_ins['cx']) / depth_ins['fx']
    dy = (float(my) - depth_ins['cy']) / depth_ins['fy']
    dx2 = np.square(dx)
    dy2 = np.square(dy)
    r2 = dx2 + dy2
    dxdy2 = 2 * dx * dy
    kr = 1 + ((depth_ins['k3'] * r2 + depth_ins['k2']) * r2 + depth_ins['k1']) * r2
    x = depth_ins['fx'] * (dx * kr + depth_ins['p2'] * (r2 + 2 * dx2) + depth_ins['p1'] * dxdy2) + depth_ins['cx']
    y = depth_ins['fy'] * (dy * kr + depth_ins['p1'] * (r2 + 2 * dy2) + depth_ins['p2'] * dxdy2) + depth_ins['cy']
    return x, y

def depth_to_color(mx, my, dz, depth_ins, color_ins):
    # based on the C++ implementation in libfreenect2, constants are hardcoded into sdk
    depth_q = 0.01
    color_q = 0.002199

    mx = (mx - depth_ins['cx']) * depth_q
    my = (my - depth_ins['cy']) * depth_q

    wx = (mx * mx * mx * color_ins['mx_x3y0']) + (my * my * my * color_ins['mx_x0y3']) + \
         (mx * mx * my * color_ins['mx_x2y1']) + (my * my * mx * color_ins['mx_x1y2']) + \
         (mx * mx * color_ins['mx_x2y0']) + (my * my * color_ins['mx_x0y2']) + \
         (mx * my * color_ins['mx_x1y1']) +(mx * color_ins['mx_x1y0']) + \
         (my * color_ins['mx_x0y1']) + (color_ins['mx_x0y0'])

    wy = (mx * mx * mx * color_ins['my_x3y0']) + (my * my * my * color_ins['my_x0y3']) +\
         (mx * mx * my * color_ins['my_x2y1']) + (my * my * mx * color_ins['my_x1y2']) +\
         (mx * mx * color_ins['my_x2y0']) + (my * my * color_ins['my_x0y2']) + (mx * my * color_ins['my_x1y1']) +\
         (mx * color_ins['my_x1y0']) + (my * color_ins['my_x0y1']) + color_ins['my_x0y0']

    rx = (wx / (color_ins['fx'] * color_q)) - (color_ins['shift_m'] / color_ins['shift_d'])
    ry = int((wy / color_q) + color_ins['cy'])

    rx = rx + (color_ins['shift_m'] / dz)
    rx = int(rx * color_ins['fx'] + color_ins['cx'])
    return rx, ry

[AAcquier]

Hi @sitzikbs ,

Thanks for your code it is really helpful but I still got a couple of questions concerning it:

• Should the depth_frames[j] in the second line of compute_point_clouds() have been edited in simply depth_frames?
• Where do the distort (line 19), depth_to_color (l 28) functions come from?

Kind regards,

Alex

[sitzikbs]

@AAcquier

1. yes, you are correct. I originally passed a sequence but changed the code a bit to answer your question, but seem to have missed that. I edited the original code.
2. these function I also converted to python from the c++ code, forgot to attach them as well. here is the code, Ill also put it in the original message for completeness :

def distort(mx, my, depth_ins):
    # see http://en.wikipedia.org/wiki/Distortion_(optics) for description
    # based on the C++ implementation in libfreenect2
    dx = (float(mx) - depth_ins['cx']) / depth_ins['fx']
    dy = (float(my) - depth_ins['cy']) / depth_ins['fy']
    dx2 = np.square(dx)
    dy2 = np.square(dy)
    r2 = dx2 + dy2
    dxdy2 = 2 * dx * dy
    kr = 1 + ((depth_ins['k3'] * r2 + depth_ins['k2']) * r2 + depth_ins['k1']) * r2
    x = depth_ins['fx'] * (dx * kr + depth_ins['p2'] * (r2 + 2 * dx2) + depth_ins['p1'] * dxdy2) + depth_ins['cx']
    y = depth_ins['fy'] * (dy * kr + depth_ins['p1'] * (r2 + 2 * dy2) + depth_ins['p2'] * dxdy2) + depth_ins['cy']
    return x, y

def depth_to_color(mx, my, dz, depth_ins, color_ins):
    # based on the C++ implementation in libfreenect2, constants are hardcoded into sdk
    depth_q = 0.01
    color_q = 0.002199

    mx = (mx - depth_ins['cx']) * depth_q
    my = (my - depth_ins['cy']) * depth_q

    wx = (mx * mx * mx * color_ins['mx_x3y0']) + (my * my * my * color_ins['mx_x0y3']) + \
         (mx * mx * my * color_ins['mx_x2y1']) + (my * my * mx * color_ins['mx_x1y2']) + \
         (mx * mx * color_ins['mx_x2y0']) + (my * my * color_ins['mx_x0y2']) + \
         (mx * my * color_ins['mx_x1y1']) +(mx * color_ins['mx_x1y0']) + \
         (my * color_ins['mx_x0y1']) + (color_ins['mx_x0y0'])

    wy = (mx * mx * mx * color_ins['my_x3y0']) + (my * my * my * color_ins['my_x0y3']) +\
         (mx * mx * my * color_ins['my_x2y1']) + (my * my * mx * color_ins['my_x1y2']) +\
         (mx * mx * color_ins['my_x2y0']) + (my * my * color_ins['my_x0y2']) + (mx * my * color_ins['my_x1y1']) +\
         (mx * color_ins['my_x1y0']) + (my * color_ins['my_x0y1']) + color_ins['my_x0y0']

    rx = (wx / (color_ins['fx'] * color_q)) - (color_ins['shift_m'] / color_ins['shift_d'])
    ry = int((wy / color_q) + color_ins['cy'])

    rx = rx + (color_ins['shift_m'] / dz)
    rx = int(rx * color_ins['fx'] + color_ins['cx'])
    return rx, ry

[AAcquier]

Thank you very much for that, is there a way to find the depth of rgb pixel an/or the physical distance between (ie in mm) between 2 rgb pixels?

[sitzikbs]

Now you have a mapping from every rgb pixel to depth pixel so you can find the corresponding point and just measure the euclidean distance between the points.

…

On Fri, Apr 17, 2020 at 12:07 AM AAcquier ***@***.***> wrote: Thank you very much for that, is there a way to find the depth of rgb pixel an/or the physical distance between (ie in mm) between 2 rgb pixels? — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#57 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/ADJHW62I4JSBYZJ5DLZCIVDRM4GLDANCNFSM4GHOMOSA> .

-- Yizhak (Itzik) Ben-Shabat | Research Fellow | ARC Centre of Excellence for Robotic Vision College of Engineering and Computer Science | Australian National University M 0401 352 546 | E yizhak.benshabat@anu.edu.au | W www.itzikbs.com Canberra ACT 2600 Australia