Can't merge two pointclouds

[darsavelidze]

---

Required Info
Camera Model	D415
Firmware Version	5.16.0.1
Operating System & Version	Win (10)
Platform	PC
SDK Version	2.56.3
Language	python, open3d
Segment	{VR/others }

Issue Description

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.

[darsavelidze]

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()

[MartyG-RealSense]

Hi @darsavelidze An Open3D / Python reference at #9590 for stitching two pointclouds with ICP might be helpful to you.