品牌网站建设方案ppt营销推广有哪些公司

一、背景

3D目标检测任务中，标注数据（伪标签和人工标注）或模型预测的车辆3D坐标存在位置精度不准（航向角和位置精度）的情况，特别是旁车道车辆位置不准不准造成目标障碍物有压线或切入的趋势，影响行车安全。2D框检测算法非常成熟（yolo系列），考虑在图像上检测车轮2D位置，通过相机内外参和车身参数将检测出的车轮接地点（接地点可以利用相似三角形将相机坐标系中的点投影到BEV坐标系下）投影到BEV坐标系下，对车辆3D坐标的yaw角和横向位置进行修复。

二、代码

针孔相机原理参考：https://blog.csdn.net/lishiyu93/article/details/143372455

输入参数

# 输入参数举例，以front_long_camera为例
# 示例相机标定的pitch角
front_long_camera_pitch_angle = 3.1397834 
#自车车轮直径
vehicle_param_wheeldiameter = 0.786
# 示例相机到坐标原点的高度（坐标原点一般是自车后轴中心）
front_long_camera_pose_translation_z = 1.1628705
# 示例相机到地面高度
front_long_camera_ground_height = front_long_camera_pose_translation_z + vehicle_param_wheeldiameter / 2
# 示例相机内参 (fx, fy, cx, cy)
front_long_camera_K = np.array([[7329.6445, 0., 1915.2565],[0., 7330.802, 1079.506],[0., 0., 1]])
# 示例相机外参（平移和旋转参数）
front_long_camera_translation = [1.982978, -0.18810384, 1.1628705]
front_long_camera_rotation = [-0.49923673, 0.49701047, -0.5016547, 0.5020815]
# 图像中的轮胎位置 (像素坐标)，通过2D模型得到
wheel1_xmin, wheel1_ymin, wheel1_xmax, wheel1_ymax = [3364.4599609375, 1882.9097900390625, 3637.5610961914062, 2160.0]
wheel2_xmin, wheel2_ymin, wheel2_xmax, wheel2_ymax = [2750.3476333618164, 1678.0156860351562, 2905.0335388183594, 2159.3624267578125]
wheel1 = [(wheel1_xmin + wheel1_xmax) / 2, wheel1_ymax]  # 图像中的轮胎1接地点坐标
wheel2 = [(wheel2_xmin + wheel2_xmax) / 2, wheel2_ymax]  # 图像中的轮胎2接地点坐标

依赖的功能函数

import numpy as np
import mathdef pixel2cam(wheel, K, pitch_angle=0, camera_ground_height=1.55):"""将图像坐标转换为相机坐标系中的 3D 坐标。参数：- wheel: 图像中的像素坐标，格式为 (u, v)- K: 相机的内参矩阵 (3x3)- pitch_angle: 相机俯仰角 (默认 0)- camera_ground_height: 相机到地面的高度 (默认 1.55 米)返回：- cam_points: 相机坐标系中的 3D 坐标"""# 将图像坐标转换为相机坐标系的 3D 点x, y = wheelpt_m = np.array([x, y, 1]).reshape(3, 1)# 相机内参的逆矩阵intrinsic_params_inverse = np.linalg.inv(K)# 反向投影，得到相机坐标系中的点org_camera_point = intrinsic_params_inverse @ pt_m# 计算俯仰角的旋转矩阵cos_pitch = math.cos(pitch_angle)sin_pitch = math.sin(pitch_angle)pitch_matrix = np.array([[1, 0, 0], [0, cos_pitch, sin_pitch], [0, -sin_pitch, cos_pitch]])# 应用旋转矩阵rotate_point = pitch_matrix @ org_camera_point# 缩放因子，保证 Z 轴的深度与地面高度一致scale = -camera_ground_height / rotate_point[1]  # 使用相机坐标系中的 y 坐标 (深度)# 计算 3D 坐标cam_points = scale * org_camera_point.flatten()return cam_pointsdef quaternion_to_euler(q):"""四元数 (w, x, y, z) 转欧拉角 (roll, pitch, yaw)输出：欧拉角 (弧度制)"""w, x, y, z = q# 计算 Rollsin_r_cos_p = 2 * (w * x + y * z)cos_r_cos_p = 1 - 2 * (x**2 + y**2)roll = np.arctan2(sin_r_cos_p, cos_r_cos_p)# 计算 Pitchsin_p = 2 * (w * y - z * x)pitch = np.arcsin(sin_p)  # 限制范围 [-1, 1]# 计算 Yawsin_y_cos_p = 2 * (w * z + x * y)cos_y_cos_p = 1 - 2 * (y**2 + z**2)yaw = np.arctan2(sin_y_cos_p, cos_y_cos_p)return np.array([roll, pitch, yaw])def quaternion2Rot(q): """功能: 四元数转旋转矩阵q: 四元素return R: 旋转矩阵"""q0,q1,q2,q3=q R=np.array([[1-2*(q2**2+q3**2),2*(q1*q2-q3*q0),2*(q1*q3+q2*q0)], [2*(q1*q2+q3*q0),1-2*(q1**2+q3**2),2*(q2*q3-q1*q0)], [2*(q1*q3-q2*q0),2*(q2*q3+q1*q0),1-2*(q1**2+q2**2)]]) return R def getPose(translation, rotation):"""功能: 获取设备的旋转平移矩阵Args:translation: 平移参数['x', 'y', 'z']rotation: 四元素参数['w', 'x', 'y', 'z']Returns:tranformation: 根据四元素和平移量计算出来的旋转平移矩阵"""tranformation = np.identity(4)if rotation.ndim == 1:tranformation[0:3, 0:3] = quaternion2Rot(rotation) elif rotation.ndim == 2:tranformation[0:3, 0:3] = rotationtranformation[0][3] = translation[0]tranformation[1][3] = translation[1]tranformation[2][3] = translation[2]return tranformationdef transformPoints(points, transformation):"""功能: 坐标转换points: 点(n,3)transformation: 旋转平移矩阵(4,4)return points: 坐标转换之后的点"""points = np.dot(transformation[0:3, 0:3], points.transpose()).transpose()points += transformation[0:3, 3]return pointsdef angle_between_points(x1, y1, x2, y2):# 计算方向向量的 y 和 x 分量delta_x = x2 - x1delta_y = y2 - y1# 使用反正切函数计算弧度angle_radians = math.atan2(delta_y, delta_x)return angle_radiansdef normalize_angle(angle):"""将角度归一化到 [-π, π] 范围。"""while angle > math.pi:angle -= 2 * math.piwhile angle < -math.pi:angle += 2 * math.pireturn angle# 通过车轮接地点修正3D框示例
def rectify_original_data(original_position_y, original_dimension_width, original_yaw, wheel_rectify_data, current_gt_ids, position_y_threshold = 0.15, yaw_threshold = 0.05):#小车后视镜距离车身0.2米左右，打车后视镜距离车身0.9米左右car_rearview_mirror_distance = 0.2big_car_rearview_mirror_distance = 0.9width_diff = abs(abs(original_position_y - wheel_rectify_data[current_gt_ids]["two_ego_point_mean_y"]) - original_dimension_width / 2)#wheel_label classes = ['LEFT_FRONT', 'LEFT_REAR', 'RIGHT_FRONT', 'RIGHT_REAR', 'MID']wheel_label = wheel_rectify_data[current_gt_ids]["wheel_label"]if wheel_label == 5:width_diff_flag = 0elif wheel_label == 4:width_diff_flag = 0elif wheel_label < 2 :width_diff_flag = -1else: width_diff_flag = 1if abs(width_diff - car_rearview_mirror_distance) < position_y_threshold:new_position_y = original_position_y + width_diff_flag * np.sign(original_yaw) * abs(width_diff - car_rearview_mirror_distance)elif abs(width_diff - big_car_rearview_mirror_distance) < position_y_threshold:new_position_y = original_position_y + width_diff_flag * np.sign(original_yaw) * abs(width_diff - big_car_rearview_mirror_distance)else:new_position_y = original_position_ytwo_ego_point_angle = wheel_rectify_data[current_gt_ids]["two_ego_point_angle"]two_ego_point_angle_2 = normalize_angle(two_ego_point_angle - math.pi)  #补角if abs(two_ego_point_angle - original_yaw) > yaw_threshold and abs(two_ego_point_angle_2 - original_yaw) > yaw_threshold:new_yaw = original_yawelse:if abs(two_ego_point_angle - original_yaw) <= yaw_threshold:new_yaw = two_ego_point_angleelse:new_yaw = two_ego_point_angle_2return new_position_y, new_yawdef find_closest_points_by_y(points_info):"""根据第二个坐标 y 的值找出距离最近的两个点:param points: 点的列表，每个点是一个 numpy 数组:return: 距离最近的点对和对应的 y 距离"""min_distance = float('inf')closest_pair = Noneclosest_indices = Noneindexed_points_info = list(enumerate(points_info))# 遍历所有点对的组合for (i1, p1), (i2, p2) in combinations(indexed_points_info, 2):y1 = p1["wheel_cam_ego"][1]  # 提取第一个点的 y 坐标y2 = p2["wheel_cam_ego"][1]  # 提取第二个点的 y 坐标distance = abs(y1 - y2)  # 计算 y 坐标的绝对差值if distance < min_distance:min_distance = distanceclosest_pair = (p1, p2)closest_indices = (i1, i2)  # 保存索引return closest_pair, min_distance, closest_indicesdef get_ego_wheel_data(camera_info, gt_lidar_info, original_wheel_data_info):ego_wheel_data_info = {}for box_id, original_box_info in original_wheel_data_info.items():if len(original_box_info) > 1:ego_wheel_data_info[str(box_id)] = {}ego_wheel_point_info = []for current_original_box_info_i in original_box_info:current_wheel = [(current_original_box_info_i["wheel_xmin"] + current_original_box_info_i["wheel_xmax"]) / 2, current_original_box_info_i["wheel_ymax"]]current_wheel_cam = pixel2cam(current_wheel, camera_info["front_long_camera_K"], \camera_info["front_long_camera_pitch_angle"], \camera_info["front_long_camera_ground_height"])current_wheel_cam_ego = transformPoints(np.array(current_wheel_cam), \np.array(camera_info["front_long_camera_transform"]))current_wheel_ego_point_info = {}current_wheel_ego_point_info["wheel_cam_ego"] = current_wheel_cam_egocurrent_wheel_ego_point_info["obs_class"] = current_original_box_info_i["obs_class"]current_wheel_ego_point_info["wheel_label"] = current_original_box_info_i["wheel_label"]ego_wheel_point_info.append(current_wheel_ego_point_info)if len(ego_wheel_point_info) > 2:new_ego_wheel_point_info, two_ego_point_min_distance, two_ego_point_closest_indices = find_closest_points_by_y(ego_wheel_point_info)ego_wheel_point = (ego_wheel_point_info[two_ego_point_closest_indices[0]]["wheel_cam_ego"], ego_wheel_point_info[two_ego_point_closest_indices[0]]["wheel_cam_ego"])else:ego_wheel_point = (ego_wheel_point_info[0]["wheel_cam_ego"], ego_wheel_point_info[1]["wheel_cam_ego"])two_ego_point_min_distance = abs(ego_wheel_point_info[0]["wheel_cam_ego"][1] - ego_wheel_point_info[1]["wheel_cam_ego"][1])two_ego_point_closest_indices = (0, 1)new_ego_wheel_point_info = ego_wheel_point_infotwo_ego_point_angle = angle_between_points(ego_wheel_point[0][0], \ego_wheel_point[0][1], \ego_wheel_point[1][0], \ego_wheel_point[1][1])ego_wheel_data_info[str(box_id)]["ego_wheel_point_info"] = ego_wheel_point_infoego_wheel_data_info[str(box_id)]["new_ego_wheel_point_info"] = new_ego_wheel_point_infoego_wheel_data_info[str(box_id)]["two_ego_point_angle"] = two_ego_point_angleego_wheel_data_info[str(box_id)]["two_ego_point_min_distance"] = two_ego_point_min_distanceego_wheel_data_info[str(box_id)]["obs_class"] = ego_wheel_point_info[0]["obs_class"]ego_wheel_data_info[str(box_id)]["two_ego_point_mean_y"] = (ego_wheel_point[0][1] + ego_wheel_point[1][1]) / 2#classes = ['LEFT_FRONT', 'LEFT_REAR', 'RIGHT_FRONT', 'RIGHT_REAR', 'MID']wheel_label_0 = ego_wheel_point_info[two_ego_point_closest_indices[0]]["wheel_label"]wheel_label_1 = ego_wheel_point_info[two_ego_point_closest_indices[1]]["wheel_label"]if wheel_label_0 == 4 or wheel_label_1 == 4:if wheel_label_0 == 4 and wheel_label_1 == 4:   #两个中间车轮current_wheel_label = 4elif wheel_label_0 == 4 and wheel_label_1 != 4:current_wheel_label = wheel_label_1else:current_wheel_label = wheel_label_0elif wheel_label_0 < 2 and wheel_label_1 < 2:current_wheel_label = 0elif wheel_label_0 >= 2 and wheel_label_1 >= 2:current_wheel_label = 2else: #一个左轮一个右轮current_wheel_label = 5ego_wheel_data_info[str(box_id)]["wheel_label"] = current_wheel_labelreturn ego_wheel_data_info

计算流程

#构建相机的外参矩阵（旋转和平移）
front_long_camera_transform = getPose(np.array(front_long_camera_translation), np.array(front_long_camera_rotation))# 将图像坐标转换为相机坐标系中的 3D 坐标。
wheel1_cam = pixel2cam(wheel1, front_long_camera_K, front_long_camera_pitch_angle, front_long_camera_ground_height)
wheel2_cam = pixel2cam(wheel2, front_long_camera_K, front_long_camera_pitch_angle, front_long_camera_ground_height)# 将相机坐标系下的点转换到自车坐标系
wheel1_cam_ego = transformPoints(np.array(wheel1_cam), np.array(front_long_camera_transform))
wheel2_cam_ego = transformPoints(np.array(wheel2_cam), np.array(front_long_camera_transform))# 计算两点连线的角度（yaw角）
two_ego_point_angle = angle_between_points(wheel1_cam_ego[0], wheel1_cam_ego[1], wheel2_cam_ego[0], wheel2_cam_ego[1])#计算车轮接地点中点
two_ego_point_mean_y = (wheel1_cam_ego[1] + wheel2_cam_ego[1]) / 2# 通过车轮接地点修正3D框示例
new_position_y, new_yaw = rectify_original_data(vehicle_position[1], vehicle_dimension[0], vehicle_yaw, wheel_rectify_data, current_gt_ids)

位置修复策略说明：

1）yaw：在自车坐标系计算两个车轮接地点的夹角（弧度），夹角或夹角补角与原始gt的yaw角差值的绝对值大于角度阈值（0.05弧度=2.866°）则不修改原始gt的yaw角，若夹角与原始gt的yaw角差值绝对值小于角度阈值则用夹角替换原始gt的yaw角，若夹角补角与原始gt的yaw角差值绝对值小于角度阈值则用夹角补角替换原始gt的yaw角；
2）横向位置：小车后视镜距离车身car_rearview_mirror_distance0.2米左右，打车后视镜距离车身big_car_rearview_mirror_distance0.9米左右。根据检测出的距离最近的车轮的属性classes = [‘LEFT_FRONT’, ‘LEFT_REAR’, ‘RIGHT_FRONT’, ‘RIGHT_REAR’, ‘MID’]，判断位置调整方向，两轮都是中间车轮时，无法判断车轮方向，width_diff_flag = 0，两轮一左一右时，width_diff_flag = 0，有一个以上为左轮时，width_diff_flag = -1，有一个以上为右轮时，width_diff_flag = 1；自车前方车辆有同向车辆和对向车辆，可以根据原始gt的yaw角正负值判断，一起用于位置调整方向判断，最终目的，计算的车辆宽度减去后视镜距离的差值绝对值后与原始gt的宽度差值绝对值小于阈值position_y_threshold（小车与大车的阈值都设为0.15米）时，以自车坐标系前进方向视角，目标车辆位于自车左侧，将目标车辆gt中心点向左移，目标车辆位于自车右侧，将目标车辆gt中心点向右移；具体计算逻辑如下，计算两个车轮接地点横向坐标的中点y_mean，计算y_mean与原始gt横向边界的差值width_diff，原始gt的中心点横向坐标为original_position_y，修复的中心点横向坐标new_position_y，

if abs(width_diff - car_rearview_mirror_distance) < position_y_threshold:new_position_y = original_position_y + width_diff_flag * np.sign(original_yaw) * abs(width_diff - car_rearview_mirror_distance)elif abs(width_diff - big_car_rearview_mirror_distance) < position_y_threshold:new_position_y = original_position_y + width_diff_flag * np.sign(original_yaw) * abs(width_diff - big_car_rearview_mirror_distance)else:new_position_y = original_position_y

二、拓展

车轮矫正策略（rectify_original_data函数），可以根据2D图像车轮检测效果和自己数据的质量对阈值和调整方法进行改进。

将3D框画到图像上可视化代码：

import cv2
import torch
import numpy as npdef check_numpy_to_torch(x):if isinstance(x, np.ndarray):return torch.from_numpy(x).float(), Truereturn x, Falsedef rotate_points_along_z(points, angle):"""Args:points: (B, N, 3 + C)angle: (B), angle along z-axis, angle increases x ==> yReturns:"""points, is_numpy = check_numpy_to_torch(points)angle, _ = check_numpy_to_torch(angle)cosa = torch.cos(angle)sina = torch.sin(angle)zeros = angle.new_zeros(points.shape[0])ones = angle.new_ones(points.shape[0])rot_matrix = torch.stack((cosa,  sina, zeros,-sina, cosa, zeros,zeros, zeros, ones), dim=1).view(-1, 3, 3).float()points_rot = torch.matmul(points[:, :, 0:3], rot_matrix)points_rot = torch.cat((points_rot, points[:, :, 3:]), dim=-1)return points_rot.numpy() if is_numpy else points_rotdef boxes_to_corners_3d(boxes3d):"""7 -------- 4/|         /|6 -------- 5 .| |        | |. 3 -------- 0|/         |/2 -------- 1Args:boxes3d:  (N, 7) [x, y, z, dx, dy, dz, heading], (x, y, z) is the box centerReturns:"""boxes3d, is_numpy = check_numpy_to_torch(boxes3d)template = boxes3d.new_tensor(([1, 1, -1], [1, -1, -1], [-1, -1, -1], [-1, 1, -1],[1, 1, 1], [1, -1, 1], [-1, -1, 1], [-1, 1, 1],)) / 2corners3d = boxes3d[:, None, 3:6].repeat(1, 8, 1) * template[None, :, :]corners3d = rotate_points_along_z(corners3d.view(-1, 8, 3).float(), boxes3d[:, 6]).view(-1, 8, 3)corners3d += boxes3d[:, None, 0:3]return corners3d.numpy() if is_numpy else corners3ddef lidar2camera(position: list, lv_trans: tuple, vc_trans: tuple):lidar_position = np.array(position).reshape(3, 1)lv_R, lv_T = lv_transvehicle_position = np.matmul(lv_R, lidar_position) + lv_Tvc_R, vc_T = vc_transcamera_postion = np.matmul(vc_R, vehicle_position) + vc_Treturn list(camera_postion.reshape(-1)), list(vehicle_position.reshape(-1))def draw_line(img, pt0, pt1, width, height, color, line_width):if 0 <= int(pt0[0]) < width and 0 <= int(pt0[1]) < height \and 0 <= int(pt1[0]) < width and 0 <= int(pt1[1]) < height:cv2.line(img, (int(pt0[0]), int(pt0[1])), (int(pt1[0]), int(pt1[1])), color, line_width)def draw_3dbbox_in_image(name, cv_img, fov, lidar_boxes3d_list=None, vehicle_boxes3d_list=None,lv_trans=None, vc_trans=None, camera_intrinsics=None, distort=None, color = (0, 255, 0), score=None, cam_key=None,bbox_show_height=True,show_score = True,show_heading = True,):if lidar_boxes3d_list is None and vehicle_boxes3d_list is None:print("error: lidar_boxes3d_list is None, vehicle_boxes3d_list is None")returnif lidar_boxes3d_list and vehicle_boxes3d_list:print("error: lidar_boxes3d_list and vehicle_boxes3d_list is  both not None!")returnheight, width  = cv_img.shape[0:2]if lidar_boxes3d_list is not None:lidar_position = lidar_boxes3d_list[0][:3]lidar_boxes3d = torch.tensor(lidar_boxes3d_list)# 雷达坐标系x朝左，y朝后，z朝上，，传入的对应的lidar_yaw角为与x轴的夹角# 其lidar_yaw角转车辆坐标系(x朝前，y朝左)需要+lv_yaw的操作,即车辆系yaw角=lidar_yaw+lv_yaw# 传入的position(x, y, z) is of the box center, not bootom centerlidar_boxes3d = boxes_to_corners_3d(lidar_boxes3d)camera_center, vehicle_center = lidar2camera(lidar_position, lv_trans, vc_trans)cor_fov = np.rad2deg(np.arctan2(camera_center[0], camera_center[2]))*2if abs(cor_fov)+3 > fov: return# print("cam_key： ", cam_key, lidar_boxes3d_list, lidar_boxes3d, __file__) # =================ct:===========# 把每个lidar的3d点转换到相机上corner_3d_gts = []for i in range(9): # 最后一維是中心点corner_3d_camera = Noneif lidar_boxes3d_list is not None and i < 8:corner_3d_camera, _ = lidar2camera(lidar_boxes3d[0][i], lv_trans, vc_trans)if i == 8:corner_3d_camera = camera_centerif "fisheye" in cam_key:corner_3d_gt, _ = cv2.fisheye.projectPoints(np.array([[corner_3d_camera]]), (0, 0, 0), (0, 0, 0), camera_intrinsics, np.array(distort))else:corner_3d_gt, _ = cv2.projectPoints(np.array(corner_3d_camera), np.array((0,0,0), dtype=np.float32).reshape(3,1), np.array((0,0,0),dtype=np.float32).reshape(3,1), camera_intrinsics, np.array(distort))# draw_3dbbox_in_image 支持 过滤无效的投影点！！！Xc,Yc,Zc = corner_3d_camera[0]/corner_3d_camera[2], corner_3d_camera[1]/corner_3d_camera[2], corner_3d_camera[2]# z=1平面上的入射点，与x轴的夹角和y轴的夹角arctanx = np.arctan(np.abs(Xc)) / np.pi * 180arctany = np.arctan(np.abs(Yc)) / np.pi * 180corner_3d_gt = corner_3d_gt[0][0]valid_mask = (corner_3d_gt[0] >= 0) & (corner_3d_gt[0] < width) \& (corner_3d_gt[1] >= 0) & (corner_3d_gt[1] < height) & (Zc > 0.01)if valid_mask==False: corner_3d_gt = [-1, -1]corner_3d_gts.append(corner_3d_gt) """7 -------- 4/|         /|6 -------- 5 .| |        | |. 3 -------- 0|/         |/2 -------- 1[0, 1, 4, 5]為正前方"""########################################### 3D框在圖像上的绘制 #############################line_width = 1cv2.circle(cv_img, (int(corner_3d_gts[8][0]), int(corner_3d_gts[8][1])), 2, (0, 0, 255),3) # 3D框中心點draw_line(cv_img, corner_3d_gts[0], corner_3d_gts[1], width, height, color, line_width)draw_line(cv_img, corner_3d_gts[0], corner_3d_gts[3], width, height, color, line_width)draw_line(cv_img, corner_3d_gts[1], corner_3d_gts[2], width, height, color, line_width)draw_line(cv_img, corner_3d_gts[2], corner_3d_gts[3], width, height, color, line_width)if bbox_show_height:draw_line(cv_img, corner_3d_gts[0], corner_3d_gts[4], width, height, color, line_width)draw_line(cv_img, corner_3d_gts[1], corner_3d_gts[5], width, height, color, line_width)draw_line(cv_img, corner_3d_gts[3], corner_3d_gts[7], width, height, color, line_width)draw_line(cv_img, corner_3d_gts[2], corner_3d_gts[6], width, height, color, line_width)draw_line(cv_img, corner_3d_gts[4], corner_3d_gts[5], width, height, color, line_width)draw_line(cv_img, corner_3d_gts[4], corner_3d_gts[7], width, height, color, line_width)draw_line(cv_img, corner_3d_gts[5], corner_3d_gts[6], width, height, color, line_width)draw_line(cv_img, corner_3d_gts[6], corner_3d_gts[7], width, height, color, line_width)if show_heading:########################################### 立方体前面四边形heading:# draw `X' in the frontdraw_line(cv_img, corner_3d_gts[0], corner_3d_gts[5], width, height, (241, 101, 72), line_width)draw_line(cv_img, corner_3d_gts[1], corner_3d_gts[4], width, height, (241, 101, 72), line_width)if show_score:info_str = ""if score: info_str += f"{score:.2f}"cv2.putText(cv_img, info_str, (int(corner_3d_gts[4][0]+1), int(corner_3d_gts[4][1])-7), cv2.FONT_HERSHEY_DUPLEX, 0.9, color, thickness=line_width)