这段代码首先检查配置文件中是否设置了保存 CSV 文件的选项(SAVE_CSV
为True
),如果是,则执行以下操作:
-
定义了一个列表
header
,包含了 CSV 文件的列名。在这个例子中,列名包括了car_center_x
、car_center_y
、x
、y
和z
,这些可能是表示车辆中心坐标和位置的数据。 -
使用
open()
函数创建了一个名为data.csv
的 CSV 文件,并以写入模式打开它。参数newline=''
表示不使用额外的换行符,而是由csv
模块来管理换行。 -
使用
csv.DictWriter()
函数创建了一个writer
对象,该对象用于将字典数据写入 CSV 文件。参数chart
是之前创建的 CSV 文件对象,header
是 CSV 文件的列名。 -
使用
writer.writeheader()
方法写入了 CSV 文件的头部,即列名。这样可以确保在写入数据之前,文件中已经有了正确的列名。
if main_cfg['ctrl']['SAVE_CSV']:header = ['car_center_x', 'car_center_y', 'x', 'y', 'z']chart = open("data.csv", "w", newline='')writer = csv.DictWriter(chart, header)writer.writeheader()
这段代码定义了一个名为 push_button_clicked_quit
的函数,用于处理当某个按钮被点击时触发的事件。函数的作用是设置全局变量 exit_signal
和 Loop
,使得程序退出主循环并退出程序。
具体来说:
global exit_signal, Loop
:这行代码声明了函数中将要使用的两个全局变量exit_signal
和Loop
。exit_signal = True
:将exit_signal
设置为True
,表示程序应该退出。print('exit')
:打印消息提示程序即将退出。Loop = False
:将Loop
设置为False
,以停止主循环的执行,从而退出程序。
def push_button_clicked_quit():global exit_signal, Loopexit_signal = Trueprint('exit')Loop = False
这段代码定义了一个名为 main()
的函数,用于程序的主逻辑。函数内部涉及到一系列变量的定义和初始化,以及根据配置文件中的设置选择相应的操作模式。
具体来说:
global targets
:声明了函数内部将要使用的全局变量targets
。camera_left = camera_right = None
:初始化左右摄像头对象为None
。image_left = image_right = None
:初始化左右图像对象为None
。ret_p = ret_q = None
:初始化一些其他变量为None
。coex_matcher = None
:初始化匹配模型对象为None
。model_car = None
:初始化车辆模型对象为None
。monitor = None
:初始化监视器对象为None
。main_scene = None
:初始化主场景对象为None
。timeout_draw_zone = False
:初始化一个标志变量为False
,用于标记是否超时。
接下来是根据配置文件中的模式选择相应的操作:
- 如果模式为
'video'
,则调用bc.get_video_loader()
函数加载视频数据。 - 如果模式为
'camera'
,则调用bc.get_camera()
函数加载摄像头数据。
最后,初始化一个 CoExMatcher
对象,用于匹配图像特征。
def main():global targetscamera_left = camera_right = Noneimage_left = image_right = Noneret_p = ret_q = Nonecoex_matcher = Nonemodel_car = Nonemonitor = Nonemain_scene = Nonetimeout_draw_zone = Falseif main_cfg['ctrl']['MODE'] == 'video':camera_left, camera_right, fps, size = bc.get_video_loader(main_cfg['video']['bin_video_left'],main_cfg['video']['bin_video_right'])elif main_cfg['ctrl']['MODE'] == 'camera':print("\nLoading binocular camera")camera_left, camera_right, ret_p, ret_q = bc.get_camera(bin_cam_cfg)print("Done")print("\nLoading matching model")coex_matcher = CoExMatcher(bin_cam_cfg)print("Done\n")
-
创建了一个名为
left_cam_cfg
的字典,用于存储左侧摄像头的内参和畸变系数。这些参数从bin_cam_cfg
中获取。 -
创建了一个
CameraPoseSolver
类的实例camera_pose_solver
,用于解算相机姿态。这个实例根据己方颜色(ALLY_COLOR
)和左侧摄像头的配置参数进行初始化。 -
如果配置文件中设置了使用锚定点(
ANCHOR
为True
),则执行以下操作:- 创建了一个名为
anchor
的锚定点对象。 - 使用
camera_left
和camera_right
读取图像数据(如果模式为'video'
)或者获取摄像头帧数据(如果模式为'camera'
)。 - 如果读取到的图像为空,则继续循环读取直到成功读取到图像。
- 对左侧图像进行手动设置锚定点的操作,然后使用锚定点初始化相机姿态解算器。
- 循环结束后退出循环。
- 创建了一个名为
-
如果未设置使用锚定点,则暂时注释掉了使用常量初始化相机姿态解算器的部分。
综上所述,这段代码的主要作用是根据配置文件的设置,初始化左侧摄像头的配置参数,并根据情况使用锚定点初始化相机姿态解算器。
left_cam_cfg = dict()left_cam_cfg['intrinsic'] = bin_cam_cfg['calib']['intrinsic1']left_cam_cfg['distortion'] = bin_cam_cfg['calib']['distortion1']camera_pose_solver = cc.CameraPoseSolver(ALLY_COLOR, left_cam_cfg)if main_cfg['ctrl']['ANCHOR']:anchor = Anchor()while True: # this while is for case where no img gotif main_cfg['ctrl']['MODE'] == 'video':ret, image_left = camera_left.read()ret, image_right = camera_right.read()elif main_cfg['ctrl']['MODE'] == 'camera':image_left = bc.get_frame(camera_left, 'left_camera', ret_p)image_right = bc.get_frame(camera_right, 'right_camera', ret_q)if image_left is None or image_right is None:continueset_by_hand(image_left, anchor)camera_pose_solver.init_by_anchor(anchor)breakelse:# camera_pose_solver.init_by_constant()pass
-
如果配置文件中设置了检测(
DETECT
为True
),则执行以下操作:- 打印消息提示正在加载车辆模型。
- 使用 YOLO 模型加载车辆模型,模型权重文件路径从配置文件中获取。
- 打印消息提示加载完成。
-
如果配置文件中设置了使用图形用户界面(GUI),则执行以下操作:
- 打印消息提示正在准备 GUI。
- 设置 GUI 的缩放属性以支持高 DPI 缩放。
- 创建一个 QApplication 对象。
- 创建一个 QMainWindow 对象,并设置其界面为通过
gui.Ui_Monitor()
创建的界面。 - 将退出按钮的点击事件连接到
push_button_clicked_quit
函数。 - 创建一个 QGraphicsScene 对象作为主场景。
- 将主场景设置到 GUI 界面中的视图中,并显示出来。
- 打印消息提示 GUI 准备完成。
- 如果存在超时的绘制区域,则向调试消息框添加一条消息。
-
最后,初始化了一些计数器和时间变量。
if main_cfg['ctrl']['DETECT']:print('Loading Car Model')model_car = YOLO(main_cfg['weights']['yolov8'])print('Done\n')if main_cfg['ctrl']['GUI']:print('preparing gui')QApplication.setAttribute(Qt.AA_EnableHighDpiScaling)app = QApplication(sys.argv)MainWindow = QMainWindow()monitor = gui.Ui_Monitor()monitor.setupUi(MainWindow)monitor.pushButton_quit.clicked.connect(push_button_clicked_quit)main_scene = QGraphicsScene()monitor.main_frame_view.setScene(main_scene)monitor.main_frame_view.show()MainWindow.show()print('done')if timeout_draw_zone:monitor.debug_msg.append('zone initialize: timeout, using saved one\n')cnt = 0start = 0.last = time.time()
-
使用全局变量
Loop
来控制程序的运行,只有当Loop
为True
时,程序才会继续执行主循环。这个变量通常在其他地方被设置为False
,从而触发程序退出。 -
如果按下键盘上的 'q' 键,那么
Loop
被设置为False
,程序将退出主循环。 -
根据配置文件中的模式选择相应的操作:
- 如果模式为
'video'
,则从左右摄像头读取图像数据。 - 如果模式为
'camera'
,则从左右摄像头获取图像帧数据。
- 如果模式为
-
对于获取的左右图像数据,进行了一系列处理:
- 如果开启了录制视频的选项,则将左右原始图像写入相应的视频文件中。
- 调用
coex_matcher.inference()
对左右图像进行立体匹配,获取深度信息和视差图。 - 对视差图进行颜色映射,并写入深度视频文件中。
- 如果开启了棋盘格检测的选项,则检测图像中的棋盘格角点,并在图像中标注对应的三维坐标信息。
-
最后,根据配置文件中的设置,显示处理后的图像或者关闭窗口。
global Loopwhile Loop:if cv2.waitKey(1) == ord('q'):Loop = Falseif main_cfg['ctrl']['MODE'] == 'video':ret, image_left = camera_left.read()ret, image_right = camera_right.read()elif main_cfg['ctrl']['MODE'] == 'camera':image_left = bc.get_frame(camera_left, 'left_camera', ret_p)image_right = bc.get_frame(camera_right, 'right_camera', ret_q)if image_right is not None and image_left is not None:if main_cfg['ctrl']['RECORDING']:left_video.write(image_left)right_video.write(image_right)re_left, point_cloud, disp_np = coex_matcher.inference(image_left, image_right)if main_cfg['ctrl']['RECORDING']:disp_video.write(disp_np)cv2.imshow('raw_disp', disp_np)disp_np = cv2.applyColorMap(2 * disp_np, cv2.COLORMAP_MAGMA)if main_cfg['ctrl']['RECORDING']:depth_video.write(disp_np)if main_cfg['ctrl']['CHESSBOARD']:pattern_size = (8, 6)corners, image_with_corners = find_chessboard_corners(re_left, pattern_size)# print(corners)if corners is not None:cv2.putText(image_with_corners,'[ ' + str(round(point_cloud[int(corners[0][0][1])][int(corners[0][0][0])][0], 2)) + ', '+ str(round(point_cloud[int(corners[0][0][1])][int(corners[0][0][0])][1], 2)) + ', '+ str(round(point_cloud[int(corners[0][0][1])][int(corners[0][0][0])][2], 2)) + ']',(int(corners[0][0][0]), int(corners[0][0][1])),fontFace=cv2.FONT_HERSHEY_SIMPLEX,fontScale=0.7,color=(255, 255, 255),thickness=1,lineType=cv2.LINE_AA)cv2.imshow('xyz', image_with_corners)
这段代码实现了以下功能:
- 跳过开始的前 20 帧以便更准确地记录时间。在第 20 帧之后开始记录时间,计算帧率。
- 使用 OpenCV 在深度图像上绘制帧率信息。
- 如果配置文件中启用了目标检测(DETECT为True),则执行以下操作:
- 对左侧摄像头的图像进行目标检测,获取目标检测结果。
- 更新目标列表中的目标信息,并根据相机坐标解算出场地坐标。
- 如果启用了调试模式(debug为True),则在左侧图像上标注目标的相机坐标。
- 将目标信息发送给串口。
- 如果启用了 GUI 模式,则在监视器上显示发送的消息。
- 如果未启用目标检测,则显示左侧摄像头的图像。
# skip starting frames for more accurate time recordif cnt == 20:start = time.time()if cnt > 20:now = time.time()fps = (cnt - 10) / (now - start)cv2.putText(disp_np,"fps: " + "%.2f" % fps,(4, 40),fontFace=cv2.FONT_HERSHEY_SIMPLEX,fontScale=0.9,color=(255, 255, 255),thickness=2,lineType=cv2.LINE_AA)cv2.imshow('disp', disp_np)if main_cfg['ctrl']['DETECT']:dst_img = np.copy(re_left)result = model_car.predict(dst_img, show=True)boxes = result[0].boxes.data.cpu()boxes = boxes.numpy()print(boxes)targets.update(boxes)for target in targets.targets:if target.conf > 0:cam_coord = [[point_cloud[int(target.center_yx[0])][int(target.center_yx[1])][0]],[point_cloud[int(target.center_yx[0])][int(target.center_yx[1])][1]],[point_cloud[int(target.center_yx[0])][int(target.center_yx[1])][2]]]field_coord = camera_pose_solver.get_field_coord(cam_coord)target.x = field_coord[0]target.y = field_coord[1]if main_cfg['debug']:msg = str(cam_coord)cv2.putText(re_left,msg,(int(target.center_yx[1]), int(target.center_yx[0])),cv2.FONT_HERSHEY_PLAIN,1.0,(0, 0, 255),thickness=1)if main_cfg['debug']:cv2.imshow('dist', re_left)for car in targets.targets:if car.conf > 0:now = time.time()# 距离上一次发送时间小于0.1s:sleepif now - last < 0.1:time.sleep(0.1 - (now - last))messager.send_enemy_location(ser, car.get_id(), car.x / 1000,car.y / 1000) # mm to mif main_cfg['ctrl']['GUI']:monitor.debug_msg.append('at [' + str(now) + '] send: ' + str(car.get_id()) + ' ' +str(car.x / 1000) + ' ' + str(car.y / 1000) + '\n')last = time.time()else:cv2.imshow('re_left', re_left)
这段代码检查了配置文件中是否启用了 GUI 模式。如果启用了 GUI 模式,则执行以下操作:
- 使用
cv2.resize()
函数将左侧摄像头的图像re_left
缩放到指定的大小(720x360像素)。 - 使用
QImage()
函数将 OpenCV 图像转换为 Qt 的图像格式QImage
。 - 在每次循环开始之前,通过
main_scene.clear()
清空上一次循环中残留的图像。 - 使用
QPixmap.fromImage()
将QImage
转换为QPixmap
。 - 使用
main_scene.addPixmap()
将QPixmap
添加到主场景中。
if main_cfg['ctrl']['GUI']:resized = cv2.resize(re_left, (720, 360))frame = QImage(resized, 720, 360, 720 * 3, QImage.Format_BGR888)main_scene.clear() # 先清空上次的残留pixel_map = QPixmap.fromImage(frame)main_scene.addPixmap(pixel_map)
-
将计数器
cnt
自增一,用于记录循环的次数。 -
输出
- end of loop -----------------------------------------------------------------------------
,作为循环结束的标志。 -
如果配置文件中的模式为
'camera'
,则关闭摄像头。 -
关闭所有 OpenCV 窗口,释放图像资源。
-
如果配置文件中设置了录制视频的选项,则将配置文件复制到视频文件夹中,并释放视频写入对象。
-
如果配置文件中设置了保存 CSV 文件的选项,则关闭 CSV 文件。
-
输出 'release!',表示所有资源已经释放。
-
最后,检查是否处于主程序的入口,如果是,则调用
main()
函数开始执行程序的主逻辑。
这段代码用于完成程序的收尾工作,包括关闭摄像头、释放资源、保存文件等操作,并最终执行程序的主逻辑。
cnt += 1'- end of loop -----------------------------------------------------------------------------'if main_cfg['ctrl']['MODE'] == 'camera':bc.camera_close(camera_left, 'camera_left')bc.camera_close(camera_right, 'camera_right')cv2.destroyAllWindows()if main_cfg['ctrl']['RECORDING']:os.system('copy bin_cam_config.yaml ' + video_folder[2:] + '/cfg/bin_cam_config.yaml')os.system('copy main_config.yaml ' + video_folder[2:] + '/cfg/main_config.yaml')left_video.release()right_video.release()disp_video.release()depth_video.release()if main_cfg['ctrl']['SAVE_CSV']:chart.close()print('release!')if __name__ == '__main__':main()