How to get correct transformation matrix for two depths from realsense d415 cameras
- 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.