def plane_detection(color_image, depth_array, loop=1):
planes_mask, planes_normal, list_plane_params = test_PlaneDetector_send(\
img_color=color_image, img_depth=depth_array)
for idx in range(loop - 1):
planes_mask_x, planes_normal_x, list_plane_params_x = test_PlaneDetector_send(\
img_color=color_image, img_depth=depth_array)
planes_mask += planes_mask_x
planes_mask_binary = planes_mask
r1, g1, b1 = 255, 0, 40 # Original value
r2, g2, b2 = 1, 1, 1 # Value that we want to replace it with
red, green, blue = planes_mask[:, :, 0], planes_mask[:, :,
1], planes_mask[:, :,
2]
mask = (red > r1) & (green == g1) & (blue > b1)
planes_mask_binary[:, :, :3][mask] = [r2, g2, b2]
planes_mask_binary = planes_mask_binary[:, :, 0]
# plane is blue (255,0,40)
return planes_mask_binary
def main():
# Create a pipeline
pipeline = rs.pipeline()
#Create a config and configure the pipeline to stream
# different resolutions of color and depth streams
config = rs.config()
rs.config.enable_device_from_file(config, args.input)
# Start streaming
profile = pipeline.start(config)
depth_sensor = profile.get_device().first_depth_sensor()
# Create an align object
# rs.align allows us to perform alignment of depth frames to others frames
# The "align_to" is the stream type to which we plan to align depth frames.
align_to = rs.stream.color
align = rs.align(align_to)
vis = x3d.visualization.Visualizer()
vis.create_window()
# ocgd = x3d.geometry.OccupancyGrid(0.05, 512)
ocgd = x3d.geometry.OccupancyGrid()
ocgd.visualize_free_area = False
flip_transform = [[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]
prev_rgbd_image = None
option = x3d.odometry.OdometryOption()
cur_trans = np.identity(4)
# Streaming loop
frame_count = 0
try:
while True:
dt0 = datetime.now()
# Get frameset of color and depth
frames = pipeline.wait_for_frames()
# Align the depth frame to color frame
aligned_frames = align.process(frames)
# Get aligned frames
aligned_depth_frame = aligned_frames.get_depth_frame()
aligned_depth_frame = depth_filter(aligned_depth_frame)
color_frame = aligned_frames.get_color_frame()
intrinsic = x3d.camera.PinholeCameraIntrinsic(
get_intrinsic_matrix(color_frame))
# Validate that both frames are valid
if not aligned_depth_frame or not color_frame:
continue
depth_temp = np.array(aligned_depth_frame.get_data())
color_temp = np.asarray(color_frame.get_data())
############ Plane Detection
## need to add smoothening between frames - by plane weights' variance?
try:
### need to add multithreading here (and maybe other methods?)
planes_mask_binary = plane_detection(color_temp, depth_temp,\
loop=3)
except TypeError as e:
try:
print("plane mask 1st error")
planes_mask, planes_normal, list_plane_params = test_PlaneDetector_send(
img_color=color_temp, img_depth=depth_temp)
except TypeError as e:
print("plane mask not detected-skipping frame")
continue
##############################################
## Hole filling for plane_mask (plane mask isn't binary - fixed that!)
planes_mask_binary = nd.binary_fill_holes(planes_mask_binary)
planes_mask_binary = planes_mask_binary.astype(np.uint8)
# Clean plane mask object detection by seg_mask
planes_mask_binary_3d = np.dstack(
(planes_mask_binary, planes_mask_binary, planes_mask_binary))
#############################################
depth_image = x3d.geometry.Image(depth_temp * planes_mask_binary)
color_image = x3d.geometry.Image(color_temp *
planes_mask_binary_3d)
rgbd_image = x3d.geometry.RGBDImage.create_from_color_and_depth(
color_image, depth_image)
if not prev_rgbd_image is None:
res, odo_trans, _ = x3d.odometry.compute_rgbd_odometry(
prev_rgbd_image, rgbd_image, intrinsic, np.identity(4),
x3d.odometry.RGBDOdometryJacobianFromHybridTerm(), option)
if res:
cur_trans = np.matmul(cur_trans, odo_trans)
prev_rgbd_image = rgbd_image
temp = x3d.geometry.PointCloud.create_from_rgbd_image(
rgbd_image, intrinsic)
temp.transform(np.matmul(cur_trans, flip_transform))
temp = temp.voxel_down_sample(0.05)
ocgd.insert(temp, cur_trans[:3, 3])
if frame_count == 0:
vis.add_geometry(ocgd)
vis.update_geometry(ocgd)
vis.poll_events()
vis.update_renderer()
dt1 = datetime.now()
process_time = dt1 - dt0
print("FPS: " + str(1 / process_time.total_seconds()))
frame_count += 1
finally:
pipeline.stop()
vis.destroy_window()
def run_loop(bag_path, seg_model, seg_opts, save_images=False, output_mode=0):
if save_images:
create_folders()
# Create pipeline
pipeline = rs.pipeline()
# Create a config object
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)
# Start streaming from file
Pipe = pipeline.start(config)
# Getting the depth sensor's depth scale (see rs-align example for explanation)
depth_sensor = Pipe.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
print("Depth Scale is: ", depth_scale) # can be commented out
if output_mode == 0 or output_mode == 1:
# Create opencv window to render image in
cv2.namedWindow("Full Stream", cv2.WINDOW_NORMAL)
flip_transform = [[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]
# Create colorizer object
colorizer = rs.colorizer()
idx = 0
# initial frame delay
idx_limit = 30
# pre_seg_mask_sum = None # previous frame path segmentation area - isn't being used right now
# Streaming loop
try:
while True:
idx += 1
# Get frameset of depth
frames = pipeline.wait_for_frames()
# ignore first idx frames
if idx < idx_limit:
continue
else:
pass
align = rs.align(rs.stream.color)
frames = align.process(frames)
# Get color frame
color_frame = frames.get_color_frame()
# Get intrinsic in Open3d format for mode 2 and 3 for point cloud output
if output_mode == 1 or output_mode == 2:
intrinsic = o3d.camera.PinholeCameraIntrinsic(
get_intrinsic_matrix(color_frame))
# Get depth frame
depth_frame = frames.get_depth_frame()
# Print intrinsics and extrinsics - not necessary : can be commented out
if idx == idx_limit:
camera_intrinsics(color_frame, depth_frame, Pipe)
color_image = np.asanyarray(color_frame.get_data())
### Add SEGMENTATION part here ###
pred = test(color_image, seg_model, seg_opts)
# pavement, floor, road, earth/ground, field, path, dirt/track - chosen classes for the model selected (we'd like an oversegmentation of the path)
seg_mask = (pred == 11) | (pred == 3) | (pred == 6) | (
pred == 13) | (pred == 29) | (pred == 52) | (
pred == 91) #.astype(np.uint8)
if idx == idx_limit: # 1st frame detection needs to be robust
pre_seg_mask_sum = np.sum(seg_mask)
# checking for bad detection
new_seg_sum = np.sum(seg_mask)
diff = abs(new_seg_sum - pre_seg_mask_sum)
# if diff > pre_seg_mask_sum/15: # smoothening between segmentation outputs - seems like a bad idea since the model inputs are not connected between timesteps
# seg_mask = np.ones_like(pred).astype(np.uint8) # need to add depth (5mt) criterea for calculation for robustness
# del new_seg_sum
# else:
pre_seg_mask_sum = new_seg_sum
### mask Hole filling
seg_mask = nd.binary_fill_holes(seg_mask).astype(int)
seg_mask = seg_mask.astype(np.uint8)
#####
seg_mask_3d = np.dstack((seg_mask, seg_mask, seg_mask))
pred_color = colorEncode(
pred,
loadmat(os.path.join(model_folder, 'color150.mat'))['colors'])
##################################
depth_frame = depth_filter(depth_frame)
depth_array = np.asarray(depth_frame.get_data())
# Colorize depth frame to jet colormap
depth_color_frame = colorizer.colorize(depth_frame)
# Convert depth_frame to numpy array to render image in opencv
depth_color_image = np.asanyarray(depth_color_frame.get_data())
############ Plane Detection
## need to add smoothening between frames - by plane weights' variance?
try:
### need to add multithreading here (and maybe other methods?)
planes_mask_binary = plane_detection(color_image*seg_mask_3d, depth_array*seg_mask,\
loop=5)
except TypeError as e:
try:
print("plane mask 1st error")
planes_mask, planes_normal, list_plane_params = test_PlaneDetector_send(
img_color=color_image * seg_mask_3d,
img_depth=depth_array * seg_mask)
except TypeError as e:
print("plane mask not detected-skipping frame")
continue
## removed this part
planes_mask = np.ones_like(depth_array).astype(np.uint8)
planes_mask = np.dstack(
(planes_mask, planes_mask, planes_mask))
##############################################
## Hole filling for plane_mask (plane mask isn't binary - fixed that!)
planes_mask_binary = nd.binary_fill_holes(planes_mask_binary)
planes_mask_binary = planes_mask_binary.astype(np.uint8)
# Clean plane mask object detection by seg_mask
planes_mask_binary *= seg_mask
planes_mask_binary_3d = np.dstack(
(planes_mask_binary, planes_mask_binary, planes_mask_binary))
edges = planes_mask_binary - nd.morphology.binary_dilation(
planes_mask_binary
) # edges calculation - between travesable and non-traversable path
#############################################
if output_mode == 1 or output_mode == 2:
odepth_image = o3d.geometry.Image(depth_array * edges)
ocolor_image = o3d.geometry.Image(color_image * np.dstack(
(edges, edges, edges)))
rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
ocolor_image,
odepth_image,
depth_scale=1.0 / depth_scale,
depth_trunc=10, # set to 10 metres
convert_rgb_to_intensity=False)
temp = o3d.geometry.PointCloud.create_from_rgbd_image(
rgbd_image, intrinsic)
temp.transform(flip_transform)
# temp = temp.voxel_down_sample(0.03)
# Point cloud output of frame is appended to the list
if output_mode == 1 or output_mode == 2:
output_list.append(temp)
# image format conversion for cv2 visualization/output
if output_mode == 0 or output_mode == 1 or save_images == True:
pred_color = cv2.cvtColor(pred_color, cv2.COLOR_RGB2BGR)
color_image = cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR)
edges = cv2.cvtColor(edges, cv2.COLOR_GRAY2BGR)
## for displaying seg_mask
seg_mask = (np.array(seg_mask) * 255).astype(np.uint8)
seg_mask = cv2.cvtColor(
seg_mask, cv2.COLOR_GRAY2BGR) # segmentation binary mask
final_output_mask = cv2.cvtColor(
planes_mask_binary,
cv2.COLOR_GRAY2BGR) # final traversable path mask
if output_mode == 0 or output_mode == 1:
# # Blending rgb and depth images for display - can check alignment with this as well
alpha = 0.2
beta = (1.0 - alpha)
dst = cv2.addWeighted(
color_image, alpha, pred_color, beta, 0.0
) # color and segmentation output from ADE20K model blended
dst2 = cv2.addWeighted(depth_color_image, alpha, color_image,
beta,
0.0) # color and depth blended together
##################################
### delete later if needed - color image masked by final traversable path
final_output = color_image * planes_mask_binary_3d
mask = (final_output[:, :, 0] == 0) & (
final_output[:, :, 1] == 0) & (final_output[:, :, 2] == 0)
final_output[:, :, :3][mask] = [255, 255, 255]
######
### Select outputs for visualization - we've chosen some as default
image_set1 = np.vstack((dst, dst2))
# image_set2 = np.vstack((planes_mask_binary_3d*255, seg_mask))
# image_set2 = np.vstack((dst, final_output))
image_set2 = np.vstack((edges, final_output))
### Choose which images you want to display from above
combined_images = np.concatenate((image_set1, image_set2),
axis=1)
if save_images == True:
# Outputs saved - you can modify this
cv2.imwrite("output/visualization/frame%d.png" % idx,
combined_images)
cv2.imwrite("data/color/frame%d.png" % idx,
color_image) # save frame as JPEG file
cv2.imwrite("data/depth/frame%d.png" % idx,
depth_array) # save frame as JPEG file
cv2.imwrite("output/edges/frame%d.png" % idx,
edges) # save frame as JPEG file
cv2.imwrite("output/segmentation_mask/frame%d.png" % idx,
seg_mask) # save frame as JPEG file
cv2.imwrite("output/output_mask/frame%d.png" % idx,
final_output_mask) # save frame as JPEG file
if output_mode == 0 or output_mode == 1:
try:
cv2.imshow('Full Stream', combined_images)
except TypeError as e:
print(idx, e)
key = cv2.waitKey(1)
# if pressed escape exit program
if key == 27:
cv2.destroyAllWindows()
break
finally:
pipeline.stop()
if output_mode == 0 or output_mode == 1:
cv2.destroyAllWindows()
return
def run_loop(bag_path, seg_model, seg_opts, save_images=False):
# Create pipeline
pipeline = rs.pipeline()
# Create a config object
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)
# Start streaming from file
Pipe = pipeline.start(config)
# Getting the depth sensor's depth scale (see rs-align example for explanation)
depth_sensor = Pipe.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
print("Depth Scale is: ", depth_scale)
# Create opencv window to render image in
cv2.namedWindow("Full Stream", cv2.WINDOW_NORMAL)
# Create colorizer object
colorizer = rs.colorizer()
idx = 0
# initial frame delay
idx_limit = 30
pre_seg_mask_sum = None # previous frame path segmentation area
# Streaming loop
try:
while True:
idx += 1
# Get frameset of depth
frames = pipeline.wait_for_frames()
# ignore first idx frames
if idx < idx_limit:
continue
else:
pass
align = rs.align(rs.stream.color)
frames = align.process(frames)
# Get color frame
color_frame = frames.get_color_frame()
# Get depth frame
depth_frame = frames.get_depth_frame()
# Get intrinsics and extrinsics
if idx == idx_limit:
camera_intrinsics(color_frame, depth_frame, Pipe)
color_image = np.asanyarray(color_frame.get_data())
### Add Segmentation part here ###
pred = test(color_image, seg_model, seg_opts)
# pavement, floor, road, earth/ground, field, path, dirt/track
seg_mask = (pred == 11) | (pred == 3) | (pred == 6) | (
pred == 13) | (pred == 29) | (pred == 52) | (
pred == 91) #.astype(np.uint8)
if idx == idx_limit: # 1st frame detection needs to be robust
pre_seg_mask_sum = np.sum(seg_mask)
# checking for bad detection
new_seg_sum = np.sum(seg_mask)
diff = abs(new_seg_sum - pre_seg_mask_sum)
# if diff > pre_seg_mask_sum/15: # smoothening between segmentation outputs - seems like a bad idea since the model inputs are not connected between timesteps
# seg_mask = np.ones_like(pred).astype(np.uint8) # need to add depth (5mt) criterea for calculation for robustness
# del new_seg_sum
# else:
pre_seg_mask_sum = new_seg_sum
### mask Hole filling
seg_mask = nd.binary_fill_holes(seg_mask).astype(int)
seg_mask = seg_mask.astype(np.uint8)
#####
seg_mask_3d = np.dstack((seg_mask, seg_mask, seg_mask))
pred_color = colorEncode(
pred,
loadmat(os.path.join(model_folder, 'color150.mat'))['colors'])
##################################
depth_frame = depth_filter(depth_frame)
depth_array = np.asarray(depth_frame.get_data())
# Colorize depth frame to jet colormap
depth_color_frame = colorizer.colorize(depth_frame)
# Convert depth_frame to numpy array to render image in opencv
depth_color_image = np.asanyarray(depth_color_frame.get_data())
############ Plane Detection
## need to add smoothening between frames - by plane weights' variance?
try:
### need to add multithreading here (and maybe other methods?)
planes_mask_binary = plane_detection(color_image*seg_mask_3d, depth_array*seg_mask,\
loop=5)
except TypeError as e:
try:
print("plane mask 1st error")
planes_mask, planes_normal, list_plane_params = test_PlaneDetector_send(
img_color=color_image * seg_mask_3d,
img_depth=depth_array * seg_mask)
except TypeError as e:
print("plane mask not detected-skipping frame")
continue
## removed this part
planes_mask = np.ones_like(depth_array).astype(np.uint8)
planes_mask = np.dstack(
(planes_mask, planes_mask, planes_mask))
##############################################
## Hole filling for plane_mask (plane mask isn't binary - fixed that!)
planes_mask_binary = nd.binary_fill_holes(planes_mask_binary)
planes_mask_binary = planes_mask_binary.astype(np.uint8)
# Clean plane mask object detection by seg_mask
planes_mask_binary *= seg_mask
planes_mask_binary_3d = np.dstack(
(planes_mask_binary, planes_mask_binary, planes_mask_binary))
edges = planes_mask_binary - nd.morphology.binary_dilation(
planes_mask_binary) # edges calculation
edges = cv2.cvtColor(edges, cv2.COLOR_GRAY2BGR)
#############################################
# for cv2 output
pred_color = cv2.cvtColor(pred_color, cv2.COLOR_RGB2BGR)
color_image = cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR)
# if save_images == True:
# cv2.imwrite("data_/color/frame%d.png" % idx, color_image) # save frame as JPEG file
# cv2.imwrite("data_/depth/frame%d.png" % idx, depth_array) # save frame as JPEG file
# cv2.imwrite("data_/color_depth/frame%d.png" % idx, depth_color_image) # save frame as JPEG file
# cv2.imwrite("data_/thresholded_color/frame%d.png" % idx, thresholded_color_image) # save frame as JPEG file
# # cv2.imwrite("data_/thresholded_depth/frame%d.png" % idx, thresholded_depth_image) # save frame as JPEG file
# # Blending images
alpha = 0.2
beta = (1.0 - alpha)
dst = cv2.addWeighted(color_image, alpha, pred_color, beta, 0.0)
# kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(11,11))
# res = cv2.morphologyEx(planes_mask,cv2.MORPH_OPEN,kernel)
dst2 = cv2.addWeighted(depth_color_image, alpha, color_image, beta,
0.0)
## for displaying seg_mask
seg_mask = (np.array(seg_mask) * 255).astype(np.uint8)
seg_mask = cv2.cvtColor(seg_mask, cv2.COLOR_GRAY2BGR)
##################################
### delete later
final_output = color_image * planes_mask_binary_3d
mask = (final_output[:, :, 0] == 0) & (
final_output[:, :, 1] == 0) & (final_output[:, :, 2] == 0)
final_output[:, :, :3][mask] = [255, 255, 255]
######
# if np.sum(planes_mask) == depth_array.shape[0]*depth_array.shape[1]:
# image_set1 = np.vstack((dst, color_image))
# else:
image_set1 = np.vstack((color_image, depth_color_image))
# image_set2 = np.vstack((planes_mask_binary_3d*255, seg_mask))
image_set2 = np.vstack((dst, final_output))
# image_set2 = np.vstack((edges, final_output))
combined_images = np.concatenate((image_set1, image_set2), axis=1)
if save_images == True:
cv2.imwrite("./meeting_example/frame%d.png" % idx,
combined_images)
try:
cv2.imshow('Full Stream', combined_images)
except TypeError as e:
print(idx, e)
key = cv2.waitKey(1)
# if pressed escape exit program
if key == 27:
cv2.destroyAllWindows()
break
finally:
pipeline.stop()
cv2.destroyAllWindows()
# if save_images == True:
# pkl.dump( threshold_mask, open( "data_/depth_threshold.pkl", "wb" ) )
# print("Mask pickle saved")
return
def run_loop(bag_path, seg_model, seg_opts):
# Create pipeline
pipeline = rs.pipeline()
# Create a config object
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)
# Start streaming from file
Pipe = pipeline.start(config)
# Getting the depth sensor's depth scale (see rs-align example for explanation)
depth_sensor = Pipe.get_device().first_depth_sensor()
depth_scale = depth_sensor.get_depth_scale()
print("Depth Scale is: ", depth_scale)
# Create colorizer object
colorizer = rs.colorizer()
idx = 0
# initial frame delay
idx_limit = 30
vis = x3d.visualization.Visualizer()
vis.create_window()
ocgd = x3d.geometry.OccupancyGrid(0.03, 512)
flip_transform = [[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]
# pre_seg_mask_sum = None # previous frame path segmentation area
# Streaming loop
frame_count = 0
try:
while True:
idx += 1
# Get frameset of depth
frames = pipeline.wait_for_frames()
# ignore first idx frames
if idx < idx_limit:
continue
else:
pass
align = rs.align(rs.stream.color)
frames = align.process(frames)
# Get color frame
color_frame = frames.get_color_frame()
# Get depth frame
depth_frame = frames.get_depth_frame()
# Get intrinsics and extrinsics
if idx == idx_limit:
camera_intrinsics(color_frame, depth_frame, Pipe)
# intrinsic = x3d.camera.PinholeCameraIntrinsic(get_intrinsic_matrix(color_frame))
intrinsics = color_frame.profile.as_video_stream_profile(
).intrinsics
# color_intrin = color_frame.profile.as_video_stream_profile().intrinsics
# intrinsic = x3d.camera.PinholeCameraIntrinsic((color_intrin))
intrinsic = x3d.camera.PinholeCameraIntrinsic(
640, 480, intrinsics.fx, intrinsics.fy, intrinsics.ppx,
intrinsics.ppy)
color_image = np.asanyarray(color_frame.get_data())
### Add Segmentation part here ###
pred = test(color_image, seg_model, seg_opts)
# pavement, floor, road, earth/ground, field, path, dirt/track
seg_mask = (pred == 11) | (pred == 3) | (pred == 6) | (
pred == 13) | (pred == 29) | (pred == 52) | (
pred == 91) #.astype(np.uint8)
# seg_mask = np.ones((color_image.shape[0], color_image.shape[1]))
# if idx == idx_limit: # 1st frame detection needs to be robust
# pre_seg_mask_sum = np.sum(seg_mask)
# checking for bad detection
# new_seg_sum = np.sum(seg_mask)
# diff = abs(new_seg_sum-pre_seg_mask_sum)
# if diff > pre_seg_mask_sum/15: # smoothening between segmentation outputs - seems like a bad idea since the model inputs are not connected between timesteps
# seg_mask = np.ones_like(pred).astype(np.uint8) # need to add depth (5mt) criterea for calculation for robustness
# del new_seg_sum
# else:
# pre_seg_mask_sum = new_seg_sum
### mask Hole filling
seg_mask = nd.binary_fill_holes(seg_mask).astype(int)
seg_mask = seg_mask.astype(np.uint8)
#####
seg_mask_3d = np.dstack((seg_mask, seg_mask, seg_mask))
# pred_color = colorEncode(pred, loadmat(os.path.join(model_folder, 'color150.mat'))['colors'])
##################################
depth_frame = depth_filter(depth_frame)
depth_array = np.asarray(depth_frame.get_data())
# Colorize depth frame to jet colormap
depth_color_frame = colorizer.colorize(depth_frame)
# Convert depth_frame to numpy array to render image in opencv
depth_color_image = np.asanyarray(depth_color_frame.get_data())
############ Plane Detection
## need to add smoothening between frames - by plane weights' variance?
try:
### need to add multithreading here (and maybe other methods?)
planes_mask_binary = plane_detection(color_image*seg_mask_3d, depth_array*seg_mask,\
loop=3)
except TypeError as e:
try:
print("plane mask 1st error")
planes_mask, planes_normal, list_plane_params = test_PlaneDetector_send(
img_color=color_image * seg_mask_3d,
img_depth=depth_array * seg_mask)
except TypeError as e:
print("plane mask not detected-need to skip frames")
##############################################
## Hole filling for plane_mask (plane mask isn't binary - fixed that!)
planes_mask_binary = nd.binary_fill_holes(planes_mask_binary)
planes_mask_binary = planes_mask_binary.astype(np.uint8)
# Clean plane mask object detection by seg_mask
planes_mask_binary *= seg_mask
planes_mask_binary_3d = np.dstack(
(planes_mask_binary, planes_mask_binary, planes_mask_binary))
#############################################
depth_image = x3d.geometry.Image(depth_array * planes_mask_binary)
color_image = x3d.geometry.Image(color_image *
planes_mask_binary_3d)
rgbd_image = x3d.geometry.RGBDImage.create_from_color_and_depth(
color_image,
depth_image,
depth_scale=1.0 / depth_scale,
depth_trunc=10,
convert_rgb_to_intensity=False)
temp = x3d.geometry.PointCloud.create_from_rgbd_image(
rgbd_image, intrinsic)
temp.transform(flip_transform)
temp = temp.voxel_down_sample(0.03)
ocgd.insert(temp, np.zeros(3))
if frame_count == 0:
vis.add_geometry(ocgd)
vis.update_geometry(ocgd)
vis.poll_events()
vis.update_renderer()
# dt1 = datetime.now()
# process_time = dt1 - dt0
# print("FPS: " + str(1 / process_time.total_seconds()))
frame_count += 1
finally:
pipeline.stop()
vis.destroy_window()
return