Merge 2 point clouds created with intel real sense d435i

[elenadipietro]

Hi everyone! I'm using this code to merge two point clouds created using 2 cameras intel real sense d435i. I calculated the rotation matrix and the traslation vector between the 2 with the calibration. The problem is that very often the 2 point clouds are not aligned, what should be the problem? This is the code I'm using to merge them: import pyrealsense2 as rs import numpy as np from enum import IntEnum from datetime import datetime import open3d as o3d

from os.path import abspath import sys sys.path.append(abspath(file)) import pyrealsense2 as rs

def get_profiles(device_index): ctx = rs.context() #contesto devices = ctx.query_devices() #lista dei dispositivi che sono connessi al pc #inizializzazione liste vuote per colore e profondità color_profiles = [] depth_profiles = []

device = devices[device_index]
name = device.get_info(rs.camera_info.name)
serial = device.get_info(rs.camera_info.serial_number)
print('Sensor: {}, {}'.format(name, serial)) #per ogni telecamera viene stampato nome e numero seriale
for sensor in device.query_sensors(): #viene iterato su ciascun dispositivo
    for stream_profile in sensor.get_stream_profiles(): #vengono ottenuti i profili di flusso per ciascun dispositivo (dati relativi al flusso video)
        stream_type = str(stream_profile.stream_type()) #viene ottenuto il tipo di flusso del profilo corrente

        if stream_type in ['stream.color', 'stream.depth']: #se questo tipo di flusso è colore o profondità_
            v_profile = stream_profile.as_video_stream_profile()
            fmt = stream_profile.format() #formato
            w, h = v_profile.width(), v_profile.height() #w è larghezza del frame video, h è altezza del frame video ma sempre tutti in pixel
            fps = v_profile.fps() #frame per second

            video_type = stream_type.split('.')[-1]
            #in base al tipo di flusso si aggiunge il profilo alla lista appropriata
            if video_type == 'color':
                color_profiles.append((w, h, fps, fmt))
            else:
                depth_profiles.append((w, h, fps, fmt))

return color_profiles, depth_profiles, serial

#definisce alcune costanti numeriche per rappresentare preset di configurazioni class Preset(IntEnum): Custom = 0 Default = 1 Hand = 2 HighAccuracy = 3 HighDensity = 4 MediumDensity = 5

def get_intrinsic_matrix(frame): #funzione che prende un frame come input e restituisce la matrice intrinseca associata a quel frame intrinsics = frame.profile.as_video_stream_profile().intrinsics #estrae le informazioni intrinseche del frame e converte il profilo in un profilo di flusso video out = o3d.camera.PinholeCameraIntrinsic(640, 480, intrinsics.fx, # crea un oggetto PinholeCameraIntrinsic dalla libreria Open3D; questo oggetto rappresenta intrinsics.fy, intrinsics.ppx, #i parametri intrinseci della telecamera come lunghezza focale asse x e y(fx, fy) intrinsics.ppy) #punto principale x e y - 640 e 480 sono altezza e larghezza in questo caso (sempre in pixel) return out

#def convert_depth_frame(depth_frame, depth_scale): #converte un frame di profondità in un oggetto immagine utilizzato da Open3D. depth_array = np.asarray(depth_frame.get_data(), dtype=np.uint16) #converte il frame di profondità in un array NumPy di tipo uint16 depth_array = depth_array * depth_scale # moltiplica tutti i valori nell'array di profondità per una scala specifica depth_scale -> per convertire per esempio in metri if depth_array.max() == 0: #controlla se il valore massimo nell'array di profondità è zero depth_array_scaled = depth_array.astype(np.uint8) #se il valore massimo è zero, viene semplicemente convertito l'array di profondità in un array di tipo uint8 else: depth_array_scaled = (depth_array / depth_array.max() * 255).astype(np.uint8) #i dati di profondità vengono normalizzati dividendo per il valore massimo e quindi moltiplicati per 255 per portare i valori nella gamma 0-255 richiesta per un'immagine a 8 bit return o3d.geometry.Image(depth_array_scaled)

def config_enable_stream(config, c_pro, d_pro, serial): #configura il flusso di dati provenienti da una fotocamera RealSense; sembrano abilitare lo streaming dei dati di profondità e colore con specifiche di risoluzione, formato e frame rate predefiniti config.enable_device(serial) w, h, fps, fmt = d_pro[0] config.enable_stream(rs.stream.depth, w, h, fmt, fps) w, h, fps, fmt = c_pro[0] config.enable_stream(rs.stream.color, w, h, fmt, fps) return config

if name == "main": #si creano 2 pipeline che rappresentano il flusso di dati dalla telecamera RealSense al computer pipeline_1 = rs.pipeline() pipeline_2 = rs.pipeline()

#Create 2 config and configure the pipeline to stream
#  different resolutions of color and depth streams

config_1 = rs.config()
config_2 = rs.config()

color_profiles, depth_profiles, serial = get_profiles(1)
config_1 = config_enable_stream(config_1, color_profiles, depth_profiles, serial)
color_profiles, depth_profiles, serial = get_profiles(0)
config_2 = config_enable_stream(config_2, color_profiles, depth_profiles, serial)


# Start streaming
profile_1 = pipeline_1.start(config_1)
depth_sensor_1 = profile_1.get_device().first_depth_sensor() #viene ottenuto il profilo in uso e il primo sensore di profondità associato al dispositivo
profile_2 = pipeline_2.start(config_2)
depth_sensor_2 = profile_2.get_device().first_depth_sensor()

    # Using preset HighAccuracy for recording
depth_sensor_1.set_option(rs.option.visual_preset, Preset.HighAccuracy) #imposta la modalità di visualizzazione del sensore di profondità su "HighAccuracy"
depth_sensor_1.set_option(rs.option.emitter_always_on,1) #imposta il controllo del proiettore di luce strutturata (emettitore) del sensore di profondità in modalità "sempre acceso" (1)
depth_sensor_2.set_option(rs.option.visual_preset, Preset.HighAccuracy) #imposta la modalità di visualizzazione del sensore di profondità su "HighAccuracy"
depth_sensor_2.set_option(rs.option.emitter_always_on,1) #imposta il controllo del proiettore di luce strutturata (emettitore) del sensore di profondità in modalità "sempre acceso" (1)

# Getting the depth sensor's depth scale
depth_scale_1 = depth_sensor_1.get_depth_scale()  #ottiene il valore di scala della profondità dal sensore:quanta profondità è rappresentata da un singolo
                                             #passo nell'immagine di profondità acquisita

depth_scale_2 = depth_sensor_2.get_depth_scale() 
                                             
# We will not display the background of objects more than clipping_distance_in_meters meters away
clipping_distance_in_meters = 3 #gli oggetti situati oltre questa distanza non verranno visualizzati nella rappresentazione visuale della profondità
clipping_distance_1 = clipping_distance_in_meters / depth_scale_1 #questo calcolo converte la distanza di taglio desiderata in unità che corrispondono  
clipping_distance_2 = clipping_distance_in_meters / depth_scale_2 #questo calcolo converte la distanza di taglio desiderata in unità che corrispondono  


# Create an align object
align_to = rs.stream.color #si allineano i frame di profondità al flusso di colore
align_1 = rs.align(align_to) #serve per allineare i frame di profondità con i frame a colori
align_2 = rs.align(align_to)

vis = o3d.visualization.Visualizer()  #viene creato un oggetto Visualizer di Open3D per la visualizzazione 3D
vis.create_window() #si apre la finestra

pcd_1 = o3d.geometry.PointCloud() #oggetto PointCloud di Open3D, che verrà utilizzato per rappresentare la nuvola di punti
pcd_2 = o3d.geometry.PointCloud() 

flip_transform = [[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]]  ##è una matrice di trasformazione che esegue una trasformazione per "capovolgere" la nuvola di punti lungo l'asse y

transformation_initial = [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]

frame_count = 0
try:
    while True:
        dt0 = datetime.now()

        # Get frameset of color and depth
        frames_1 = pipeline_1.wait_for_frames()
        frames_2 = pipeline_2.wait_for_frames()
        
        aligned_frames_1 = align_1.process(frames_1)
        aligned_frames_2 = align_2.process(frames_2)
        
        depth_frame_1 = aligned_frames_1.get_depth_frame()
        depth_frame_2 = aligned_frames_2.get_depth_frame()
        color_frame_1 = aligned_frames_1.get_color_frame()
        color_frame_2 = aligned_frames_2.get_color_frame()
        
        intrinsic_1 = o3d.camera.PinholeCameraIntrinsic(get_intrinsic_matrix(color_frame_1))
        intrinsic_2 = o3d.camera.PinholeCameraIntrinsic(get_intrinsic_matrix(color_frame_2))
        # Validate that both frames are valid
        if not depth_frame_1 or not color_frame_1 or not depth_frame_2 or not color_frame_2:
            continue
        
        depth_image_1 = o3d.geometry.Image(np.array(depth_frame_1.get_data()))
        color_image_1 = o3d.geometry.Image(np.array(color_frame_1.get_data()))
        depth_image_2 = o3d.geometry.Image(np.array(depth_frame_2.get_data()))
        color_image_2 = o3d.geometry.Image(np.array(color_frame_2.get_data()))
        
        rgbd_image_1 = o3d.geometry.RGBDImage.create_from_color_and_depth(
            color_image_1,
            depth_image_1,
            depth_scale=1.0 / depth_scale_1,
            depth_trunc=clipping_distance_in_meters,
            convert_rgb_to_intensity=False
        )
        rgbd_image_2 = o3d.geometry.RGBDImage.create_from_color_and_depth(
            color_image_2,
            depth_image_2,
            depth_scale=1.0 / depth_scale_2,
            depth_trunc=clipping_distance_in_meters,
            convert_rgb_to_intensity=False
        )

        reconstruct_3d_pc_1 = o3d.geometry.PointCloud.create_from_rgbd_image(
            rgbd_image_1, intrinsic_1
        )
        reconstruct_3d_pc_2 = o3d.geometry.PointCloud.create_from_rgbd_image(
            rgbd_image_2, intrinsic_2
        )
        
        # Applica la trasformazione dalla telecamera 1 alla telecamera 2
        
        rotation_matrix = np.array([[ 0.7374363,   0.00391701,  0.67540533],
                                    [ 0.06240363,  0.99531076, -0.07390724],
                                    [-0.67252769,  0.09664962,  0.73373385]


                                                                                ])

        translation_vector = np.array([[-0.610775419378742,	-0.0107363310132382,	0.255119810184988]
                                        ]) 
        transform_matrix = np.eye(4) #inizializza una matrice di trasformazione transform_matrix come una matrice identità 4x4.
        transform_matrix[:3, :3] = rotation_matrix
        transform_matrix[:3, 3] = translation_vector #quarta colonna; flatten() è necessario se translation_vector è inizialmente un array multidimensional

        
        # Ottenere le coordinate dei punti dalla nuvola di punti della telecamera 1
        points = np.asarray(reconstruct_3d_pc_1.points) # è un array numpy contenente le coordinate (x, y, z) dei punti nella nuvola di punti della telecamera 1.
        #transformed_points = points
        # Moltiplicare manualmente le coordinate dei punti per la matrice di trasformazione
        transformed_points = np.dot(points, np.transpose(transform_matrix[:3,:3])) + transform_matrix[:3,3]

        # Aggiornare le coordinate dei punti nella nuvola di punti della telecamera 1 con le coordinate trasformate
        reconstruct_3d_pc_1.points = o3d.utility.Vector3dVector(transformed_points) #sostituisce le coordinate dei punti nella nuvola di punti della telecamera 1 con le nuove coordinate trasformate rappresentate dall'array transformed_points.

        
        reconstruct_3d_pc_1.transform(flip_transform)
        reconstruct_3d_pc_2.transform(flip_transform)

        pcd_1.points = reconstruct_3d_pc_1.points
        pcd_1.colors = reconstruct_3d_pc_1.colors
        pcd_2.points = reconstruct_3d_pc_2.points
        pcd_2.colors = reconstruct_3d_pc_2.colors
        
        
        if frame_count == 0:
            vis.add_geometry(pcd_1)
            vis.add_geometry(pcd_2)
        vis.update_geometry(pcd_1)   
        vis.update_geometry(pcd_2)
        vis.poll_events()
        vis.update_renderer()

        process_time = datetime.now() - dt0
        print("\rFPS: " + str(1 / process_time.total_seconds()), end='')
        frame_count += 1

finally:
    pipeline_1.stop()
    pipeline_2.stop()
    vis.destroy_window()

Can it be a problem of calibration maybe?

Thanks everyone for the help