（记录）ORB-SLAM3跑自己的双目相机（ZED相机）数据

ORB-SLAM3安装

按照下面链接安装，注意版本为ORB-SLAM3-1.0
链接: link

ZED相机驱动安装

由于本人采集设备是windows系统，因此这边选择使用windows驱动
本部分参考
链接: link
进入官网下载https://www.stereolabs.com/developers/release/3.7
选择3.7版本的原因是新版本对应的python api代码有较大变动，网上参考过少。
默认安装到C:\Program Files (x86)\ZED SDK 路径下
可以使用C:\Program Files (x86)\ZED SDK\tools下的ZED Explorer.exe对相机进行测试

ZED Python API安装

在Anaconda新建一个python环境（本人版本为python3.10，未测试过其他版本）
新建一个python 的 Project
将C:\Program Files (x86)\ZED SDK整个文件夹复制到Project中，因为在C盘后续会有权限限制的问题
打开终端运行下面的命令进行安装
python ./get_python_api.py

具体过程见
链接: link

ZED相机内参获取

import pyzed.sl as sl
import cv2
def hello_zed():
    # 创建相机对象
    zed = sl.Camera()  # Camera是非常重要的一个类

    # 创建初始化参数对象并配置初始化参数
    init_params = sl.InitParameters()
    init_params.sdk_verbose = False  # 相机有很多可以初始化的参数，用到一个认识一个

    # 打开相机（终端打开，但是看不到相机的画面，需要用到cv2.imshow显示相机画面，后面再介绍）
    err = zed.open(init_params)  # 指定参数打开相机
    if err != sl.ERROR_CODE.SUCCESS:
        exit(1)
    # 获得相机的信息，笔者列举了一部分，并不是全部信息，读者可以自行探究
    zed_info = zed.get_camera_information()
    print('相机序列号：%s' % zed_info.serial_number)
    print('相机型号：%s' % zed_info.camera_model)
    print('相机分辨率: width:%s, height:%s' % (zed_info.camera_resolution.width, zed_info.camera_resolution.height))
    print('相机FPS：%s' % zed_info.camera_fps)
    print('相机外部参数：')
    print('相机旋转矩阵R：%s' % zed_info.calibration_parameters.R)
    print('相机变换矩阵T：%s' % zed_info.calibration_parameters.T)
    print('相机基距：%s' % zed_info.calibration_parameters.get_camera_baseline())
    print('初始化参数：')
    zed_init = zed.get_init_parameters()
    print('相机分辨率：%s' % (zed_init.camera_resolution))
    print('深度最小：%s' % (zed_init.depth_minimum_distance))
    print('深度最大：%s' % (zed_init.depth_maximum_distance))
    # 关闭相机
    zed.close()

if __name__ == "__main__":
    hello_zed()

ZED相机采集代码

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# @Time        :2021/6/10 上午10:00
# @Author      :weiz
# @ProjectName :zed-samples
# @File        :CameraZed2.py
# @Description :封装一下zed摄像头
import cv2
import os
import time
import pyzed.sl as zed
import shutil
import math


class CameraZed2:
    def __init__(self, resolutionRatio=None, depthMode=None):
        self.cam = zed.Camera()
        self.input_type = zed.InputType()
        self.camInit = zed.InitParameters(input_t=self.input_type)

        # 设置分辨率
        if resolutionRatio == "HD2K":
            self.camInit.camera_resolution = zed.RESOLUTION.HD2K
        elif resolutionRatio == "HD1080":
            self.camInit.camera_resolution = zed.RESOLUTION.HD1080
        else:
            self.camInit.camera_resolution = zed.RESOLUTION.HD720
        # 设置获取深度信息的模式
        if depthMode == "PERFORMANCE":
            self.camInit.depth_mode = zed.DEPTH_MODE.PERFORMANCE
        elif depthMode == "QUALITY":
            self.camInit.depth_mode = zed.DEPTH_MODE.QUALITY
        else:
            self.camInit.depth_mode = zed.DEPTH_MODE.ULTRA
        self.camInit.coordinate_units = zed.UNIT.MILLIMETER  # 单位毫米
        err = self.cam.open(self.camInit)
        if err != zed.ERROR_CODE.SUCCESS:
            print(repr(err))
            self.cam.close()
            exit(1)

        self.image_size = self.cam.get_camera_information().camera_resolution
        self.image_zed = zed.Mat(self.image_size.width, self.image_size.height, zed.MAT_TYPE.U8_C4)

        # Set runtime parameters after opening the camera
        self.runtime = zed.RuntimeParameters()
        self.runtime.sensing_mode = zed.SENSING_MODE.STANDARD

    def dataStreamRunning(self):
        """
        用于获取摄像头的数据流
        :return:
        """
        image_zed_left = zed.Mat()  # left_img
        image_zed_right = zed.Mat()  # right_img
        depth_image = zed.Mat(self.image_size.width, self.image_size.height, zed.MAT_TYPE.U8_C4)
        self.point_cloud = zed.Mat()
        self.depth_map = zed.Mat()

        while True:
            self.cam.grab(self.runtime)

            # 左图
            self.cam.retrieve_image(image_zed_left, zed.VIEW.LEFT)
            image_cv_left = image_zed_left.get_data()

            # 右图
            self.cam.retrieve_image(image_zed_right, zed.VIEW.RIGHT)
            image_cv_right = image_zed_right.get_data()

            # 深度信息和点云
            self.cam.retrieve_image(depth_image, zed.VIEW.DEPTH, zed.MEM.CPU, self.image_size)
            self.cam.retrieve_measure(self.point_cloud, zed.MEASURE.XYZRGBA, zed.MEM.CPU, self.image_size)
            self.cam.retrieve_measure(self.depth_map, zed.MEASURE.DEPTH)

            self.image_cv_left = cv2.cvtColor(image_cv_left, 1)
            self.image_cv_right = cv2.cvtColor(image_cv_right, 1)
            self.image_depth = depth_image.get_data()

            yield

    def showImage(self):
        while True:
            next(self.dataStreamRunning())

            cv2.imshow("left", self.image_cv_left)
            cv2.imshow("right", self.image_cv_right)
            # cv2.imshow("depth", self.image_depth)
            if cv2.waitKey(1) & 0xFF == 27:
                break
    def showImage(self):
        while True:
            next(self.dataStreamRunning())

            cv2.imshow("left", self.image_cv_left)
            cv2.imshow("right", self.image_cv_right)
            # cv2.imshow("depth", self.image_depth)
            if cv2.waitKey(1) & 0xFF == 27:
                break

    def saveVideo(self):
        # 定义视频编码器和创建VideoWriter对象
        # fourcc = cv2.VideoWriter_fourcc(*'XVID')  # 常用的视频编码器之一
        fourcc = cv2.VideoWriter_fourcc(*'mp4v')
        out_left = cv2.VideoWriter('1/left_camera.mp4', fourcc, 30.0, (self.image_size.width, self.image_size.height))  # 定义左摄像头视频文件和帧率
        out_right = cv2.VideoWriter('1/right_camera.mp4', fourcc, 30.0, (self.image_size.width, self.image_size.height))  # 定义右摄像头视频文件和帧率
        while True:
            next(self.dataStreamRunning())

            cv2.imshow("left", self.image_cv_left)
            cv2.imshow("right", self.image_cv_right)

            # 将左摄像头图像写入视频文件
            out_left.write(self.image_cv_left)
            # 将右摄像头图像写入视频文件
            out_right.write(self.image_cv_right)
            # cv2.imshow("depth", self.image_depth)
            if cv2.waitKey(1) & 0xFF == 27:
                break
        # 释放视频写入对象
        out_left.release()
        out_right.release()
        # 关闭所有OpenCV窗口
        cv2.destroyAllWindows()

    def getMatImage(self, lr=None):
        """
        获取opencv格式的图片
        :param lr:获取左图或右图的标识位
        :return:
        """
        next(self.dataStreamRunning())

        if lr == "right":
            return self.image_cv_right
        else:
            return self.image_cv_left

    def getDepthValue(self, x, y):
        """
        获得某个像素点的深度信息
        :param x:
        :param y:
        :return:
        """
        next(self.dataStreamRunning())
        _, value = self.depth_map.get_value(x, y)
        return value

    def getPointCloud(self, x, y):
        """
        获得某个像素点的点云信息
        :param x:
        :param y:
        :return: [x, y, z, color],离左摄像头的距离
        """
        next(self.dataStreamRunning())
        _, point3D = self.point_cloud.get_value(x, y)
        distance = math.sqrt(point3D[0] * point3D[0] + point3D[1] * point3D[1] + point3D[2] * point3D[2])
        return point3D, distance

    def saveImageOfKey(self, savePath=None):
        """
        根据按键信息保存图片
        :param savePath:
        :return:
        """
        if savePath == None:
            savePath = "./zed2ImagesOfKey"
            if os.path.exists(savePath):
                shutil.rmtree(savePath)
            os.mkdir(savePath)
        else:
            if os.path.exists(savePath):
                shutil.rmtree(savePath)
            os.mkdir(savePath)

        num_l = 1
        num_r = 1
        while True:
            next(self.dataStreamRunning())

            cv2.imshow("left", self.image_cv_left)
            cv2.imshow("right", self.image_cv_right)
            key = cv2.waitKey(1)
            if key & 0xFF == ord('l'):
                tmSavePath = os.path.join(savePath, "L{:0>3d}.png".format(num_l))
                cv2.imwrite(tmSavePath, self.image_cv_left)
                num_l = num_l + 1
            if key & 0xFF == ord('r'):
                tmSavePath = os.path.join(savePath, "R{:0>3d}.png".format(num_r))
                cv2.imwrite(tmSavePath, self.image_cv_right)
                num_r = num_r + 1
            if key & 0xFF == 27:
                break

    def saveImage(self, img, savePath=None, imageName=None):
        """
        保存图片，注意每秒只能保存一张图片
        :param savePath:
        :param imageName:
        :return:
        """
        if savePath == None:
            savePath = "./zed2Images"
        if imageName == None:
            imageName = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime())

        if not os.path.exists(savePath):
            os.mkdir(savePath)
        savePath = os.path.join(savePath, imageName + ".png")

        cv2.imwrite(savePath, img)


if __name__ == "__main__":
    cam = CameraZed2(depthMode="PERFORMANCE")
    # cam.showImage()
    cam.saveVideo()
    # while True:
    #     img = cam.getMatImage()
    #     cv2.imshow("img", img)
    #     if cv2.waitKey(1) & 0xFF == 27:
    #         break

    # cam.saveImageOfKey("./abc")

    # img = cam.getMatImage()
    # cam.saveImage(img)

    # print(cam.getDepthValue(100, 100))
    #
    # print(cam.getPointCloud(100, 100))

数据对ORB-SLAM3的适配

参照双目EuRoC数据集的格式进行处理

视频处理代码

将视频处理成图片序列

import cv2
from PIL import Image
import numpy as np
import os


def video_to_rotated_images_with_display(video_path, output_dir):
    # 确保输出目录存在
    if not os.path.exists(output_dir):
        os.makedirs(output_dir)

    # 初始化时间戳计数器
    timestamp_counter = 0

    # 读取视频
    cap = cv2.VideoCapture(video_path)
    if not cap.isOpened():
        print("Error opening video file")
        return

    while True:
        # 逐帧读取
        ret, frame = cap.read()
        if not ret:
            break
        # 实时显示处理后的图像
        cv2.imshow('Rotated Frame', frame)

        # 生成文件名并保存图像
        filename = f"{timestamp_counter:020d}.png"
        timestamp_counter += 1
        cv2.imwrite(os.path.join(output_dir, filename), frame)


        # 按'q'退出
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

    cap.release()
    cv2.destroyAllWindows()
    print("All frames have been processed and saved.")


# 使用示例
video_path = 'left_camera.mp4'  # 将此路径替换为你的视频文件路径
output_dir = 'left'  # 保存旋转后图像的目录
video_to_rotated_images_with_display(video_path, output_dir)

video_path = 'right_camera.mp4'  # 将此路径替换为你的视频文件路径
output_dir = 'right'  # 保存旋转后图像的目录
video_to_rotated_images_with_display(video_path, output_dir)

生成20位时间戳txt

start_timestamp = "00000000000000000000"
end_line = 3466 # 改为图片序列的数量

# Open a file in write mode
with open("zed.txt", "w") as file:
    # For each line, increment the timestamp as an integer and write it formatted as a 20-digit string
    for i in range(end_line):
        timestamp = str(i).zfill(20)  # Pad the number to ensure it is 20 digits
        file.write(timestamp + "\n")

制作相机参数文件yaml

#相机1内参矩阵
Camera1.fx: 686.9675
Camera1.fy: 686.9675
Camera1.cx: 625.6688
Camera1.cy: 332.6237
#相机1畸变
Camera1.k1: 0.0
Camera1.k2: 0.0
Camera1.p1: 0.0
Camera1.p2: 0.0
#相机2内参矩阵
Camera2.fx: 686.9675
Camera2.fy: 686.9675
Camera2.cx: 625.6688
Camera2.cy: 332.6237
#相机2畸变
Camera2.k1: 0.0
Camera2.k2: 0.0
Camera2.p1: 0.0
Camera2.p2: 0.0
#相机1和相机2的变换矩阵（注意最右列平移向量单位为米）
Stereo.T_c1_c2: !!opencv-matrix
  rows: 4
  cols: 4
  dt: f
  data: [1.000000000000000,0.000000000000000,0.000000000000000,0.11984635162,
         0.000000000000000,1.000000000000000,0.000000000000000,0.000000000000000,
         0.000000000000000,0.000000000000000,1.000000000000000,0.000000000000000,
         0.000000000000000,0.000000000000000,0.000000000000000,1.000000000000000]

运行

./Examples/Stereo/stereo_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo/ZED.yaml  /home/path_to_dataset ./Examples/Stereo/ZED_TimeStamps/zed.txt