isl-org / isl-org/Open3D

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

Open
#7,113 0 comments 0 reactions 0 assignees View on GitHub
question
Dominant language
C++
Stars
14k
Forks
2.6k
Avg merge
5d 18h
Merged PRs (30d)
6

Description

### Checklist

- [X] I have searched for [similar issues](https://github.com/isl-org/Open3D/issues).
- [X] For Python issues, I have tested with the [latest development wheel](https://www.open3d.org/docs/latest/getting_started.html#development-version-pip).
- [X] I have checked the [release documentation](https://www.open3d.org/docs/release/) and the [latest documentation](https://www.open3d.org/docs/latest/) (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()
```

Contributor guide

No contributing guide indexed for this repository

Assessment

This issue has not been assessed yet.

Get new issues in your inbox

A short digest of beginner-friendly GitHub issues.