基于轨迹信息的图像近距离可行驶区域方案验证

一图像可行驶区域方案

1.1 标定场景

标定场地

1.2 标定步骤

设计一定间距标定场，在标定场固定位置设置摄像头标定标识点。
主车开到标定场固定位置
录制主车在该位置各个摄像头数据，通过摄像头捕获图像获取图像上关键点坐标pts-2d
基于标定场设计，计算图像关键点对应车体坐标系中的3d坐标pts-3d
通过cv2.findHomography(obj_points, img_points, cv2.RANSAC, 5.0) 获取相机坐标系到地面的单应性变换矩阵H

1.3 实车使用

实时获取车辆行进过程中的固定纵向距离的轨迹点信息，使用单应性变换矩阵H反向计算轨迹信息在图像中的投影位置，从而获取到图像中检测目标的距离区间。

二初步验证结果

'''
Author: XIEXINYAN “1532642675@qq.com”
Date: 2024-07-01 04:52:07
LastEditors: XIEXINYAN “1532642675@qq.com”
LastEditTime: 2024-07-03 05:40:06
FilePath: /202407/hom_matrix.py
Description: Copyright (c) 2024 by 1532642675@qq.com, All Rights Reserved. 
'''
import cv2  
import numpy as np  
import os
import argparseclass Counter:cnt = 0def __init__(self):Counter.cnt +=1@classmethoddef get_counter(cls):return cls.cnt 
class Calibrate:def __init__(self, pattern_size, real_square_size, offset_x, offset_y):self.pattern_size = pattern_sizeself.real_square_size = real_square_sizeself.offset_x = offset_xself.offset_y = offset_y# 1. 检测棋格板角点  def find_chessboard_corners(self, image, color=(0, 255, 0), vis=False,  save=False, calib=False): self.image = image.copy()image_painted = image.copy()gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)  ret, corners = cv2.findChessboardCorners(gray, pattern_size, None)  if ret:  criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)  #按照pattern_size[0]的个数排序，绘制的第一组数据个数=pattern_size[0]的个数self.corner = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)if self.corner is not None:'''0  3  6          2  5  8           6  3  0            8  5  21  4  7          1  4  7            7  4  1            7  4  1    2  5  8          0  3  6            8  5  2            6  3  0  '''if vis:#判断corners排序顺序index_list = []if (self.corner[0][0][1]<self.corner[1][0][1] and self.corner[0][0][0]<self.corner[np.prod(self.pattern_size)-1][0][0]):print("mode A")elif (self.corner[0][0][1]>self.corner[1][0][1] and self.corner[0][0][0]<self.corner[np.prod(self.pattern_size)-1][0][0]):print("mode B")for j in range(pattern_size[1]):for i in range(pattern_size[0]):index_list.append(pattern_size[0]-1-i+j*pattern_size[0])elif (self.corner[0][0][1]<self.corner[1][0][1] and self.corner[0][0][0]>self.corner[np.prod(self.pattern_size)-1][0][0]):print("mode C")index_list = []for j in range(pattern_size[1]):for i in range(pattern_size[0]):index_list.append(i+(pattern_size[1]-1-j)*pattern_size[0])print(index_list)elif (self.corner[0][0][1]>self.corner[1][0][1] and self.corner[0][0][0]>self.corner[np.prod(self.pattern_size)-1][0][0]):print("mode D")index_list = []for j in range(pattern_size[1]):for i in range(pattern_size[0]):index_list.append(i+(pattern_size[1]-1-j)*pattern_size[0])print(index_list)else:print("horizonal mode")self.corner = self.corner[index_list]cv2.circle(image_painted, (int(self.corner[0][0][0]), int(self.corner[0][0][1])), 25, (0, 255, 255), -1)  cv2.circle(image_painted, (int(self.corner[1][0][0]), int(self.corner[1][0][1])), 25, (0, 0, 255), -1) for corner in self.corner:cv2.circle(image_painted, (int(corner[0][0]), int(corner[0][1])), 5, (0, 0, 255), -1)  cv2.drawChessboardCorners(image_painted, pattern_size, self.corner, True)  cv2.imshow("chess",image_painted)if save:instance=Counter()cv2.imwrite(str(instance.get_counter())+".jpg", image)if calib:self.find_hom_matrix(vis=True, save=True)return Truereturn Falsedef world_chess_board_loc(self):# 初始化obj_points数组，注意使用齐次坐标（即每个点都是[x, y, 1]）  obj_points = np.zeros((np.prod(self.pattern_size), 3), dtype=np.float32)  # x_start是每行开始的x坐标  # y_start是每行开始的y坐标 # # 填充obj_points数组  '''从左上角开始0  3  6  9  1  4  7  102  5  8  11'''# obj_points =np.array([#     [  0.,   0.,   1.],[  0.,  60.,   1.],[  0., 120.,   1.],#     [ 60.,   0.,   1.],[ 60.,  60.,   1.],[ 60., 120.,   1.],#     [120.,   0.,   1.],[120.,  60.,   1.],[120., 120.,   1.],#     [180.,  0.,   1.],[180.,  60.,   1.],[180., 120.,   1.]], dtype=np.float32)index = 0 for i in range(pattern_size[1]):  for j in range(pattern_size[0]):  # 计算x和y坐标  x = i * self.real_square_size y = j * self.real_square_size  # 将点添加到obj_points数组中，注意使用齐次坐标形式  obj_points[index, :] = [x, y, 1.0]  index += 1  return obj_points'''将世界坐标系下点转化为wraped图像上点'''def world_transation(self):obj_points = self.world_chess_board_loc()obj_points_t = obj_points.copy()obj_points_t[:,0] += self.offset_xobj_points_t[:,1] += self.offset_yobj_points_t[:,:2] *= 1000return obj_points_tdef world_to_image(self, img, vis=False, save=False):# obj_points 是世界坐标系下的点，需要是齐次坐标形式 obj_points_t = self.world_transation()# 使用np.dot进行矩阵乘法，并计算归一化的图像坐标  img_points_homogeneous = np.dot(self.H, obj_points_t.T).T  img_points = img_points_homogeneous[:, :2] / img_points_homogeneous[:, 2:].reshape(-1, 1) for pt_2d in img_points:cv2.circle(img, (int(pt_2d[0]), int(pt_2d[1])), 5, (0, 0, 255), -1) if vis:cv2.imshow("eval image", img)if save:counter = Counter()cv2.imwrite("eval_"+str(counter.get_counter())+".jpg",img)returndef find_hom_matrix(self, vis=False, save=False):# 世界坐标值 【横，纵，高】obj_points = self.world_chess_board_loc() # 偏移到某个坐标系obj_points[:,0] += self.offset_x obj_points[:,1] += self.offset_yobj_points[:,:2] *= 1000# print(obj_points)# 3. 计算单应性矩阵  img_points = self.corner.reshape(-1, 1, 2).astype(np.float32)  self.H, _ = cv2.findHomography(obj_points, img_points, cv2.RANSAC, 5.0)  warped_image = cv2.warpPerspective(self.image, self.H,  (self.image.shape[1], self.image.shape[0]))if vis:cv2.imshow("wrapped image",warped_image)cv2.waitKey()if save:cv2.imwrite("wrapped_"+str(Counter.cnt)+".jpg", warped_image)return def find_qrcode_corners(image, vis=False):gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)  # 二值化  _, binary = cv2.threshold(gray, 127, 255, cv2.THRESH_BINARY)  # # 查找轮廓  # contours, _ = cv2.findContours(binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)# 查找轮廓（OpenCV 4.x） contours, hierarchy = cv2.findContours(binary, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)  #打印轮廓信息  print("总轮廓数:", len(contours))# 筛选和排序轮廓（这里我们假设二维码是最大的轮廓）  contours = sorted(contours, key=cv2.contourArea, reverse=True)  qrcode_contour = contours[0]  # 多边形近似  epsilon = 0.02 * cv2.arcLength(qrcode_contour, True)  approx = cv2.approxPolyDP(qrcode_contour, epsilon, True)  image_out = image.copy()# 提取角点坐标  corners = approx.reshape((-1, 2))  # 在原图上绘制角点  if len(corners) == 4:for corner in corners:  cv2.circle(image_out, (int(corner[0]), int(corner[1])), 25, (0, 0, 255), -1)  # 显示图像  cv2.imshow('QRCode Corners', image_out)  cv2.drawContours(image_out, qrcode_contour, -1, (0, 255, 0), 3)  cv2.imshow('Corners', image_out)  cv2.waitKey(1)   def argParser():parser = argparse.ArgumentParser()parser.add_argument('--rows', type=int, default=3,help='chess board raw num')parser.add_argument('--cols', type=int, default=4,help='chess board col num')parser.add_argument('--online',type=bool, default=False, help='online camera calib')opt = parser.parse_args()return optif __name__ == '__main__':opt = argParser()pattern_size = (opt.rows, opt.cols)calib = Calibrate(pattern_size, 0.06, 0.3, 0.7)cap = cv2.VideoCapture(0)if not cap.isOpened():exit()print(" now start calib %d\n",opt.online)if opt.online:while True:ret, frame = cap.read()flag = calib.find_chessboard_corners(frame, vis=True, save=True, calib=True) cv2.waitKey(1)  if flag:breakelse:path = "img"image_list = os.listdir(path)for img in image_list:img_path = os.path.join(path,img)if os.path.isfile(img_path):frame = cv2.imread(img_path)else:continueprint("image path is: ",img_path)flag = calib.find_chessboard_corners(frame, vis=True, save=False, calib=True)cv2.waitKey(1)if flag:breakprint(calib.H)print(" now start eval \n")if opt.online:while True:ret, frame = cap.read()# 如果正确读取帧，ret为Trueif not ret:print("无法接收帧，请退出")break       calib.world_to_image(frame,True, False)# 显示实时画面cv2.imshow('raw', frame)# 按 'q' 键退出循环if cv2.waitKey(1) & 0xFF == ord('q'):break# 释放摄像头资源并关闭所有窗口cap.release()else:path = "img"image_list = os.listdir(path)for img in image_list:img_path = os.path.join(path,img)if os.path.isfile(img_path):frame = cv2.imread(img_path)else:continuecalib.world_to_image(frame, True, True)# 显示实时画面cv2.imshow('raw', frame)# 按 'q' 键退出循环if cv2.waitKey(1) & 0xFF == ord('q'):breakcv2.destroyAllWindows()