Hi!
I have a problem with generating a point cloud with 100% correct RGB colors. There is some problem, either in Python wrapper or my code (below). Namely, there is some places, where point cloud colors are being wrong, i.e. instead of gray, they are purple-blue. Look at the screen below:
This is how it should look like (via ZED Depth Viewer):
Processing: 2022-10-21 11_39_08-performance.png…
And this is how it looks, when I open generated point cloud in CloudCompare:
Processing: 2022-10-26 17_26_06-Window.png…
As you can see, part of the cloud points has correct color (i.e. grass is green and paving is grey), so channels are not swapped (tried all combinations anyway).
Can you check my code? Do you have any suggestions?
Thank you!
Code:
import pyzed.sl as sl
import numpy as np
import sys
from PIL import Image
import struct
def main():
# Create a Camera object
print('create camera')
zed = sl.Camera()
# Create a InitParameters object and set configuration parameters
print('create init param')
init_params = sl.InitParameters()
init_params.camera_resolution = sl.RESOLUTION.HD2K # HD2K, HD1080, HD720, VGA
init_params.camera_fps = 15 # HD2K - 15, HD1080 - 15, 30, HD720 - 15, 30, 60, VGA - 15, 30, 60, 100
init_params.depth_mode = sl.DEPTH_MODE.NEURAL # NONE, PERFORMANCE, QUALITY, ULTRA, NEURAL
init_params.coordinate_units = sl.UNIT.METER # METER, CENTIMETER, MILLIMETER, INCH, FOOT
init_params.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Z_UP_X_FWD # Z up, X forward, Y right
init_params.depth_minimum_distance = 0.2 # [m]
init_params.depth_maximum_distance = 6
init_params.depth_stabilization = True
# Open the camera
print('open camera')
err = zed.open(init_params)
if err != sl.ERROR_CODE.SUCCESS:
exit(1)
print('ERROR when opening')
# Create and set RuntimeParameters after opening the camera
print('create runtime param')
runtime_parameters = sl.RuntimeParameters()
runtime_parameters.sensing_mode = sl.SENSING_MODE.STANDARD # STANDARD, FILL
runtime_parameters.confidence_threshold = 100 # range [1, 100]. A lower value means more confidence and precision (but less density).
runtime_parameters.texture_confidence_threshold = 100 # range [1, 100]. A lower value means more confidence and precision (but less density)
runtime_parameters.remove_saturated_areas = True # Defines if the saturated area (Luminance>=255) must be removed from depth map estimationd.
# Capture images and depth, then stop
image = sl.Mat()
point_cloud = sl.Mat()
# A new image is available if grab() returns SUCCESS
print('grab')
if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
# FOTO:
print('foto')
zed.retrieve_image(image, sl.VIEW.LEFT) # Get the left image
image_np = image.get_data() # image data to numpy matrix
image_np = image_np[:, :, [2, 1, 0, 3]] # BGRA --> RGBA
image_pil = Image.fromarray(image_np) # RGBA to PIL object
print('save foto')
image_pil.save("zed_img.png") # save PIL object to image file
# POINT CLOUD:
print('point cloud')
zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA) # Retrieve colored point cloud. Point cloud is aligned on the left image.
point_cloud_np = point_cloud.get_data() # point cloud data to numpy matrix
point_cloud_np = point_cloud_np.reshape(-1, 4) # numpy matrix reshape
point_cloud_np = point_cloud_np[np.isfinite(point_cloud_np).any(axis=1)] # delete NaN and INF rows
color = point_cloud_np[:, 3] # isolate point cloud color column (4x8bit)
color = np.array(list(map(color_decode, color))) # decode colors to RGBA 0..255
point_cloud_np = np.delete(point_cloud_np, 3, 1) # delete 32bit color column
point_cloud_np = np.append(point_cloud_np, color, axis=1) # add 0..255 RGBA columns
print('pc shape: ', point_cloud_np.shape)
print('file save')
np.savetxt("point_cloud.txt", point_cloud_np, delimiter=", ", fmt='%1.4f %1.4f %1.4f %d %d %d %d')
print('flush')
sys.stdout.flush()
# Close the camera
zed.close()
def color_decode(x):
packed = struct.pack('f', x)
r, g, b, a = struct.unpack('BBBB', packed)
return r, g, b, a
if __name__ == "__main__":
print('start')
main()