How to construct and visualize a poincloud of an isolated part of the color image

alessio8russo commented 2 years ago

Before opening a new issue, we wanted to provide you with some useful suggestions (Click "Preview" above for a better view):
- Consider checking out SDK examples.
- Have you looked in our documentations?
- Is you question a frequently asked one?
- Try searching our GitHub Issues (open and closed) for a similar issue.
All users are welcomed to report bugs, ask questions, suggest or request enhancements and generally feel free to open new issue, even if they haven't followed any of the suggestions above :)

Required Info
Camera Model	D400i
Firmware Version	05.13.00.50
Operating System & Version	Win 10
Platform	PC
Language	python
Segment	others

Issue Description

Hi, i'm very new with python and with pyrealsense, so probably is an easy question. I construct a background removal+color mask in order to isolate only the cable , so all the pixel that are not of the cable are black, but obviously doing in this way when i construct the poincloud using open3d it give as result also the black point. This is my code:

from email.mime import image
import string
from turtle import shape
import pyrealsense2 as rs
import numpy as np
import cv2
import argparse
import time
import open3d
import os
import matplotlib.pyplot as plt
from skimage.filters import frangi

def rescaleFrame(frame,scale):
    width = int(frame.shape[1]*scale)
    height = int(frame.shape[0]*scale)
    dimensions = (width,height)
    return cv2.resize(frame, dimensions,interpolation=cv2.INTER_AREA)

if __name__=="__main__":
    time_start = time.time()
    var = True
    parser = argparse.ArgumentParser(description="Read recorded bag file and display depth stream in jet colormap.\
                                    Remember to change the stream resolution, fps and format to match the recorded.")
    # Add argument which takes path to a bag file as an input
    parser.add_argument("-i", "--input", type=str, help="Path to the bag file")
    # Parse the command line arguments to an object
    args = parser.parse_args()
    # Safety if no parameter have been given
    if not args.input:
        print("No input paramater have been given.")
        print("For help type --help")
        exit()
    # Check if the given file have bag extension
    if os.path.splitext(args.input)[1] != ".bag":
        print("The given file is not of correct file format.")
        print("Only .bag files are accepted")
        exit()

    hole_filling0 = rs.hole_filling_filter(0)
    align = rs.align(rs.stream.color)
    pipeline = rs.pipeline()
    config = rs.config()
    # Tell config that we will use a recorded device from filem to be used by the pipeline through playback.
    rs.config.enable_device_from_file(config, args.input)
    # Configure the pipeline to stream the depth stream
    config.enable_stream(rs.stream.depth,rs.format.z16, 30)
    config.enable_stream(rs.stream.color,rs.format.rgb8, 30)

    # Start streaming from file
    profile = pipeline.start(config)

    intr = profile.get_stream(rs.stream.color).as_video_stream_profile().get_intrinsics()

    pinhole_camera_intrinsic = open3d.camera.PinholeCameraIntrinsic(intr.width, intr.height, intr.fx, intr.fy, intr.ppx, intr.ppy)

    geometrie_added = False
    vis = open3d.visualization.Visualizer()

    vis.create_window("Pointcloud")
    pointcloud = open3d.geometry.PointCloud()
# Getting the depth sensor's depth scale (see rs-align example for explanation)
    depth_sensor = profile.get_device().first_depth_sensor()
    depth_scale = depth_sensor.get_depth_scale()
# # We will be removing the background of objects more than clipping_distance_in_meters meters away
    clipping_distance_in_meters = 0.5 #0.5 meter
    clipping_distance = clipping_distance_in_meters / depth_scale
    try:
        while True:

            # Get frameset of depth
            frames = pipeline.wait_for_frames()

            frames = hole_filling0.process(frames).as_frameset()

             #Align the depth frame to color frame
            aligned_frames0 = align.process(frames)

            # Get color frame
            color_frame = aligned_frames0.get_color_frame()
            #color_frame1 = aligned_frames2.get_color_frame()
            color_image = np.asanyarray(color_frame.get_data())
            color_image = rescaleFrame(color_image,0.5)
            depth_frame0 = aligned_frames0.get_depth_frame()
            depth_image0 = np.asanyarray(depth_frame0.get_data())

            depth_image0 = rescaleFrame(depth_image0,0.5)
            color_image1 = cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR)

# # #------------------------------------------- Background removal + color mask ------------------------------------------------------#

#             # Remove background - Set pixels further than clipping_distance to grey
            grey_color = 155
            depth_image_3d0 = np.dstack((depth_image0,depth_image0,depth_image0)) #depth image is 1 channel, color is 3 channels
            bg_removed = np.where((depth_image_3d0 > clipping_distance) | (depth_image_3d0 <=0.24/depth_scale), grey_color, color_image1)

#             #Create color mask
            hsv = cv2.cvtColor(bg_removed, cv2.COLOR_BGR2HSV)
            lower_black1 = np.array([[49, 59, 37]])
            upper_black1= np.array([(179,255, 90)])
            mask = cv2.inRange(hsv, lower_black1, upper_black1)

            kernel2 = np.ones((5,5),np.uint8)

            dilation = cv2.dilate(mask,kernel2,iterations = 1)
            cv2.imshow('dilation',dilation)

# #             # #------------------------------------------- CONTOURS EXTRACTION_dil ------------------------------------------------------#

# #             #determination of contours (on the basis of the mask_new) stored in a tuple

            cont_dil, hierarchy_dil = cv2.findContours(dilation, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
            N_contours_dil = len(cont_dil) # number of contours found

            # # determination of the lengths of all the contours

            Lengths_dil = list(range(0, N_contours_dil)) # vector from 0 to the N° of contours
            vector_of_lengths_dil = [] # definition of an empty vector

            for jj in Lengths_dil:
                value_now_dil = cv2.arcLength(cont_dil[jj], True) # length of the ii-th contour
                vector_of_lengths_dil.append(value_now_dil) # update of the empty vector

            # find the longest contour 

            longest_contour_dil = max(vector_of_lengths_dil)

            # get the index of the cell where these two contours are stored

            max_index_dil = vector_of_lengths_dil.index(longest_contour_dil)

            # determination of which contours should be used

            good_contours_dil = []

            # for qq in Lengths_dil:
            #     if vector_of_lengths_dil[qq] >= parameter_dil:
            contour_now_dil = cont_dil[max_index_dil]
            good_contours_dil.append(contour_now_dil)

            empty_img = np.zeros(color_image1.shape, color_image1.dtype)
            auxiliary_mask = cv2.fillPoly(empty_img, good_contours_dil, color = (255, 255, 255))

            auxiliary_mask= cv2.cvtColor(auxiliary_mask,cv2.COLOR_BGR2GRAY) #to use in the bitwise_and that need a mask that is 1 channel
            cv2.imshow('mask',auxiliary_mask)
            res = cv2.bitwise_and(color_image1,color_image1, mask=auxiliary_mask )
            depth = open3d.geometry.Image(depth_image0)
            color = open3d.geometry.Image(res)

            rgbd = open3d.geometry.RGBDImage.create_from_color_and_depth(color, depth, convert_rgb_to_intensity = False)
            pcd = open3d.geometry.PointCloud.create_from_rgbd_image(rgbd, pinhole_camera_intrinsic)
            pcd.transform([[1,0,0,0],[0,-1,0,0],[0,0,-1,0],[0,0,0,1]])

            if not pcd:
                continue

            pointcloud.clear()
            pointcloud += pcd

            if not geometrie_added:
                vis.add_geometry(pointcloud)
                geometrie_added = True
            #informs the vis that the related geometries are updated
            vis.update_geometry(pointcloud)
            #the visualizer renders a new frame by calling:
            vis.poll_events()
            vis.update_renderer()

#             #depth_color_frame = rs.colorizer().colorize(depth_frame)

#             #depth_color_image = np.asanyarray(depth_color_frame.get_data())
#             #depth_color_image = rescaleFrame(depth_color_image,0.5)
#             #cv2.imshow("Depth Stream", depth_color_image)
            key = cv2.waitKey(1)

            # if pressed escape exit program or q
            if key & 0xFF == ord('q') or key == 27 :
                cv2.destroyAllWindows()
                break

    finally:
        pipeline.stop()
        pass

So and I obtained a pointcloud like: poincloud_close_F

So in order to resolve this problem i thought to use np.nonzero on the mask in order to have the index o pixel that are different from zero and then use this index in rs2_deproject_pixel_to_point(), but i have difficulties to define the depth value for each pixel , this is my code to check if they have the same depth that gives me error "IndexError: index 2 is out of bounds for axis 1 with size 2"

 index = np.transpose(np.nonzero(auxiliary_mask))
index_row = list(range(0, index.shape[0]))
index_col = list(range(0, index.shape[1]))
 for mm in index_row:
                for nn in index_col:
                    z = depth_frame0.get_distance(index[mm,mm],index[mm,nn])
                print(z)

And also in case i will able to use rs2_deproject_pixel_to_point() , i didn't get how to use these points in order to construct and visualize the poincloud with open3d. So there is an easiest way to construct only the poincloud ?

MartyG-RealSense commented 2 years ago

Hi @alessio8russo May I first ask whether the cable is black? Because of general physics principles (not RealSense specifically), color shades such as dark grey and black absorb light, making it difficult for depth cameras to analyze those surfaces for depth information. So a depth image containing a black cable may appear to show a cable but it may actually just be a cable-shaped black empty area with no depth values in it.

If the cable is black, changing to a different colored cable would help it to be better detected. Alternatively, projecting a strong light source onto a black area can help to bring out depth information from it.

alessio8russo commented 2 years ago

Hi, thanks for reply so quickly, but the problem i think is not the color of cable, because i have a problem how to code what i need to do, because after applying the hole filling ( to resolve the problem of random hole wiht 0 depth in the cable) i have resolved any problem with the depth and depth value. So my problem is how to have the poincloud with only the cable using the data from color masking that isolate the cable. Because i tried also to do a depth filtering and obviously in this way the pointcloud isolate only the cable but all the processing on the color image in this case is useless.

MartyG-RealSense commented 2 years ago

I note that you are applying the hole-filling post processing filter before the depth-color alignment is applied, which is the order that Intel recommends. So the alignment part of the script is likely okay.

I also see that you seem to be taking account of the OpenCV default color space being BGR instead of RGB so that you are not getting a reverse-color image after converting the RealSense image to OpenCV.

The impression that I get from the image though is that the RGB image of the cable is not aligned with the depth image of the cable.

Does the pointcloud look better if you test the Open3D pyrealsense2 pointcloud script at https://github.com/isl-org/Open3D/issues/473#issuecomment-508139946

alessio8russo commented 2 years ago

this code is almost identical to mine , the only difference is the fact that i rescaled the depth and the color image, and without rescaling i have a better pointcloud, but the issue for which i have created this post remain, i want only the cable without the black part. Where as i said, i had also the black part because with the construction of poincloud it project also the part that i don't need of the masked color image (the res image below). For this I asked for suggestion how to have this with np.nonzero and rs2_deproject_pixel_to_point(), or if there are other way to obtain only the cable after i isolated it in the color image. immagine

MartyG-RealSense commented 2 years ago

The C++ rs-grabcuts OpenCV example that also creates a cut-out takes the approach of creating a foreground and background mask and then extracts foreground pixels. Although it is a C++ example, its cv code may provide some useful insights about your own isolation project.

https://github.com/IntelRealSense/librealsense/blob/master/wrappers/opencv/grabcuts/rs-grabcuts.cpp#L87

alessio8russo commented 2 years ago

Ok thank you for the reply, i will try to see, so the way of use np.nonzero and rs2_deproject_pixel_to_point() isn't good? Because i have some doubts how to use it, so how to visualize the point that i obtain from rs2_deproject_pixel_to_point() ?

alessio8russo commented 2 years ago

I have obtained what i need how we can see from the image: poincloud

this is my code, if anyone have my same issues :

from email.mime import image
import string
from turtle import shape
import pyrealsense2 as rs
import numpy as np
import cv2
import argparse
import time
import open3d
import os
import matplotlib.pyplot as plt
from skimage.filters import frangi

def rescaleFrame(frame,scale):
    width = int(frame.shape[1]*scale)
    height = int(frame.shape[0]*scale)
    dimensions = (width,height)
    return cv2.resize(frame, dimensions,interpolation=cv2.INTER_AREA)

if __name__=="__main__":
    time_start = time.time()
    var = True
    parser = argparse.ArgumentParser(description="Read recorded bag file and display depth stream in jet colormap.\
                                    Remember to change the stream resolution, fps and format to match the recorded.")
    # Add argument which takes path to a bag file as an input
    parser.add_argument("-i", "--input", type=str, help="Path to the bag file")
    # Parse the command line arguments to an object
    args = parser.parse_args()
    # Safety if no parameter have been given
    if not args.input:
        print("No input paramater have been given.")
        print("For help type --help")
        exit()
    # Check if the given file have bag extension
    if os.path.splitext(args.input)[1] != ".bag":
        print("The given file is not of correct file format.")
        print("Only .bag files are accepted")
        exit()

    hole_filling0 = rs.hole_filling_filter(0)
    align = rs.align(rs.stream.color)
    pipeline = rs.pipeline()
    config = rs.config()
    # Tell config that we will use a recorded device from filem to be used by the pipeline through playback.
    rs.config.enable_device_from_file(config, args.input)
    # Configure the pipeline to stream the depth stream
    config.enable_stream(rs.stream.depth,rs.format.z16, 30)
    config.enable_stream(rs.stream.color,rs.format.rgb8, 30)

    # Start streaming from file
    profile = pipeline.start(config)

    intr = profile.get_stream(rs.stream.color).as_video_stream_profile().get_intrinsics()

    pinhole_camera_intrinsic = open3d.camera.PinholeCameraIntrinsic(intr.width, intr.height, intr.fx, intr.fy, intr.ppx, intr.ppy)

    geometrie_added = False
    vis = open3d.visualization.Visualizer()

    vis.create_window("Pointcloud")
    pointcloud = open3d.geometry.PointCloud()
# Getting the depth sensor's depth scale (see rs-align example for explanation)
    depth_sensor = profile.get_device().first_depth_sensor()
    depth_scale = depth_sensor.get_depth_scale()
# # We will be removing the background of objects more than clipping_distance_in_meters meters away
    clipping_distance_in_meters = 0.5 #0.5 meter
    clipping_distance = clipping_distance_in_meters / depth_scale
    try:
        while True:

            # Get frameset of depth
            frames = pipeline.wait_for_frames()

            frames = hole_filling0.process(frames).as_frameset()

             #Align the depth frame to color frame
            aligned_frames0 = align.process(frames)

            # Get color frame
            color_frame = aligned_frames0.get_color_frame()
            #color_frame1 = aligned_frames2.get_color_frame()
            color_image = np.asanyarray(color_frame.get_data())
            #color_image = rescaleFrame(color_image,0.5)
            depth_frame0 = aligned_frames0.get_depth_frame()
            depth_image0 = np.asanyarray(depth_frame0.get_data())

            #depth_image0 = rescaleFrame(depth_image0,0.5)
            color_image1 = cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR)

# # #------------------------------------------- Background removal + color mask ------------------------------------------------------#

#             # Remove background - Set pixels further than clipping_distance to grey
            grey_color = 155
            depth_image_3d0 = np.dstack((depth_image0,depth_image0,depth_image0)) #depth image is 1 channel, color is 3 channels
            bg_removed = np.where((depth_image_3d0 > clipping_distance) | (depth_image_3d0 <=0.24/depth_scale), grey_color, color_image1)

#             #Create color mask
            hsv = cv2.cvtColor(bg_removed, cv2.COLOR_BGR2HSV)
            lower_black1 = np.array([[49, 59, 37]])
            upper_black1= np.array([(179,255, 90)])
            mask = cv2.inRange(hsv, lower_black1, upper_black1)

            kernel2 = np.ones((5,5),np.uint8)

            dilation = cv2.dilate(mask,kernel2,iterations = 1)
            cv2.imshow('dilation',dilation)

# #             # #------------------------------------------- CONTOURS EXTRACTION_dil ------------------------------------------------------#

# #             #determination of contours (on the basis of the mask_new) stored in a tuple

            cont_dil, hierarchy_dil = cv2.findContours(dilation, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
            N_contours_dil = len(cont_dil) # number of contours found

            # # determination of the lengths of all the contours

            Lengths_dil = list(range(0, N_contours_dil)) # vector from 0 to the N° of contours
            vector_of_lengths_dil = [] # definition of an empty vector

            for jj in Lengths_dil:
                value_now_dil = cv2.arcLength(cont_dil[jj], True) # length of the ii-th contour
                vector_of_lengths_dil.append(value_now_dil) # update of the empty vector

            # find the longest contour 

            longest_contour_dil = max(vector_of_lengths_dil)

            # get the index of the cell where these two contours are stored

            max_index_dil = vector_of_lengths_dil.index(longest_contour_dil)

            # determination of which contours should be used

            good_contours_dil = []

            # for qq in Lengths_dil:
            #     if vector_of_lengths_dil[qq] >= parameter_dil:
            contour_now_dil = cont_dil[max_index_dil]
            good_contours_dil.append(contour_now_dil)

            empty_img = np.zeros(color_image1.shape, color_image1.dtype)
            auxiliary_mask = cv2.fillPoly(empty_img, good_contours_dil, color = (255, 255, 255))

            auxiliary_mask= cv2.cvtColor(auxiliary_mask,cv2.COLOR_BGR2GRAY) #to use in the bitwise_and that need a mask that is 1 channel
            cv2.imshow('mask',auxiliary_mask)
            res = cv2.bitwise_and(color_image1,color_image1, mask=auxiliary_mask )
            cv2.imshow('res',res)
            res= cv2.cvtColor(res,cv2.COLOR_BGR2RGB)

            #find the index of pixel with non zero value:
            index = np.transpose(np.nonzero(auxiliary_mask))
            index_row = list(range(0, index.shape[0]))

            #construction of empty np.array float64
            point= np.empty((index.shape[0],3))
            #construction of point in 3D
            for mm in index_row:
                z = depth_frame0.get_distance(index[mm,1],index[mm,0])
                point[mm] =  rs.rs2_deproject_pixel_to_point(intr,[index[mm,1],index[mm,0]],z)
             #Trasformation of point from numpy array to Open3D format and add this point to the pointcloud
            pointcloud.points = open3d.utility.Vector3dVector(point)
            pointcloud.transform([[1,0,0,0],[0,-1,0,0],[0,0,-1,0],[0,0,0,1]])

            if not geometrie_added:
                vis.add_geometry(pointcloud)
                geometrie_added = True
            #informs the vis that the related geometries are updated
            vis.update_geometry(pointcloud)
            #the visualizer renders a new frame by calling:
            vis.poll_events()
            vis.update_renderer()

            key = cv2.waitKey(1)

            # if pressed escape exit program or q
            if key & 0xFF == ord('q') or key == 27 :
                cv2.destroyAllWindows()
                break

    finally:
        pipeline.stop()
        pass

so i obtained only the cable (i don't know why it is blue), but there are already some outlier, any suggestion how to remove them? In addition to that there is a way in order to downsample the index of points that i have obtained using np.nonzero on the mask to be less, so in this way the pointcloud has lower number of points but only the most important? Because i need the pointcloud to do a tracking of the cable, so have only the most important points is better.

MartyG-RealSense commented 2 years ago

It's great to hear that you made significant progress. Thanks so much for sharing your code with the RealSense community!

Open3D has an official tutorial for pointcloud outlier removal at the link below.

http://www.open3d.org/docs/latest/tutorial/Advanced/pointcloud_outlier_removal.html

In regard to reducing the number of points, you could try applying the High Accuracy camera configuration preset. This will be stricter with the depth values that it considers accurate and exclude from the image the depth values that it has less confidence in. An example of a pyrealsense2 script for setting the High Accuracy preset can be found at https://github.com/IntelRealSense/librealsense/issues/2577#issuecomment-432137634

alessio8russo commented 2 years ago

Thank you so much. I will try with both. Is there any down-sampling on the color image or 1 channel-image?

MartyG-RealSense commented 2 years ago

The 'High Accuracy' preset will only affect depth coordinates. It does not down-sample, it just decides whether a depth point is included on the image or not based on how confident the SDK is in the accuracy of that point's depth value.

alessio8russo commented 2 years ago

I read it, for this i asked for a method that can be applied on the color point , and so i can down-sample the color image

MartyG-RealSense commented 2 years ago

Down-sampling of RGB is supported by the Decimation post-processing filter for the RGB sensor,

There is not a code example for the RGB decimation filter. I would speculate though that the code would be similar to the decimation filter for depth, except for setting the sensor to be accessed to an index number of '1' (the RGB sensor) as described at https://github.com/IntelRealSense/librealsense/issues/10002#issuecomment-987865431

alessio8russo commented 2 years ago

Thank you, now i'm trying with outlier removal of opend3d, to reduce the points

MartyG-RealSense commented 1 year ago

Hi @alessio8russo Do you require further assistance with this case, please? Thanks!

alessio8russo commented 1 year ago

No now i'm searching for other issues, relate to how to interpolate the obtained points, any suggestion?

MartyG-RealSense commented 1 year ago

I researched Open3D interpolation in regards to RealSense. Of the small number of references that were available, they were all interpolating with approximately the same cv2.INTER_AREA instruction that you are already using in your own script.

An example of such a reference is https://github.com/isl-org/Open3D/issues/3452

IntelRealSense / librealsense

How to construct and visualize a poincloud of an isolated part of the color image #10894

Issue Description