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, intr1.ppx, intr1.ppy)
    cam1 = rgbdTools.Camera(intr1.fx, intr1.fy, intr1.ppx, intr1.ppy)

    pipeline2 = rs.pipeline()
    rs_config.enable_device(connect_device[1])
    pipeline_profile2 = pipeline2.start(rs_config)

    intr2 = pipeline_profile2.get_stream(rs.stream.color).as_video_stream_profile().get_intrinsics()
    pinhole_camera2_intrinsic = o3d.camera.PinholeCameraIntrinsic(intr2.width, intr2.height, intr2.fx, intr2.fy, intr2.ppx, intr2.ppy)
    cam2 = rgbdTools.Camera(intr2.fx, intr2.fy, intr2.ppx, intr2.ppy)

    # 计算两个相机的点云的旋转矩阵
    print('Calculating Transformation Matrix:')
    cam1_point = []
    cam2_point = []
    for view in range(chessBoard_num):
        # 读取先去拍摄的棋盘校正RGB-D图
        cam1_rgb = cv2.imread('./output/cam1_color_'+str(view)+'.png')
        cam1_depth = cv2.imread('./output/cam1_depth_'+str(view)+'.png', -1)
        # 将深度领出来，后面需要利用RGB坐标与深度信息来推导空间坐标
        cam1_depth_array = np.asanyarray(cam1_depth)
        cam2_rgb = cv2.imread('./output/cam2_color_'+str(view)+'.png')
        cam2_depth = cv2.imread('./output/cam2_depth_'+str(view)+'.png', -1)
        cam2_depth_array = np.asanyarray(cam2_depth)
        # 输入校正棋盘的rgb图像，并输入棋盘的内角数，输出内角点的位置（RGB）
        chessboard_found1, corners1 = cv2.findChessboardCorners(cam1_rgb, (9, 6))
        corners1 = np.asanyarray(corners1).squeeze()
        chessboard_found2, corners2 = cv2.findChessboardCorners(cam2_rgb, (9, 6))
        corners2 = np.asanyarray(corners2).squeeze()

        # 将每一个内角点（9*6=54）的位置找对应的三维空间坐标点
        for p_2d in range(54):
            # 取出角点坐标，并四舍五入，化为int数据
            m1 = int(round(corners1[p_2d][1]))
            n1 = int(round(corners1[p_2d][0]))
            # 找到对应的深度信息（如果存在），并获取空间坐标
            if cam1_depth_array[m1, n1] > 0:
                x1, y1, z1 = rgbdTools.getPosition(cam1, cam1_depth_array, m1, n1)
            else:
                continue
            # 获取另一个相机的对应点空间坐标
            m2 = int(round(corners2[p_2d][1]))
            n2 = int(round(corners2[p_2d][0]))
            if cam2_depth_array[m2, n2] > 0:
                x2, y2, z2 = rgbdTools.getPosition(cam2, cam2_depth_array, m2, n2)
            else:
                continue
            # 将空间坐标点添加到列表中
            cam1_point.append([x1, y1, z1])
            cam2_point.append([x2, y2, z2])

    # 基于两个相机的棋盘内角点的对应关系可以计算出两个点云的空间融合转换矩阵
    Transformation = registration.rigid_transform_3D(np.asarray(cam1_point), np.asarray(cam2_point))
    print(Transformation)

    geometrie_added = False
    # 调用主要的可视化工具类（VisualizerWithEditing、VisualizerWithKeyCallback、VisualizerWithVertexSelection具有特殊功能）
    vis = o3d.visualization.Visualizer()
    vis.create_window("Pointcloud")
    # 创建点云类
    pointcloud = o3d.geometry.PointCloud()

    try:
        time_beigin = time.time()
        while True:
            time_start = time.time()
            pointcloud.clear()

            # 等待数据进来
            frames1 = pipeline1.wait_for_frames()
            frames2 = pipeline2.wait_for_frames()

            # 将进来的RGBD数据对齐
            aligned_frames1 = align.process(frames1)
            aligned_frames2 = align.process(frames2)

            # 将对其的RGB—D图取出来
            color_frame1 = aligned_frames1.get_color_frame()
            depth_frame1 = aligned_frames1.get_depth_frame()

            # 数组化数据便于处理
            color_image1 = np.asanyarray(color_frame1.get_data())
            depth_image1 = np.asanyarray(depth_frame1.get_data())

            color_frame2 = aligned_frames2.get_color_frame()
            depth_frame2 = aligned_frames2.get_depth_frame()

            color_image2 = np.asanyarray(color_frame2.get_data())
            depth_image2 = np.asanyarray(depth_frame2.get_data())

            # 对与距离大于1m的数据清零
            depth_image11 = np.where((depth_image1 > 1000) | (depth_image1 < 0), 0, depth_image1)
            depth_image22 = np.where((depth_image2 > 1000) | (depth_image2 < 0), 0, depth_image2)

            # 转格式
            depth1 = o3d.geometry.Image(depth_image11)
            color1 = o3d.geometry.Image(cv2.cvtColor(color_image1, cv2.COLOR_BGR2RGB))

            depth2 = o3d.geometry.Image(depth_image22)
            color2 = o3d.geometry.Image(cv2.cvtColor(color_image2, cv2.COLOR_BGR2RGB))

            # 利用RGB-D数据点云建模
            rgbd1 = o3d.geometry.RGBDImage.create_from_color_and_depth(color1, depth1, convert_rgb_to_intensity=False)
            pcd1 = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd1, pinhole_camera1_intrinsic)

            rgbd2 = o3d.geometry.RGBDImage.create_from_color_and_depth(color2, depth2, convert_rgb_to_intensity=False)
            pcd2 = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd2, pinhole_camera2_intrinsic)

            time_now = time.time()
            # 刚开始的前两秒只加载相机2的点云
            if time_now - time_beigin < 2:
                pointcloud += pcd2
            else:
                # 将两个点云利用前面校正的点云旋转矩阵融合在一起
                pointcloud += (pcd1.transform(Transformation) + pcd2)
            # 将点云做空间转换
            pointcloud.transform([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])

            # 在第一次加载的时候利用add_geometry刷入open3d空间
            if not geometrie_added:
                vis.add_geometry(pointcloud)
                geometrie_added = True

            # 每次点云更新就刷新一下
            vis.update_geometry(pointcloud)
            vis.poll_events()
            vis.update_renderer()

            time_end = time.time()

            print("FPS = {0}".format(int(1/(time_end-time_start))))

    finally:
        pipeline1.stop()
        pipeline2.stop()