Realsense学习--棋盘校正

棋盘矫正

由于棋盘校正时的内角点是较为优秀的特征点，能够为我们对相机的校正和模型的匹配提供很好的基准。尤其在深度相机中，通过在RGB中提取内角点，再结合深度图即可得到对应点的空间坐标。

关键函数

def findChessboardCorners(image, patternSize, corners=None, flags=None): 
    """
    .   @param image Source chessboard view. It must be an 8-bit grayscale or color image.
    .   @param patternSize Number of inner corners per a chessboard row and column
    .   ( patternSize = cv::Size(points_per_row,points_per_colum) = cv::Size(columns,rows) ).
    .   @param corners Output array of detected corners.
    .   @param flags Various operation flags that can be zero or a combination of the following values:
    """
    pass

该函数尝试确定输入图像是否是棋盘图案的视图，并找到棋盘内部的角。 如果找到了所有角并将它们按一定顺序放置（逐行，在每一行中从左到右），该函数将返回一个非零值。 否则，如果函数无法找到所有角或对其重新排序，则它将返回0。例如，常规棋盘具有8 x 8正方形和7 x 7个内部角，即黑色正方形相互接触的点。

校正代码

本代码将实时显示并寻找图像中的棋盘，小键盘s将会保存当前图片，方便与以后的校正。

import pyrealsense2 as rs
import cv2
import numpy as np
import time
import os
import sys
from tools import rgbdTools


if __name__ == '__main__':
    # 定义分辨率与帧率
    resolution_width = 1280  # pixels
    resolution_height = 720  # pixels
    frame_rate = 15  # fps

    # 配置相机，注册数据流，对其RGBD图像
    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()
    cam1 = rgbdTools.Camera(intr1.fx, intr1.fy, intr1.ppx, intr1.ppy)
    print('cam1 intrinsics:')
    print(intr1.width, intr1.height, 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()
    cam2 = rgbdTools.Camera(intr2.fx, intr2.fy, intr2.ppx, intr2.ppy)
    print('cam2 intrinsics:')
    print(intr2.width, intr2.height, intr2.fx, intr2.fy, intr2.ppx, intr2.ppy)

    i = 0
    try:
        while True:
            time_start = time.time()
            frames1 = pipeline1.wait_for_frames()
            frames2 = pipeline2.wait_for_frames()

            aligned_frames1 = align.process(frames1)
            aligned_frames2 = align.process(frames2)

            color_frame1 = aligned_frames1.get_color_frame()
            depth_frame1 = aligned_frames1.get_depth_frame()

            depth_frame1 = rs.decimation_filter(1).process(depth_frame1)
            depth_frame1 = rs.disparity_transform(True).process(depth_frame1)
            depth_frame1 = rs.spatial_filter().process(depth_frame1)
            depth_frame1 = rs.temporal_filter().process(depth_frame1)
            depth_frame1 = rs.disparity_transform(False).process(depth_frame1)

            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()
            depth_frame2 = rs.decimation_filter(1).process(depth_frame2)
            depth_frame2 = rs.disparity_transform(True).process(depth_frame2)
            depth_frame2 = rs.spatial_filter().process(depth_frame2)
            depth_frame2 = rs.temporal_filter().process(depth_frame2)
            depth_frame2 = rs.disparity_transform(False).process(depth_frame2)

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

            color1 = color_image1.copy()
            color2 = color_image2.copy()

            # 找到图片中的校正棋盘，其中内角点为9*6
            chessboard_found1, corners1 = cv2.findChessboardCorners(color1, (9, 6))
            cv2.drawChessboardCorners(color1, (9, 6), corners1, chessboard_found1)

            chessboard_found2, corners2 = cv2.findChessboardCorners(color2, (9, 6))
            cv2.drawChessboardCorners(color2, (9, 6), corners2, chessboard_found2)

            cv2.namedWindow('cam1_cam2', cv2.WINDOW_AUTOSIZE)
            cv2.imshow('cam1_cam2', np.hstack((color1, color2)))

            key = cv2.waitKey(1)
            time_end = time.time()
            print('FPS:', 1/(time_end-time_start))

            if key & 0xFF == ord('s'):
                if not os.path.exists('./output/'): 
                    os.makedirs('./output')
                cv2.imwrite('./output/cam1_depth_'+str(i)+'.png', depth_image1)
                cv2.imwrite('./output/cam1_color_'+str(i)+'.png', color_image1)
                cv2.imwrite('./output/cam2_depth_'+str(i)+'.png', depth_image2)
                cv2.imwrite('./output/cam2_color_'+str(i)+'.png', color_image2)
                print('No.'+str(i) + ' shot is saved.')
                i += 1

            elif key & 0xFF == ord('q') or key == 27:
                cv2.destroyAllWindows()
                break

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