Realsense学习--双相机点云实时注册

思路

利用双相机的棋盘校正内角点的空间坐标推导出其中一个相机点云相对另一个相机点云的坐标系的空间转换矩阵。

代码

import pyrealsense2 as rs
import cv2
import numpy as np
import time
import os
import sys
import open3d as o3d  
from tools import rgbdTools, registration


if __name__ == '__main__':

    # 有几个棋盘校正图
    chessBoard_num = 4

    # 确定图像的输入分辨率与帧率
    resolution_width = 1280  # pixels
    resolution_height = 720  # pixels
    frame_rate = 15  # fps

    # 注册数据流，并对其图像
    align = rs.align(rs.stream.color)
    rs_config = rs.config()
    rs_config.enable_stream(rs.stream.depth, resolution_width, resolution_height, rs.format.z16, frame_rate)
    rs_config.enable_stream(rs.stream.color, resolution_width, resolution_height, rs.format.bgr8, frame_rate)

    # check相机是不是进来了
    connect_device = []
    for d in rs.context().devices:
        if d.get_info(rs.camera_info.name).lower() != 'platform camera':
            connect_device.append(d.get_info(rs.camera_info.serial_number))

    if len(connect_device) < 2:
        print('Registrition needs two camera connected.But got one.')
        exit()

    # 确认相机并获取相机的内部参数
    pipeline1 = rs.pipeline()
    rs_config.enable_device(connect_device[0])
    pipeline_profile1 = pipeline1.start(rs_config)

    # 获取深度相机的内部参数
    intr1 = pipeline_profile1.get_stream(rs.stream.color).as_video_stream_profile().get_intrinsics()
    # 将参数传入open3d的小孔相机模型中，为下一步的建模与显示做准备，width为图像的宽度，height为图像的高度，fx、fy为xy轴的焦距。ppx、ppy为xy轴的原理点
    pinhole_camera1_intrinsic = o3d.camera.PinholeCameraIntrinsic(intr1.width, intr1.height, intr1.fx, intr1.fy