Search code examples
pythonnumpyrealsense

python pointcloud filtering with numpy


I'm trying to filter a point cloud with numpy.

i convert everything to a numpy array.

verts = np.asarray (points.get_vertices (2)). reshape (h, w, 3)

now I would e.g. like to only view values in a certain xmin, xmax range. or also xmin, xmax, ymin, ymax.

I tried the following for the x filter

verts = np.asarray(points.get_vertices(2)).reshape(h, w, 3)
texcoords = np.asarray(points.get_texture_coordinates(2))


xmin = -0.25
xmax = 0.25
ymin = 0.0
ymax = 1.0

inidx = np.all(np.logical_and(xmin <= verts, verts <= xmax), axis=0)
inbox = verts[inidx]
print(verts.length, inbox.length)

but already here I get an error message

IndexError: boolean index did not match indexed array along dimension 1; dimension is 212 but corresponding boolean dimension is 3

Solution

  • By separating your different components with np.split:

    >>> x, y, z = np.split(verts, 3, axis=-1)
    

    You can combine conditions with the multiplication operator based on x, and y:

    >>> mask = (xmin <= x)*(x <= xmax)*(ymin <= y)*(y <= ymax)
    

    Then mask your verts array:

    >>> verts_masked = verts[mask[..., 0]]
    tensor([[ 0.1221,  0.2402,  0.7808],
            [ 0.1274,  0.1203, -0.9398],
            [ 0.0789,  0.8018, -0.7915],
            [ 0.1515,  0.3616, -0.2061],
            [ 0.2166,  0.3970,  0.4706],
            [ 0.2421,  0.0457,  0.1082],
            [ 0.0480,  0.9252,  0.2259],
            [ 0.2145,  0.5752, -0.3701],
            [-0.2099,  0.4220,  0.2342],
            [ 0.0949,  0.9467, -0.4768],
            [ 0.0746,  0.2131, -0.1160],
            [-0.2072,  0.4472, -0.3754],
            [-0.0994,  0.8972, -0.7704],
            [ 0.2424,  0.3210, -0.2291],
            [ 0.1093,  0.2599, -0.2868],
            [-0.2482,  0.6001, -0.3283]])
    

    Additionally, if you are looking to mask texcoords with mask as well:

    >>> textcoords_masked = textcoords[mask[..., 0]]
    

    To sort the resulting arrays based on z you can use np.argsort:

    >>> indices = np.argsort(verts_masked[:,-1])
    

    Then get the filtered-sorted arrays for verts and textcoords as verts_masked[indices] and textcoords_masked[indices] respectively.