Git Product home page Git Product logo

Comments (8)

sitzikbs avatar sitzikbs commented on May 25, 2024 2

@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

from pylibfreenect2.

sitzikbs avatar sitzikbs commented on May 25, 2024 1

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)

from pylibfreenect2.

manurare avatar manurare commented on May 25, 2024

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?

from pylibfreenect2.

AAcquier avatar AAcquier commented on May 25, 2024

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???

from pylibfreenect2.

AAcquier avatar AAcquier commented on May 25, 2024

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

from pylibfreenect2.

sitzikbs avatar sitzikbs commented on May 25, 2024

@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

from pylibfreenect2.

AAcquier avatar AAcquier commented on May 25, 2024

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?

from pylibfreenect2.

sitzikbs avatar sitzikbs commented on May 25, 2024

from pylibfreenect2.

Related Issues (20)

Recommend Projects

  • React photo React

    A declarative, efficient, and flexible JavaScript library for building user interfaces.

  • Vue.js photo Vue.js

    🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.

  • Typescript photo Typescript

    TypeScript is a superset of JavaScript that compiles to clean JavaScript output.

  • TensorFlow photo TensorFlow

    An Open Source Machine Learning Framework for Everyone

  • Django photo Django

    The Web framework for perfectionists with deadlines.

  • D3 photo D3

    Bring data to life with SVG, Canvas and HTML. 📊📈🎉

Recommend Topics

  • javascript

    JavaScript (JS) is a lightweight interpreted programming language with first-class functions.

  • web

    Some thing interesting about web. New door for the world.

  • server

    A server is a program made to process requests and deliver data to clients.

  • Machine learning

    Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.

  • Game

    Some thing interesting about game, make everyone happy.

Recommend Org

  • Facebook photo Facebook

    We are working to build community through open source technology. NB: members must have two-factor auth.

  • Microsoft photo Microsoft

    Open source projects and samples from Microsoft.

  • Google photo Google

    Google ❤️ Open Source for everyone.

  • D3 photo D3

    Data-Driven Documents codes.