How to get correct transformation matrix for two depths from realsense d415 cameras

[darsavelidze]

Checklist

• I have searched for similar issues.
• For Python issues, I have tested with the latest development wheel.
• I have checked the release documentation and the latest documentation (for main branch).

My Question

Hello! I have two RealSense D415 cameras. I am trying to merge two point clouds. The relative position between the cameras is fixed. I calibrated them using the stereoCameraCalibrator module based on the RGB stream. It provided me with a rotation matrix and a translation vector. However, when I apply them, the second camera rotates the second point cloud correctly but does not move the camera to the correct position.

I doubt that calibration based on RGB can be accurately used for point clouds, as depth uses different parameters. The calculation in stereoCameraCalibrator was in millimeters, and I converted it to meters, but it still doesn’t align correctly.

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__))
from devices import *


class Preset(IntEnum):
    Custom = 0
    Default = 1
    Hand = 2
    HighAccuracy = 3
    HighDensity = 4
    MediumDensity = 5


def get_intrinsic_matrix(frame):
    intrinsics = frame.profile.as_video_stream_profile().intrinsics
    out = o3d.camera.PinholeCameraIntrinsic(640, 480, intrinsics.fx,
                                            intrinsics.fy, intrinsics.ppx,
                                            intrinsics.ppy)
    return out


rotation_matrix = np.array([[0.68004686, 0.00374204, -0.7331591],
                            [0.01934606, 0.9995472, 0.02304625],
                            [0.73291336, -0.02985627, 0.67966654]])

translation_vector = np.array([-363.090602, 0.190083449, -19.4196045]) / 1000


def create_transformation_matrix(rotation, translation):
    transformation = np.eye(4)  # Создаём единичную матрицу 4x4
    transformation[:3, :3] = np.array(rotation).reshape(3, 3)  # Матрица вращения
    transformation[:3, 3] = translation  # Вектор трансляции
    return transformation


transformation_matrix = create_transformation_matrix(rotation_matrix, translation_vector)

if __name__ == "__main__":
    c = rs.config()
    c.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 60)
    c.enable_stream(rs.stream.color, 640, 480, rs.format.rgb8, 60)
    c.enable_stream(rs.stream.infrared, 1, 640, 480, rs.format.y8, 60)

    device_manager = DeviceManager(rs.context(), c)
    device_manager.enable_all_devices(enable_ir_emitter=True)
    colorizer = rs.colorizer()

    vis = o3d.visualization.Visualizer()
    vis.create_window()

    pcd = o3d.geometry.PointCloud()
    flip_transform = [[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()
            frames = device_manager.poll_frames()

            combined_pcd = o3d.geometry.PointCloud()

            for dev_info, frame_set in frames.items():
                color_frame = frame_set.get(rs.stream.color)
                depth_frame = frame_set.get(rs.stream.depth)

                if not color_frame or not depth_frame:
                    continue

                color_image = np.asanyarray(color_frame.get_data())
                depth_image_raw = np.asanyarray(depth_frame.get_data())

                depth_image = o3d.geometry.Image(depth_image_raw)
                color_image_o3d = o3d.geometry.Image(color_image)

                intrinsic = get_intrinsic_matrix(color_frame)

                rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(
                    color_image_o3d,
                    depth_image,
                    depth_scale=3000.0,
                    convert_rgb_to_intensity=False
                )

                temp_pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_image, intrinsic)

                # Применяем трансформацию для второй камеры
                if dev_info[0] == '916512060244':  # Укажите серийный номер второй камеры
                    temp_pcd.transform(transformation_matrix)

                temp_pcd.transform(flip_transform)

                combined_pcd += temp_pcd

            pcd.points = combined_pcd.points
            pcd.colors = combined_pcd.colors

            if frame_count == 0:
                vis.add_geometry(pcd)

            vis.update_geometry(pcd)
            vis.poll_events()
            vis.update_renderer()

            process_time = datetime.now() - dt0
            print("\rFPS: {:.2f}".format(1 / process_time.total_seconds()), end='')
            frame_count += 1

    finally:
        device_manager.disable_streams()
        vis.destroy_window()