1244 lines
52 KiB
Python
1244 lines
52 KiB
Python
# -- coding: utf-8 --
|
||
from ctypes import *
|
||
import cv2 # 4.9.0
|
||
print(cv2.__version__) # 4.9.0
|
||
import numpy as np
|
||
print(np.__version__) # 1.24.4
|
||
import open3d as o3d
|
||
print(o3d.__version__) # 0.18.0
|
||
import math
|
||
import os
|
||
import time
|
||
# from skimage import feature
|
||
import aurco_3d_detect as aurco_3d_detect
|
||
from os import abort
|
||
from utils import Log
|
||
from config import *
|
||
|
||
# 点云整体移动 kk
|
||
# 边缘点散射后需要重新投影到平面
|
||
# 顶点算法再优化
|
||
|
||
# 流程
|
||
# 3D点云整理修正，kk参数(该参数与物体反射率相关，通过MAP映射，后期不断更新MAP内容)
|
||
# 3D点云空间过滤
|
||
# 3D点云提取标定板 seg_mark_cloud
|
||
# 生成 im_2d_to_3d （采用相机内参）
|
||
# 生成 im_project （采用相机内参）
|
||
# 通过 im_project 识别标定板2D区域 obj_roi
|
||
# 将 im_2d_to_3d 中的3D点云数据按照 obj_roi 进行归集，形成单独的标定板的3D数据文件
|
||
# 优化标定板3D数据 correct_mark_cloud
|
||
# 3D 数据平滑、游离点\噪点去除
|
||
# 使用 RANSAC 平面拟合，计算平面模型 [A,B,C,D]、计算内点集合 inlier_cloud 、计算外点集合 outlier_cloud
|
||
# 最小二乘计算 inlier_cloud 拟合的平面
|
||
# 通过平面模型 [A,B,C,D] 将内外点重投至该平面，计算 inlier_pcd_correct
|
||
# 将 inlier_pcd_correct 和 inlier_cloud 合并至 inlier_pcd_correct
|
||
# 3D标定板角点提取 select_conners_from_mark_cloud_2d
|
||
# 通过 select_conners_from_mark_cloud_3d 对 inlier_pcd_correct 自动选取 3D 角点 （3D角点自动提取算法）
|
||
# 将 inlier_pcd_correct 投影至 2D
|
||
# 生成 im_2d_to_3d_n (n:1-4)（采用相机内参）
|
||
# 生成 im_project_n (n:1-4) （采用相机内参）
|
||
# 在2D图中人工标记角点
|
||
# 通过2D点、H、[A,B,C,D] 得到3D角点 c_lidar[16]
|
||
# 【check】通过3D角点 c_lidar[16] 计算得出的标定板边长 l 和实际值的误差（绝对误差、4条边的均方差）
|
||
# 2D标定板角点提取
|
||
# 定位 ARUCO 标记，计算外参
|
||
# 计算标定板4个角点的3D值 c_camera[16]
|
||
# 【check】通过3D角点 c_camera[16] 计算得出的标定板边长 l 和实际值的误差（绝对误差、4条边的均方差）
|
||
# SVD
|
||
# 使用SVD c_lidar[16] 和 c_camera[16] 计算相机和雷达的旋转矩阵 RT
|
||
# 循环采用不同数据通过SVD进行上述计算，采用最大似然优化 RT
|
||
# 【check】c_lidar[16] 通过 RT 恢复到的位置和 c_camera[16] 比较（绝对误差、4条边的均方差）
|
||
# 【check】3D点云通过重投影到2D图像的角点和标定板图像角点的误差估计（人工）
|
||
# 推理验证
|
||
# 2D识别标定板
|
||
# 2D标定板角点提取
|
||
# 通过2D点、H、[A,B,C,D] 得到3D角点，c_lidar_camera[16]
|
||
# 【check】c_lidar_camera[16]、c_lidar[16] 的误差（绝对误差、4条边的均方差）
|
||
# 【check】c_lidar_camera[16]、c_camera[16] 、c_lidar[16]计算得出的边长和实际值的误差（绝对误差、4条边的均方差）
|
||
# 自动化参数优化
|
||
# 实现自动推理验证流程
|
||
# 生成参数和结果对照记录
|
||
# 自动优化参数
|
||
# 预埋件实测
|
||
|
||
def _eulerAnglesToRotationMatrix(theta) :
|
||
R_x = np.array([[1, 0, 0 ],
|
||
[0, math.cos(theta[0]), -math.sin(theta[0]) ],
|
||
[0, math.sin(theta[0]), math.cos(theta[0]) ]
|
||
])
|
||
R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1]) ],
|
||
[0, 1, 0 ],
|
||
[-math.sin(theta[1]), 0, math.cos(theta[1]) ]
|
||
])
|
||
R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0],
|
||
[math.sin(theta[2]), math.cos(theta[2]), 0],
|
||
[0, 0, 1]
|
||
])
|
||
R = np.dot(R_z, np.dot( R_y, R_x ))
|
||
return R
|
||
|
||
def cal_project_matrix():
|
||
# src_size是原始图像的大小
|
||
src_size = (w, h)
|
||
|
||
# 计算新的摄像机矩阵
|
||
new_camera_matrix = cv2.getOptimalNewCameraMatrix(camera_matrix, dist_coeffs, src_size, alpha=0)
|
||
Log.info("2D 投影矩阵计算结果为：")
|
||
print(new_camera_matrix)
|
||
# print(new_camera_matrix)
|
||
return new_camera_matrix
|
||
|
||
|
||
# 定义获取点云深度最大最小值
|
||
def get_pcd_depth_range(pts_3d):
|
||
# 初始化最小值和最大值为第一个点的x坐标
|
||
min_x = 0
|
||
max_x = 0
|
||
|
||
# 遍历所有点，更新最小值和最大值
|
||
for point_3d in pts_3d.points:
|
||
x = point_3d[0]
|
||
if x < min_x:
|
||
min_x = x
|
||
elif x > max_x:
|
||
max_x = x
|
||
|
||
return max_x, min_x
|
||
|
||
def undistort(distorted_img):
|
||
# 矫正畸变
|
||
undistorted_img = cv2.undistort(distorted_img, camera_matrix, dist_coeffs, None, camera_matrix)
|
||
# cv2.imwrite("distorted_img.png", undistorted_img)
|
||
# undistorted_img = cv2.imread("distorted_img.png")
|
||
# cv2.imshow("1", undistorted_img)
|
||
# cv2.waitKey(0)
|
||
return undistorted_img
|
||
|
||
def clear_folder(folder_path):
|
||
for file_name in os.listdir(folder_path):
|
||
file_path = os.path.join(folder_path, file_name)
|
||
if os.path.isfile(file_path):
|
||
os.remove(file_path)
|
||
else:
|
||
clear_folder(file_path)
|
||
os.rmdir(file_path)
|
||
|
||
|
||
def sort_marks_pcd_3d(masks):
|
||
new_mask = []
|
||
left_pt_0, left_pt_1, left_pt_2, left_pt_3 = 0,0,0,0
|
||
delete_index = []
|
||
coordinate_temp = [0,1,2,3]
|
||
max_val = 0
|
||
min_val = 999999
|
||
for i in range(4):
|
||
left_pt = masks[i][0]
|
||
val = left_pt[0] + left_pt[1]
|
||
if val > max_val:
|
||
left_pt_3 = i
|
||
max_val = val
|
||
if val < min_val:
|
||
left_pt_0 = i
|
||
min_val = val
|
||
delete_index.append(left_pt_0)
|
||
delete_index.append(left_pt_3)
|
||
coordinate_temp = np.delete(coordinate_temp, delete_index, axis=0)
|
||
if masks[coordinate_temp[0]][0][0] > masks[coordinate_temp[1]][0][0]:
|
||
left_pt_1 = coordinate_temp[0]
|
||
left_pt_2 = coordinate_temp[1]
|
||
else:
|
||
left_pt_2 = coordinate_temp[0]
|
||
left_pt_1 = coordinate_temp[1]
|
||
new_mask.append(masks[left_pt_0])
|
||
new_mask.append(masks[left_pt_1])
|
||
new_mask.append(masks[left_pt_2])
|
||
new_mask.append(masks[left_pt_3])
|
||
return new_mask
|
||
|
||
|
||
def detect_marks_roi(im_project, show=False):
|
||
masks = []
|
||
|
||
offset = 40
|
||
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (30, 30))
|
||
im_tmp = cv2.morphologyEx(im_project, cv2.MORPH_DILATE, kernel)
|
||
im_tmp = cv2.GaussianBlur(im_tmp, (29, 29), 0)
|
||
|
||
# im_tmp = cv2.resize(im_tmp, (900, 700))
|
||
# cv2.imshow(".",im_tmp)
|
||
# cv2.waitKey(0)
|
||
|
||
# 查找联通区域
|
||
gray = cv2.cvtColor(im_tmp, cv2.COLOR_BGR2GRAY)
|
||
_, binary = cv2.threshold(gray, 128, 255, cv2.THRESH_BINARY)
|
||
# num_labels, labels, stats, centroids = cv2.connectedComponentsWithStats(binary, connectivity=8, ltype=cv2.CV_32S)
|
||
|
||
|
||
contours, _ = cv2.findContours(binary, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
|
||
colors = [ (255, 0, 0) , (255, 255, 0) , (0, 0, 255) , (255, 0, 255) ]
|
||
color_idx = 0
|
||
|
||
Log.info("3D->2D 投影连通区域的像素点数量：")
|
||
for contour in contours:
|
||
rect_conner_points = []
|
||
size = cv2.contourArea(contour)
|
||
top_idx = 0
|
||
right_idx = 0
|
||
bottom_idx = 0
|
||
left_idx = 0
|
||
x, y, w, h = cv2.boundingRect(contour)
|
||
for idx in range(len(contour)):
|
||
if y == contour[idx][0][1]: top_idx = idx
|
||
if x == contour[idx][0][0]: left_idx = idx
|
||
if y + h - 1 == contour[idx][0][1]: bottom_idx = idx
|
||
if x + w - 1 == contour[idx][0][0]: right_idx = idx
|
||
contour[left_idx][0][0] -= offset
|
||
contour[top_idx][0][1] -= offset
|
||
contour[right_idx][0][0] += offset
|
||
contour[bottom_idx][0][1] += offset
|
||
if contour[left_idx][0][0] < 0 or contour[top_idx][0][1] < 0 or \
|
||
contour[right_idx][0][0] > global_w or contour[bottom_idx][0][1] > global_h:
|
||
continue
|
||
if size > rmin:
|
||
print(size)
|
||
if size > rmin and size < rmax:
|
||
contour[left_idx][0][0] -= offset
|
||
contour[top_idx][0][1] -= offset
|
||
contour[right_idx][0][0] += offset
|
||
contour[bottom_idx][0][1] += offset
|
||
cv2.circle(im_project, (contour[top_idx][0][0], contour[top_idx][0][1]), 25, colors[color_idx] , -1)
|
||
cv2.circle(im_project, (contour[right_idx][0][0], contour[right_idx][0][1]), 25, colors[color_idx], -1)
|
||
cv2.circle(im_project, (contour[bottom_idx][0][0], contour[bottom_idx][0][1]), 25, colors[color_idx], -1)
|
||
cv2.circle(im_project, (contour[left_idx][0][0], contour[left_idx][0][1]), 25, colors[color_idx], -1)
|
||
rect_conner_points.append(contour[left_idx][0])
|
||
rect_conner_points.append(contour[top_idx][0])
|
||
rect_conner_points.append(contour[right_idx][0])
|
||
rect_conner_points.append(contour[bottom_idx][0])
|
||
masks.append(rect_conner_points)
|
||
color_idx += 1
|
||
|
||
|
||
if len(masks) != MARK_COUNT:
|
||
Log.error("3D 重投影未找到4个mark")
|
||
show = True
|
||
if show:
|
||
im_project = cv2.resize(im_project, (900, 700))
|
||
cv2.imshow("im", im_project)
|
||
cv2.waitKey(0)
|
||
return []
|
||
|
||
# 按照 左上1，右上2，左下3，右下4的顺序重新排序
|
||
masks = sort_marks_pcd_3d(masks)
|
||
|
||
# for test
|
||
if show:
|
||
im_project = cv2.resize(im_project, (900, 700))
|
||
cv2.imshow("im", im_project)
|
||
cv2.waitKey(0)
|
||
return masks
|
||
|
||
|
||
def project_cloud(pcd_cloud):
|
||
im_project = np.zeros((global_h, global_w, 3), dtype=np.uint8)
|
||
im_2d_to_3d = np.zeros((global_h ,global_w, 3), dtype=np.float64)
|
||
rvec = np.array([np.array([0.]), np.array([0.]), np.array([0.])])
|
||
tvec = np.float64([0.0, 0.0, 0.0])
|
||
pts_2d, _ = cv2.projectPoints(np.array(pcd_cloud.points), rvec, tvec, camera_matrix, dist_coeffs)
|
||
color = (255,255,255)
|
||
list_pts_2d = pts_2d.tolist()
|
||
list_pts_2d = [[[int(item) for item in subsublist] for subsublist in sublist] for sublist in list_pts_2d]
|
||
points_np = np.asarray(pcd_cloud.points)
|
||
for i, pt_2d in enumerate(list_pts_2d):
|
||
# x, y = pt_2d.ravel()
|
||
# if x == float('nan') or y == float('nan'): continue
|
||
# x, y = int(x), int(y)
|
||
# x, y = pt_2d[0][0], pt_2d[0][1]
|
||
if 0 <= pt_2d[0][0] < global_w and 0 <= pt_2d[0][1] < global_h:
|
||
im_project[pt_2d[0][1], pt_2d[0][0]] = color
|
||
# im_2d_to_3d[y, x][0] = pcd_cloud.points[i][0]
|
||
# im_2d_to_3d[y, x][1] = pcd_cloud.points[i][1]
|
||
# im_2d_to_3d[y, x][2] = pcd_cloud.points[i][2]
|
||
im_2d_to_3d[pt_2d[0][1], pt_2d[0][0]] = points_np[i]
|
||
return im_project, im_2d_to_3d, pts_2d, list_pts_2d
|
||
|
||
def seg_mark_cloud(ply_file_path, kk, show=False):
|
||
marks_pcd = []
|
||
|
||
src_cloud_load = o3d.io.read_point_cloud(ply_file_path)
|
||
if src_cloud_load is None or len(src_cloud_load.points) == 0 :
|
||
abort()
|
||
|
||
start_time = time.time()
|
||
|
||
# 点云旋转
|
||
Rx = src_cloud_load.get_rotation_matrix_from_xyz((roll, 0, 0))
|
||
Ry = src_cloud_load.get_rotation_matrix_from_xyz((0, pitch, 0))
|
||
Rz = src_cloud_load.get_rotation_matrix_from_xyz((0, 0, yaw))
|
||
# t_matrix = np.identity(4)
|
||
# t_matrix[:3, 3] = T_vector
|
||
src_cloud_load.rotate(Rx, (1,0,0))
|
||
src_cloud_load.rotate(Ry, (0,1,0))
|
||
src_cloud_load.rotate(Rz, (0,0,1))
|
||
src_cloud_load.translate(T_vector)
|
||
|
||
# 点云筛选
|
||
project_inlier_cloud = o3d.geometry.PointCloud()
|
||
for point_3d in np.asarray(src_cloud_load.points):
|
||
if -2.0 < point_3d[0] < 2.0 and -2.0 < point_3d[1] < 2.0 and point_3d[2] < 3.5: # 筛选x,y,z坐标
|
||
_r = math.sqrt(pow(point_3d[0], 2) + pow(point_3d[1], 2) + pow(point_3d[2], 2))
|
||
if _r == 0: continue
|
||
if point_3d[0] > 0: point_3d[0] = point_3d[0] + point_3d[0] / _r * kk
|
||
else: point_3d[0] = point_3d[0] + point_3d[0] / _r * kk
|
||
if point_3d[1] > 0: point_3d[1] = point_3d[1] + point_3d[1] / _r * kk
|
||
else: point_3d[1] = point_3d[1] + point_3d[1] / _r * kk
|
||
point_3d[2] = point_3d[2] + point_3d[2] / _r * kk
|
||
# point_3d[2] -= 0.0
|
||
project_inlier_cloud.points.append((point_3d[0], point_3d[1], point_3d[2]))
|
||
|
||
# 投影3D点云
|
||
im_project, im_2d_to_3d, pts_2d, list_pts_2d = project_cloud(project_inlier_cloud)
|
||
|
||
# 自动识别ROI角点
|
||
mark_rois = detect_marks_roi(im_project, show)
|
||
if len(mark_rois) != MARK_COUNT:
|
||
return marks_pcd
|
||
|
||
# 保存4个点云的数据到文件
|
||
# clear_folder("./detect_cloud")
|
||
clear_folder(conf_temp_cloud_path)
|
||
# list_pts_2d = pts_2d.tolist()
|
||
# list_pts_2d = [[[int(item) for item in subsublist] for subsublist in sublist] for sublist in list_pts_2d]
|
||
for i in range(len(mark_rois)):
|
||
if len(mark_rois[i]) < 4: continue
|
||
one_mark_pcd = o3d.geometry.PointCloud()
|
||
mark_rois_tmp = np.asarray(mark_rois[i])
|
||
for _, pt_2d in enumerate(list_pts_2d):
|
||
# x, y = pt_2d.ravel()
|
||
# x, y = int(x), int(y)
|
||
# x, y = pt_2d[0][0], pt_2d[0][1]
|
||
if cv2.pointPolygonTest(mark_rois_tmp, (pt_2d[0][0],pt_2d[0][1]), False) >= 0:
|
||
one_mark_pcd.points.append((im_2d_to_3d[pt_2d[0][1], pt_2d[0][0]][0],
|
||
im_2d_to_3d[pt_2d[0][1], pt_2d[0][0]][1],
|
||
im_2d_to_3d[pt_2d[0][1], pt_2d[0][0]][2]))
|
||
# o3d.io.write_point_cloud("./detect_cloud/" + str(i) + ".ply", tmp_cloud)
|
||
o3d.io.write_point_cloud(conf_temp_cloud_path + str(i) + ".ply", one_mark_pcd)
|
||
if show:
|
||
o3d.visualization.draw_geometries([one_mark_pcd], mesh_show_wireframe=True)
|
||
marks_pcd.append(one_mark_pcd)
|
||
|
||
if show:
|
||
im_project = cv2.resize(im_project, (int(934), int(700)), interpolation=cv2.INTER_LINEAR)
|
||
cv2.imshow("im_project", im_project)
|
||
cv2.waitKey(0)
|
||
|
||
end_time = time.time()
|
||
execution_time = end_time - start_time
|
||
Log.info("3D 分割点云数据耗时：")
|
||
print(execution_time * 1000)
|
||
return marks_pcd
|
||
|
||
def filter_cloud(pcd):
|
||
# 获取点云中的点坐标
|
||
points = np.asarray(pcd.points)
|
||
|
||
# 定义 x, y, z 的范围
|
||
x_min, x_max = -1.8, 1.8
|
||
y_min, y_max = -1.5, 1.5
|
||
z_min, z_max = 0.0, global_z_max
|
||
|
||
# 创建布尔掩膜
|
||
mask = (
|
||
(points[:, 0] >= x_min) & (points[:, 0] <= x_max) &
|
||
(points[:, 1] >= y_min) & (points[:, 1] <= y_max) &
|
||
(points[:, 2] >= z_min) & (points[:, 2] <= z_max)
|
||
)
|
||
|
||
# 应用掩膜以过滤点云
|
||
filtered_points = points[mask]
|
||
|
||
# 更新点云对象
|
||
pcd.points = o3d.utility.Vector3dVector(filtered_points)
|
||
return pcd
|
||
|
||
def seg_mark_cloud_simple(ply_file_path, kk, show=False):
|
||
marks_pcd = []
|
||
|
||
src_cloud_load = o3d.io.read_point_cloud(ply_file_path)
|
||
if src_cloud_load is None or len(src_cloud_load.points) == 0 :
|
||
abort()
|
||
|
||
start_time = time.time()
|
||
|
||
# 点云旋转
|
||
Rx = src_cloud_load.get_rotation_matrix_from_xyz((roll, 0, 0))
|
||
Ry = src_cloud_load.get_rotation_matrix_from_xyz((0, pitch, 0))
|
||
Rz = src_cloud_load.get_rotation_matrix_from_xyz((0, 0, yaw))
|
||
# t_matrix = np.identity(4)
|
||
# t_matrix[:3, 3] = T_vector
|
||
src_cloud_load.rotate(Rx, (1,0,0))
|
||
src_cloud_load.rotate(Ry, (0,1,0))
|
||
src_cloud_load.rotate(Rz, (0,0,1))
|
||
src_cloud_load.translate(T_vector)
|
||
|
||
# 点云筛选
|
||
project_inlier_cloud = o3d.geometry.PointCloud()
|
||
if kk != 0.0:
|
||
for point_3d in np.asarray(src_cloud_load.points):
|
||
if -2.0 < point_3d[0] < 2.0 and -2.0 < point_3d[1] < 2.0 and point_3d[2] < global_z_max: # 筛选x,y,z坐标
|
||
_r = math.sqrt(pow(point_3d[0], 2) + pow(point_3d[1], 2) + pow(point_3d[2], 2))
|
||
if _r == 0: continue
|
||
if point_3d[0] > 0: point_3d[0] = point_3d[0] + point_3d[0] / _r * kk
|
||
else: point_3d[0] = point_3d[0] + point_3d[0] / _r * kk
|
||
if point_3d[1] > 0: point_3d[1] = point_3d[1] + point_3d[1] / _r * kk
|
||
else: point_3d[1] = point_3d[1] + point_3d[1] / _r * kk
|
||
point_3d[2] = point_3d[2] + point_3d[2] / _r * kk
|
||
# point_3d[2] -= 0.0
|
||
project_inlier_cloud.points.append((point_3d[0], point_3d[1], point_3d[2]))
|
||
else:
|
||
project_inlier_cloud = filter_cloud(src_cloud_load)
|
||
|
||
# 投影3D点云
|
||
im_project, im_2d_to_3d, pts_2d, list_pts_2d = project_cloud(project_inlier_cloud)
|
||
|
||
# 自动识别ROI角点
|
||
mark_rois = detect_marks_roi(im_project, show)
|
||
if len(mark_rois) != MARK_COUNT:
|
||
return marks_pcd
|
||
|
||
# 保存4个点云的数据到文件
|
||
# clear_folder("./detect_cloud")
|
||
# clear_folder(conf_temp_cloud_path)
|
||
# list_pts_2d = pts_2d.tolist()
|
||
# list_pts_2d = [[[int(item) for item in subsublist] for subsublist in sublist] for sublist in list_pts_2d]
|
||
Log.info("3D->2D project points")
|
||
for i in range(len(mark_rois)):
|
||
if len(mark_rois[i]) < 4: continue
|
||
one_mark_pcd = o3d.geometry.PointCloud()
|
||
mark_rois_tmp = np.asarray(mark_rois[i])
|
||
select_itmes = 0
|
||
for _, pt_2d in enumerate(list_pts_2d):
|
||
# x, y = pt_2d.ravel()
|
||
# x, y = int(x), int(y)
|
||
# x, y = pt_2d[0][0], pt_2d[0][1]
|
||
if cv2.pointPolygonTest(mark_rois_tmp, (pt_2d[0][0],pt_2d[0][1]), False) >= 0:
|
||
# one_mark_pcd.points.append((im_2d_to_3d[y, x][0], im_2d_to_3d[y, x][1], im_2d_to_3d[y, x][2]))
|
||
one_mark_pcd.points.append((im_2d_to_3d[pt_2d[0][1], pt_2d[0][0]][0],
|
||
im_2d_to_3d[pt_2d[0][1], pt_2d[0][0]][1],
|
||
im_2d_to_3d[pt_2d[0][1], pt_2d[0][0]][2]))
|
||
select_itmes += 1
|
||
print(select_itmes)
|
||
# o3d.io.write_point_cloud("./detect_cloud/" + str(i) + ".ply", tmp_cloud)
|
||
|
||
# 不写入文件，节省时间
|
||
# o3d.io.write_point_cloud(conf_temp_cloud_path + str(i) + ".ply", one_mark_pcd)
|
||
# if show:
|
||
# o3d.visualization.draw_geometries([one_mark_pcd], mesh_show_wireframe=True)
|
||
|
||
marks_pcd.append(one_mark_pcd)
|
||
|
||
if show:
|
||
im_project = cv2.resize(im_project, (int(934), int(700)), interpolation=cv2.INTER_LINEAR)
|
||
cv2.imshow("im_project", im_project)
|
||
cv2.waitKey(0)
|
||
|
||
end_time = time.time()
|
||
execution_time = end_time - start_time
|
||
Log.info("3D 分割点云数据耗时：")
|
||
print(execution_time * 1000)
|
||
return marks_pcd
|
||
|
||
# distance_threshold (float):
|
||
# 平面内点的最大距离阈值。这是指一个点到估计平面的距离，如果距离小于或等于这个阈值，则认为该点属于此平面。
|
||
# ransac_n (int):
|
||
# 每次迭代中用于估计模型的点数。对于平面分割，通常设置为 3，因为三个非共线点可以唯一确定一个平面。
|
||
# num_iterations (int):
|
||
# RANSAC 算法的迭代次数。更多的迭代次数会增加找到最优解的概率，但也增加了计算时间。
|
||
def correct_mark_cloud(mark_pcd):
|
||
plane_model, inliers = mark_pcd.segment_plane(distance_threshold=0.0155,
|
||
ransac_n=3,
|
||
num_iterations=10000)
|
||
inlier_pcd = mark_pcd.select_by_index(inliers)
|
||
inlier_pcd = reproject_cloud(plane_model, inlier_pcd) #利用投影得到的图像
|
||
inlier_pcd = inlier_pcd.paint_uniform_color([1.0, 0, 0])
|
||
outlier_cloud = mark_pcd.select_by_index(inliers, invert=True)
|
||
|
||
# 暂时没有用到
|
||
# radius = 0.05
|
||
# inlier_pcd = smooth_point_cloud(inlier_pcd, radius) #利用拟合得到的图像
|
||
|
||
inlier_pcd_correct = reproject_cloud(plane_model, mark_pcd) #利用投影得到的图像
|
||
inlier_pcd_correct = inlier_pcd_correct.paint_uniform_color([0.0, 1.0, 0])
|
||
# o3d.visualization.draw_geometries([ inlier_pcd, inlier_pcd_correct ])
|
||
# inlier_pcd_correct = inlier_pcd # ???
|
||
return plane_model, inlier_pcd, inlier_pcd_correct
|
||
|
||
|
||
# 3D标定板角点提取 select_conners_from_mark_cloud_2d
|
||
# 通过 select_conners_from_mark_cloud_3d 对 inlier_pcd_correct 自动选取 3D 角点 （3D角点自动提取算法）
|
||
# 将 inlier_pcd_correct 投影至 2D
|
||
# 生成 im_2d_to_3d_n (n:1-4)（采用相机内参）
|
||
# 生成 im_project_n (n:1-4) （采用相机内参）
|
||
# 在2D图中人工标记角点
|
||
# 通过2D点、H、[A,B,C,D] 得到3D角点 c_lidar[16]
|
||
# 【check】通过3D角点 c_lidar[16] 计算得出的标定板边长 l 和实际值的误差（绝对误差、4条边的均方差）
|
||
def cal_H(im_2d_to_3d, pts_2d, list_pts_2d):
|
||
pts_3d = []
|
||
# list_pts_2d = pts_2d.tolist()
|
||
# list_pts_2d = [[[int(item) for item in subsublist] for subsublist in sublist] for sublist in list_pts_2d]
|
||
for i, pt_2d in enumerate(list_pts_2d):
|
||
# x, y = pt_2d.ravel()
|
||
# x, y = int(x), int(y)
|
||
# x, y = pt_2d[0][0], pt_2d[0][1]
|
||
pts_3d.append([im_2d_to_3d[pt_2d[0][1], pt_2d[0][0]][0],
|
||
im_2d_to_3d[pt_2d[0][1], pt_2d[0][0]][1],
|
||
im_2d_to_3d[pt_2d[0][1], pt_2d[0][0]][2]])
|
||
ransacReprojThreshold = 3.0
|
||
# 查找单应矩阵
|
||
# 修改第三个参数 1.0 -> 5.0
|
||
# 利用基于RANSAC的鲁棒算法选择最优的四组配对点，再计算转换矩阵H(3*3)并返回,以便于反向投影错误率达到最小
|
||
# ransacReprojThreshold 设置为 1.0
|
||
# H, mask = cv2.findHomography(np.array(src_points), np.array(dst_points), cv2.RANSAC, ransacReprojThreshold, maxIters=1500, confidence=0.998) ## 修改 1.0 -> 5.0
|
||
pts_hom = []
|
||
for i in range(len(pts_2d)):
|
||
pt = pts_2d[i]
|
||
x,y = pt[0][0], pt[0][1]
|
||
pts_hom.append([x*1.0, y*1.0, 1.0])
|
||
H, _ = cv2.findHomography(np.array(pts_3d), np.array(pts_hom), cv2.RANSAC, ransacReprojThreshold) ## 修改 1.0 -> 5.0
|
||
Log.info("2D-3D 单应性矩阵计算结果为：")
|
||
print(H)
|
||
return H
|
||
|
||
def sort_conners_2d(conners):
|
||
sort_conner = []
|
||
left, top, right, bottom = 0, 0, 0, 0
|
||
# maxx,maxy = 0,0
|
||
maxx,maxy = -100000,-100000
|
||
minx,miny = 100000,100000
|
||
for i in range(len(conners)):
|
||
conner = conners[i]
|
||
if conner[0] < minx:
|
||
minx = conner[0]
|
||
left = i
|
||
if conner[0] > maxx:
|
||
maxx = conner[0]
|
||
right = i
|
||
if conner[1] < miny:
|
||
miny = conner[1]
|
||
top = i
|
||
if conner[1] > maxy:
|
||
maxy = conner[1]
|
||
bottom = i
|
||
sort_conner.append(conners[left])
|
||
sort_conner.append(conners[top])
|
||
sort_conner.append(conners[right])
|
||
sort_conner.append(conners[bottom])
|
||
return sort_conner
|
||
|
||
def remove_convex_hull_points(contour):
|
||
hull = cv2.convexHull(contour, returnPoints=True)
|
||
hull_indices = set(hull.flatten())
|
||
non_hull_points = [point for i, point in enumerate(contour) if i not in hull_indices]
|
||
return np.array(non_hull_points)
|
||
|
||
def smooth_convex_hull(contour, epsilon=0.1):
|
||
# 计算凸包
|
||
hull = cv2.convexHull(contour)
|
||
# 使用 approxPolyDP 减少顶点数量，从而达到平滑效果
|
||
epsilon_value = epsilon * cv2.arcLength(hull, True)
|
||
smoothed_hull = cv2.approxPolyDP(hull, epsilon_value, True)
|
||
|
||
return np.array(smoothed_hull)
|
||
|
||
from scipy.interpolate import splprep, splev
|
||
def spline_fit(points, s=0, k=2):
|
||
# 确保有足够的数据点
|
||
if len(points) <= k:
|
||
return None
|
||
|
||
tck, u = splprep(points.T, s=s, k=k)
|
||
u_new = np.linspace(u.min(), u.max(), 1000)
|
||
x_new, y_new = splev(u_new, tck)
|
||
return np.vstack((x_new, y_new)).T
|
||
|
||
def smooth_convex_hull_with_spline(contour, s=0):
|
||
hull = cv2.convexHull(contour)
|
||
points = hull.reshape(-1, 2)
|
||
smoothed_points = spline_fit(points, s=s)
|
||
if smoothed_points is None:
|
||
return None
|
||
return smoothed_points.astype(np.int32)
|
||
|
||
def find_largest_contour(contours):
|
||
if not contours:
|
||
return None
|
||
|
||
# 计算所有轮廓的面积
|
||
areas = [cv2.contourArea(cnt) for cnt in contours]
|
||
|
||
# 找到面积最大的轮廓索引
|
||
max_area_index = np.argmax(areas)
|
||
|
||
# 返回面积最大的轮廓
|
||
return contours[max_area_index]
|
||
|
||
from shapely.geometry import Polygon,MultiPolygon # 需要安装 shapely 库来处理多边形相交
|
||
def rect_to_polygon(rect):
|
||
box = cv2.boxPoints(rect)
|
||
return Polygon(box)
|
||
|
||
def polygon_intersection(poly1, poly2):
|
||
inter_poly = poly1.intersection(poly2)
|
||
if not inter_poly.is_empty:
|
||
return inter_poly
|
||
else:
|
||
return None
|
||
|
||
def fill_intersection_white(image, rect1, rect2):
|
||
"""
|
||
在给定图像中，用白色填充两个旋转矩形的相交区域。
|
||
|
||
参数:
|
||
image (numpy.ndarray): 输入图像
|
||
rect1 (tuple): 第一个旋转矩形，格式为 (center, size, angle)
|
||
rect2 (tuple): 第二个旋转矩形，格式为 (center, size, angle)
|
||
"""
|
||
# 将旋转矩形转换为多边形
|
||
poly1 = rect_to_polygon(rect1)
|
||
poly2 = rect_to_polygon(rect2)
|
||
|
||
# 计算相交区域
|
||
intersection = polygon_intersection(poly1, poly2)
|
||
|
||
if intersection is not None:
|
||
# 创建掩膜并填充白色
|
||
mask = np.zeros_like(image)
|
||
|
||
# 如果相交区域是多边形
|
||
if isinstance(intersection, Polygon):
|
||
inter_points = np.array(intersection.exterior.coords, dtype=np.int32)
|
||
cv2.fillPoly(mask, [inter_points], (255, 255, 255))
|
||
elif isinstance(intersection, MultiPolygon):
|
||
for geom in intersection.geoms:
|
||
inter_points = np.array(geom.exterior.coords, dtype=np.int32)
|
||
cv2.fillPoly(mask, [inter_points], (255, 255, 255))
|
||
|
||
# 应用掩膜到原图
|
||
image[mask == 255] = 255
|
||
|
||
|
||
def select_conners_from_mark_cloud_2d_old(mark_pcd, plane_model):
|
||
# 将 inlier_pcd_correct 投影至 2D
|
||
im_project, im_2d_to_3d, pts_2d, list_pts_2d = project_cloud(mark_pcd)
|
||
im_project_gray = cv2.cvtColor(im_project, cv2.COLOR_BGR2GRAY)
|
||
non_zero = cv2.findNonZero(im_project_gray)
|
||
|
||
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (20, 20))
|
||
im_project = cv2.morphologyEx(im_project, cv2.MORPH_DILATE, kernel)
|
||
|
||
for point in non_zero:
|
||
im_project[point[0][1], point[0][0]] = (255,255,255)
|
||
rect = cv2.minAreaRect(non_zero)
|
||
box = cv2.boxPoints(rect)
|
||
box = np.int0(box)
|
||
# for test
|
||
# cv2.drawContours(im_project, [box], 0, (255), 10)
|
||
# im_project = cv2.resize(im_project, (1600, 1200))
|
||
# cv2.imshow("_im", im_project)
|
||
# cv2.waitKey(0)
|
||
|
||
# 计算 mark cloud 点云的单应矩阵
|
||
H = cal_H(im_2d_to_3d, pts_2d, list_pts_2d)
|
||
|
||
# 获取高精确的角点 2D 位置
|
||
conners = sort_conners_2d(list(box))
|
||
|
||
# 通过单应矩阵计算角点 3D 位置
|
||
H_inv = np.linalg.inv(H)
|
||
conners_pcd = o3d.geometry.PointCloud()
|
||
for i in range(4):
|
||
conner = conners[i]
|
||
x, y = conner[0], conner[1]
|
||
pts_hom = np.array([x, y, 1])
|
||
pts_3d = np.dot(H_inv, pts_hom.T)
|
||
pts_3d = pts_3d/pts_3d[2]
|
||
conners_pcd.points.append((pts_3d[0],pts_3d[1],pts_3d[2]))
|
||
conners_pcd = reproject_cloud(plane_model, conners_pcd)
|
||
|
||
Log.info("2D标记角点，得到得边长为：")
|
||
for i in range(4):
|
||
print(np.linalg.norm(conners_pcd.points[i] - conners_pcd.points[(i+1)%4]))
|
||
return conners_pcd
|
||
|
||
def select_conners_from_mark_cloud_2d(mark_pcd, plane_model):
|
||
# 将 inlier_pcd_correct 投影至 2D
|
||
im_project, im_2d_to_3d, pts_2d, list_pts_2d = project_cloud(mark_pcd)
|
||
im_project_gray = cv2.cvtColor(im_project, cv2.COLOR_BGR2GRAY)
|
||
non_zero = cv2.findNonZero(im_project_gray)
|
||
|
||
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (20, 20))
|
||
im_project = cv2.morphologyEx(im_project, cv2.MORPH_DILATE, kernel)
|
||
|
||
for point in non_zero:
|
||
im_project[point[0][1], point[0][0]] = (255,255,255)
|
||
rect = cv2.minAreaRect(non_zero)
|
||
box = cv2.boxPoints(rect)
|
||
box = np.int0(box)
|
||
|
||
#================================================
|
||
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (40, 40))
|
||
im_project_gray = cv2.morphologyEx(im_project_gray, cv2.MORPH_DILATE, kernel)
|
||
kernel = cv2.getStructuringElement(cv2.MORPH_CROSS, (40, 40))
|
||
im_project_gray = cv2.morphologyEx(im_project_gray, cv2.MORPH_CLOSE, kernel)
|
||
|
||
# im_project_gray = cv2.resize(im_project_gray, (1600, 1200))
|
||
# cv2.imshow("im_project_gray", im_project_gray)
|
||
# cv2.waitKey(0)
|
||
_, binary_img = cv2.threshold(im_project_gray, 0, 255, cv2.THRESH_BINARY)
|
||
contours, _ = cv2.findContours(binary_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
||
# 对每个轮廓进行处理
|
||
# processed_contours = [remove_convex_hull_points(cnt) for cnt in contours]
|
||
processed_contours = [smooth_convex_hull(cnt) for cnt in contours]
|
||
contour = find_largest_contour(processed_contours)
|
||
rect2 = cv2.minAreaRect(contour)
|
||
box2 = cv2.boxPoints(rect2)
|
||
box2 = np.int0(box2)
|
||
# box = box2
|
||
|
||
temp_img = np.zeros((im_project_gray.shape[0], im_project_gray.shape[1], 1), dtype=np.uint8)
|
||
fill_intersection_white(temp_img, rect, rect2)
|
||
non_zero = cv2.findNonZero(temp_img)
|
||
rect3 = cv2.minAreaRect(non_zero)
|
||
box3 = cv2.boxPoints(rect3)
|
||
box3 = np.int0(box3)
|
||
|
||
box = box2
|
||
|
||
# processed_contours = []
|
||
# for cnt in contours:
|
||
# smoothed_hull = smooth_convex_hull_with_spline(cnt, s=0.0)
|
||
# if smoothed_hull is None: continue
|
||
# # cv2.polylines(drawing, [smoothed_hull], isClosed=True, color=(0, 255, 0), thickness=2)
|
||
# processed_contours.append(smoothed_hull)
|
||
# drawing = np.zeros_like(im_project)
|
||
# cv2.drawContours(drawing, processed_contours, -1, color=255, thickness=1)
|
||
# cv2.imshow("Processed Contours", drawing)
|
||
# cv2.waitKey(0)
|
||
#
|
||
# for test
|
||
# cv2.drawContours(im_project, [box2], 0, (255), 15)
|
||
# # cv2.drawContours(im_project, processed_contours, -1, color=(0,0,255), thickness=10)
|
||
# cv2.drawContours(im_project, [box3], 0, (0,255,0), 20)
|
||
# for point in non_zero:
|
||
# im_project[point[0][1], point[0][0]] = (255,255,255)
|
||
# im_project = cv2.resize(im_project, (1400, 1000))
|
||
# cv2.imshow("_im", im_project)
|
||
# cv2.waitKey(0)
|
||
#================================================
|
||
|
||
# 计算 mark cloud 点云的单应矩阵
|
||
H = cal_H(im_2d_to_3d, pts_2d, list_pts_2d)
|
||
|
||
# 获取高精确的角点 2D 位置
|
||
conners = sort_conners_2d(list(box))
|
||
|
||
Log.error("3d-2d 的边长")
|
||
print(np.linalg.norm(conners[0] - conners[1]))
|
||
print(np.linalg.norm(conners[1] - conners[2]))
|
||
print(np.linalg.norm(conners[2] - conners[3]))
|
||
print(np.linalg.norm(conners[3] - conners[0]))
|
||
|
||
# 通过单应矩阵计算角点 3D 位置
|
||
H_inv = np.linalg.inv(H)
|
||
conners_pcd = o3d.geometry.PointCloud()
|
||
for i in range(4):
|
||
conner = conners[i]
|
||
x, y = conner[0], conner[1]
|
||
pts_hom = np.array([x, y, 1])
|
||
pts_3d = np.dot(H_inv, pts_hom.T)
|
||
pts_3d = pts_3d/pts_3d[2]
|
||
conners_pcd.points.append((pts_3d[0],pts_3d[1],pts_3d[2]))
|
||
conners_pcd = reproject_cloud(plane_model, conners_pcd)
|
||
|
||
Log.info("2D标记角点，得到得边长为：")
|
||
for i in range(4):
|
||
print(np.linalg.norm(conners_pcd.points[i] - conners_pcd.points[(i+1)%4]))
|
||
return conners_pcd
|
||
|
||
def extend_line_3d(pt1, pt2, distance):
|
||
"""
|
||
计算 3D 空间两个点的延长线上的一个点
|
||
参数:
|
||
- point1: 第一个点的坐标 (x1, y1, z1)，类型为列表或数组
|
||
- point2: 第二个点的坐标 (x2, y2, z2)，类型为列表或数组
|
||
- distance: 从第二个点延长的距离 (正值表示延长方向，负值表示反向)
|
||
返回:
|
||
- 延长线上的点坐标 (x, y, z)，类型为数组
|
||
"""
|
||
# 转换为 NumPy 数组
|
||
point1 = np.array(pt1)
|
||
point2 = np.array(pt2)
|
||
# 计算方向向量
|
||
direction = point2 - point1
|
||
# 计算方向向量的单位向量
|
||
direction_unit = direction / np.linalg.norm(direction)
|
||
# 延长线上的点
|
||
extended_point = point2 + distance * direction_unit
|
||
return extended_point
|
||
|
||
def correct_mask_conner(inlier_pcd_correct, pcd_conners_2d):
|
||
oriented_bounding_box = inlier_pcd_correct.get_axis_aligned_bounding_box()
|
||
center_point = oriented_bounding_box.get_center()
|
||
# 0.3536
|
||
real_len = 0.3535534
|
||
pcd_correct_conners = o3d.geometry.PointCloud()
|
||
for i in range(4):
|
||
error_len = np.linalg.norm(pcd_conners_2d.points[i] - center_point) - real_len
|
||
real_conner = extend_line_3d(center_point, pcd_conners_2d.points[i], -error_len)
|
||
pcd_correct_conners.points.append(real_conner)
|
||
# print("中心点距离:" + str(np.linalg.norm(pcd_conners_2d.points[i] - center_point) ))
|
||
# print(real_conner)
|
||
return pcd_correct_conners, center_point
|
||
|
||
def fit_plane_least_squares(points):
|
||
# 计算中心点
|
||
centroid = np.mean(points, axis=0)
|
||
|
||
# 计算协方差矩阵
|
||
cov_matrix = (points - centroid).T.dot(points - centroid)
|
||
|
||
# 计算协方差矩阵的特征值和特征向量
|
||
eigenvalues, eigenvectors = np.linalg.eigh(cov_matrix)
|
||
|
||
# 最小特征值对应的特征向量就是平面的法向量
|
||
normal = eigenvectors[:, np.argmin(eigenvalues)]
|
||
return centroid, normal
|
||
|
||
|
||
def smooth_point_cloud(pcd, radius):
|
||
# Create a new PointCloud object
|
||
smoothed_pcd = o3d.geometry.PointCloud()
|
||
smoothed_points = []
|
||
|
||
# Build KDTree for the input point cloud
|
||
kdtree = o3d.geometry.KDTreeFlann(pcd)
|
||
|
||
# For each point, perform least squares plane fitting
|
||
for point in np.asarray(pcd.points):
|
||
# Find points within the radius
|
||
[k, idx, _] = kdtree.search_radius_vector_3d(point, radius)
|
||
if k < 3:
|
||
# If there are not enough points, skip
|
||
continue
|
||
|
||
# Extract neighboring points
|
||
neighbors = np.asarray(pcd.points)[idx, :]
|
||
|
||
# Use least squares to fit a plane
|
||
centroid, normal = fit_plane_least_squares(neighbors)
|
||
|
||
# Project the point onto the fitted plane
|
||
point_to_centroid = point - centroid
|
||
distance = point_to_centroid.dot(normal)
|
||
smoothed_point = point - distance * normal
|
||
smoothed_points.append(smoothed_point)
|
||
|
||
# Set the smoothed points
|
||
smoothed_pcd.points = o3d.utility.Vector3dVector(smoothed_points)
|
||
return smoothed_pcd
|
||
|
||
|
||
def reproject_cloud(plane_model, cloud_load):
|
||
reproject_pcd = o3d.geometry.PointCloud()
|
||
A,B,C,D = plane_model
|
||
for pts_3d in cloud_load.points:
|
||
# 求线与平面的交点
|
||
t = -D / (A * pts_3d[0] + B * pts_3d[1] + C * pts_3d[2])
|
||
# pts_3d = np.array([t * pts_3d[0], t * pts_3d[1], t * pts_3d[2]])
|
||
reproject_pcd.points.append((t * pts_3d[0], t * pts_3d[1], t * pts_3d[2]))
|
||
return reproject_pcd
|
||
|
||
|
||
def o2t(o_pcd):
|
||
t_pcd = o3d.t.geometry.PointCloud()
|
||
t_pcd.point.positions = o3d.core.Tensor(np.asarray(o_pcd.points))
|
||
t_pcd.point.colors = o3d.core.Tensor(np.asarray(o_pcd.colors))
|
||
o_pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
|
||
t_pcd.point.normals = o3d.core.Tensor(np.asarray(o_pcd.normals))
|
||
return t_pcd
|
||
|
||
|
||
def t2o(t_pcd):
|
||
o_pcd = o3d.geometry.PointCloud()
|
||
o_pcd.points = o3d.utility.Vector3dVector(t_pcd.point.positions.numpy())
|
||
# o_pcd.colors = o3d.utility.Vector3dVector(t_pcd.point.colors.numpy())
|
||
return o_pcd
|
||
|
||
|
||
# def calculate_curvature(pcd):
|
||
# # 估计法线
|
||
# pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
|
||
|
||
# # 计算曲率
|
||
# curvatures = []
|
||
# for i in range(len(pcd.points)):
|
||
# normal = np.array(pcd.normals[i])
|
||
# k_neighbors = pcd.select_by_index(pcd.nearest_k_neighbor(i, 10)[1]) # 获取最近的10个邻居
|
||
# neighbor_normals = np.array(k_neighbors.normals)
|
||
# curvature = np.mean(np.abs(neighbor_normals - normal)) # 简单地取平均绝对差作为曲率估计
|
||
# curvatures.append(curvature)
|
||
|
||
# return np.array(curvatures)
|
||
|
||
|
||
def calculate_curvature(pcd):
|
||
# 估计法线
|
||
pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
|
||
|
||
# 构建KDTree
|
||
pcd_tree = o3d.geometry.KDTreeFlann(pcd)
|
||
|
||
# 计算曲率
|
||
curvatures = []
|
||
for i in range(len(pcd.points)):
|
||
[k, idx, _] = pcd_tree.search_knn_vector_3d(pcd.points[i], 10) # 获取最近的10个邻居
|
||
neighbor_normals = np.array([pcd.normals[j] for j in idx])
|
||
normal = np.array(pcd.normals[i])
|
||
curvature = np.mean(np.abs(neighbor_normals - normal)) # 简单地取平均绝对差作为曲率估计
|
||
curvatures.append(curvature)
|
||
|
||
return np.array(curvatures)
|
||
|
||
|
||
def select_conners_from_mark_cloud_3d(inlier_cloud):
|
||
# 2. 计算凸包以获取边界点
|
||
hull, _ = inlier_cloud.compute_convex_hull()
|
||
hull_points = np.asarray(hull.vertices)
|
||
|
||
# 3. 将凸包顶点转换为点云对象以便后续处理
|
||
hull_pcd = o3d.geometry.PointCloud()
|
||
hull_pcd.points = o3d.utility.Vector3dVector(hull_points)
|
||
|
||
# 4. 简单地假设凸包是近似方形，计算四边形的四个角点
|
||
# 这里采用一种简化方法，即寻找距离最远的两个点对作为对角线端点
|
||
distances = np.linalg.norm(hull_points[:, None] - hull_points, axis=2)
|
||
max_distance_indices = np.unravel_index(np.argmax(distances), distances.shape)
|
||
diagonal_points = []
|
||
diagonal_points.append(hull_points[max_distance_indices[0]])
|
||
diagonal_points.append(hull_points[max_distance_indices[1]])
|
||
diagonal_points = np.asarray(diagonal_points)
|
||
# 找到剩余两个角点，它们应位于对角线两端点构成的直线上
|
||
remaining_points = np.delete(hull_points, max_distance_indices, axis=0)
|
||
idx = []
|
||
for i in range(len(remaining_points)):
|
||
pt = remaining_points[i]
|
||
if np.linalg.norm(pt - diagonal_points[0]) < 0.3 or np.linalg.norm(pt - diagonal_points[1]) < 0.3 :
|
||
idx.append(i)
|
||
remaining_points = np.delete(remaining_points, idx, axis=0)
|
||
distances = np.linalg.norm(remaining_points[:, None] - remaining_points, axis=2)
|
||
max_distance_indices = np.unravel_index(np.argmax(distances), distances.shape)
|
||
diagonal_points2 = []
|
||
diagonal_points2.append(remaining_points[max_distance_indices[0]])
|
||
diagonal_points2.append(remaining_points[max_distance_indices[1]])
|
||
diagonal_points2 = np.asarray(diagonal_points2)
|
||
|
||
corner_points = [diagonal_points[0], diagonal_points[1],
|
||
diagonal_points2[0], diagonal_points2[1]]
|
||
|
||
Log.info("3D标记角点，得到得边长为：")
|
||
print(np.linalg.norm(diagonal_points[0] - diagonal_points2[0]))
|
||
print(np.linalg.norm(diagonal_points[0] - diagonal_points2[1]))
|
||
print(np.linalg.norm(diagonal_points[1] - diagonal_points2[0]))
|
||
print(np.linalg.norm(diagonal_points[1] - diagonal_points2[1]))
|
||
|
||
# 5. 创建一个新的点云来表示角点，并可视化
|
||
corner_pcd = o3d.geometry.PointCloud()
|
||
corner_pcd.points = o3d.utility.Vector3dVector(corner_points)
|
||
|
||
corner_pcd.paint_uniform_color([1, 0, 0])
|
||
return corner_pcd
|
||
|
||
from sklearn.cluster import DBSCAN
|
||
import copy
|
||
import scipy
|
||
def select_conners_from_mark_cloud_3d_v2(inlier_cloud):
|
||
# 修正
|
||
downsampled_inlier_cloud = copy.deepcopy(inlier_cloud)
|
||
for i in range(len(downsampled_inlier_cloud.points)):
|
||
if i % 2 == 0:
|
||
downsampled_inlier_cloud.points[i][0] += 0.0001
|
||
# 估计法线
|
||
downsampled_inlier_cloud.estimate_normals()
|
||
# 寻找边界点
|
||
cl, ind = downsampled_inlier_cloud.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)
|
||
boundary_points = downsampled_inlier_cloud.select_by_index(ind, invert=True)
|
||
# 将边界点转换为numpy数组
|
||
points = np.asarray(boundary_points.points)
|
||
# 假设我们正在寻找的是矩形，我们可以尝试用凸包算法来找到最小包围四边形
|
||
hull = scipy.spatial.ConvexHull(points)
|
||
# 凸包顶点索引
|
||
hull_vertices = hull.vertices
|
||
# 如果凸包有4个顶点，那么这些可能是我们要找的角点
|
||
if len(hull_vertices) == 4:
|
||
corner_points = points[hull_vertices]
|
||
else:
|
||
# 如果不是4个顶点，则需要进一步处理
|
||
# 这里简化处理，直接返回凸包顶点作为角点
|
||
corner_points = points[hull_vertices]
|
||
|
||
|
||
# for i in range(len(downsampled_inlier_cloud.points)):
|
||
# if i % 2 == 0:
|
||
# downsampled_inlier_cloud.points[i][0] += 0.0001
|
||
# obb = downsampled_inlier_cloud.get_oriented_bounding_box()
|
||
|
||
# 提取角点
|
||
# corner_points = np.asarray(obb.get_box_points())
|
||
|
||
# 创建新的点云对象表示角点
|
||
corner_pcd = o3d.geometry.PointCloud()
|
||
corner_pcd.points = o3d.utility.Vector3dVector(corner_points)
|
||
|
||
# 可视化结果
|
||
o3d.visualization.draw_geometries([inlier_cloud, corner_pcd])
|
||
|
||
# corner_points = [diagonal_points[0], diagonal_points[1],
|
||
# diagonal_points2[0], diagonal_points2[1]]
|
||
|
||
# Log.info("3D标记角点，得到得边长为：")
|
||
# print(np.linalg.norm(diagonal_points[0] - diagonal_points2[0]))
|
||
# print(np.linalg.norm(diagonal_points[0] - diagonal_points2[1]))
|
||
# print(np.linalg.norm(diagonal_points[1] - diagonal_points2[0]))
|
||
# print(np.linalg.norm(diagonal_points[1] - diagonal_points2[1]))
|
||
|
||
# 5. 创建一个新的点云来表示角点，并可视化
|
||
# corner_pcd = o3d.geometry.PointCloud()
|
||
# corner_pcd.points = o3d.utility.Vector3dVector(corner_points)
|
||
|
||
# corner_pcd.paint_uniform_color([1, 0, 0])
|
||
return corner_pcd
|
||
|
||
|
||
def create_spheres(points, sizes=None, color=[0, 0.706, 1]):
|
||
"""
|
||
创建一系列球体以替代点云中的点，允许每个点有不同的大小。
|
||
|
||
参数:
|
||
points: N x 3 数组，表示N个点的位置。
|
||
sizes: N维数组或列表，表示对应点的半径大小。如果为None，则所有球体大小相同。
|
||
color: RGB颜色值，默认是橙色。
|
||
|
||
返回:
|
||
spheres: 包含所有球体的Open3D几何对象列表。
|
||
"""
|
||
spheres = []
|
||
if sizes is None:
|
||
sizes = [1] * len(points) # 如果没有提供大小，则默认所有球体大小相同
|
||
for i, point in enumerate(points):
|
||
sphere = o3d.geometry.TriangleMesh.create_sphere(radius=sizes[i])
|
||
sphere.translate(point) # 将球体移动到点的位置
|
||
sphere.paint_uniform_color(color)
|
||
spheres.append(sphere)
|
||
return spheres
|
||
|
||
|
||
def cal_marks_3d_conners(show=False):
|
||
marks_3d_conners = []
|
||
for i in range(4):
|
||
# mark_ply_file_path = './detect_cloud/' + str(i) + '.ply'
|
||
mark_ply_file_path = conf_temp_cloud_path + str(i) + '.ply'
|
||
|
||
# 计算标定板尺寸
|
||
# mark_ply_file_path = './detect_cloud/1.ply'
|
||
mark_pcd = o3d.io.read_point_cloud(mark_ply_file_path)
|
||
plane_model, inlier_cloud, inlier_pcd_correct = correct_mark_cloud(mark_pcd)
|
||
|
||
# t_pcd = o2t(inlier_cloud)
|
||
# _dir = dir(t_pcd)
|
||
# for it in _dir:
|
||
# print(it)
|
||
# t_boundaries, mask = t_pcd.compute_boundary_points(0.05, 60)
|
||
# boundaries = t2o(t_boundaries)
|
||
|
||
# # 计算曲率
|
||
# curvatures = calculate_curvature(inlier_cloud)
|
||
# threshold = np.percentile(curvatures, 95) # 使用95百分位数作为阈值
|
||
# corner_indices = np.where(curvatures > threshold)[0]
|
||
# t_corner_pcd = inlier_cloud.select_by_index(corner_indices.tolist())
|
||
# t_corner_pcd.paint_uniform_color([0, 0, 1]) # 将角点涂成红色
|
||
|
||
used_pcd = inlier_cloud # inlier_cloud / inlier_pcd_correct
|
||
pcd_conners_2d = select_conners_from_mark_cloud_2d(used_pcd, plane_model)
|
||
pcd_conners_2d_correct, center_point = correct_mask_conner(used_pcd, pcd_conners_2d)
|
||
Log.info("修正后2D标记角点，得到得边长为：")
|
||
for i in range(4):
|
||
print(np.linalg.norm(pcd_conners_2d_correct.points[i] - pcd_conners_2d_correct.points[(i+1)%4]))
|
||
# pcd_conners_3d = select_conners_from_mark_cloud_3d(inlier_pcd_correct)
|
||
marks_3d_conners.append(pcd_conners_2d)
|
||
|
||
if show:
|
||
# 角点显示的圆球半径，单位（米）
|
||
r = 0.006
|
||
sizes = [r,r,r,r]
|
||
spheres_2d = create_spheres(pcd_conners_2d.points, sizes, color=[0, 0.706, 1])
|
||
spheres_3d = create_spheres(pcd_conners_2d_correct.points, sizes, color=[0, 0, 0])
|
||
spheres_center_3d = create_spheres([center_point], [0.01], color=[0, 1, 1])
|
||
o3d.visualization.draw_geometries([
|
||
spheres_2d[0],spheres_2d[1],spheres_2d[2],spheres_2d[3],
|
||
spheres_3d[0],spheres_3d[1],spheres_3d[2],spheres_3d[3],
|
||
spheres_center_3d[0],
|
||
mark_pcd.paint_uniform_color([1, 0, 0]),
|
||
inlier_pcd_correct.paint_uniform_color([0, 1, 0]),])
|
||
|
||
# mark 顺序按照 左上，右上，左下，右下
|
||
# 每个mark的角点按照左、上、右、底顺序返回
|
||
return marks_3d_conners
|
||
|
||
def cal_marks_3d_conners_simple(marks_pcd_4, show=False):
|
||
marks_3d_conners = []
|
||
for i in range(4):
|
||
# mark_ply_file_path = './detect_cloud/' + str(i) + '.ply'
|
||
mark_ply_file_path = conf_temp_cloud_path + str(i) + '.ply'
|
||
|
||
# 计算标定板尺寸
|
||
# mark_ply_file_path = './detect_cloud/1.ply'
|
||
# 不从文件读取
|
||
# mark_pcd = o3d.io.read_point_cloud(mark_ply_file_path)
|
||
mark_pcd = marks_pcd_4[i]
|
||
plane_model, inlier_cloud, inlier_pcd_correct = correct_mark_cloud(mark_pcd)
|
||
|
||
# t_pcd = o2t(inlier_cloud)
|
||
# _dir = dir(t_pcd)
|
||
# for it in _dir:
|
||
# print(it)
|
||
# t_boundaries, mask = t_pcd.compute_boundary_points(0.05, 60)
|
||
# boundaries = t2o(t_boundaries)
|
||
|
||
# # 计算曲率
|
||
# curvatures = calculate_curvature(inlier_cloud)
|
||
# threshold = np.percentile(curvatures, 95) # 使用95百分位数作为阈值
|
||
# corner_indices = np.where(curvatures > threshold)[0]
|
||
# t_corner_pcd = inlier_cloud.select_by_index(corner_indices.tolist())
|
||
# t_corner_pcd.paint_uniform_color([0, 0, 1]) # 将角点涂成红色
|
||
|
||
used_pcd = inlier_cloud # inlier_cloud / inlier_pcd_correct
|
||
# inlier_cloud -> box
|
||
# inlier_pcd_correct -> box2\box3
|
||
pcd_conners_2d = select_conners_from_mark_cloud_2d(used_pcd, plane_model)
|
||
pcd_conners_2d_correct, center_point = correct_mask_conner(used_pcd, pcd_conners_2d)
|
||
Log.info("修正后2D标记角点，得到得边长为：")
|
||
for i in range(4):
|
||
print(np.linalg.norm(pcd_conners_2d_correct.points[i] - pcd_conners_2d_correct.points[(i+1)%4]))
|
||
# pcd_conners_3d = select_conners_from_mark_cloud_3d(inlier_pcd_correct)
|
||
marks_3d_conners.append(pcd_conners_2d_correct) # pcd_conners_2d / pcd_conners_2d_correct
|
||
|
||
|
||
# print(marks_3d_conners[0].points)
|
||
# print(marks_3d_conners[0].points[0])
|
||
# print(np.linalg.norm(marks_3d_conners[0].points[0] - marks_3d_conners[0].points[1]))
|
||
# print(np.linalg.norm(marks_3d_conners[0].points[1] - marks_3d_conners[0].points[2]))
|
||
# print(np.linalg.norm(marks_3d_conners[0].points[2] - marks_3d_conners[0].points[3]))
|
||
# print(np.linalg.norm(marks_3d_conners[0].points[3] - marks_3d_conners[0].points[0]))
|
||
# Log.info("3D->3D 通过相邻角点计算得到的边长进行对比")
|
||
if show:
|
||
# 角点显示的圆球半径，单位（米）
|
||
r = 0.006
|
||
sizes = [r,r,r,r]
|
||
spheres_2d = create_spheres(pcd_conners_2d.points, sizes, color=[0, 0.706, 1])
|
||
spheres_3d = create_spheres(pcd_conners_2d_correct.points, sizes, color=[0, 0, 0])
|
||
spheres_center_3d = create_spheres([center_point], [0.01], color=[0, 1, 1])
|
||
o3d.visualization.draw_geometries([
|
||
spheres_2d[0],spheres_2d[1],spheres_2d[2],spheres_2d[3],
|
||
spheres_3d[0],spheres_3d[1],spheres_3d[2],spheres_3d[3],
|
||
spheres_center_3d[0],
|
||
mark_pcd.paint_uniform_color([1, 0, 0]),
|
||
inlier_pcd_correct.paint_uniform_color([0, 1, 0]),])
|
||
|
||
# mark 顺序按照 左上，右上，左下，右下
|
||
# 每个mark的角点按照左、上、右、底顺序返回
|
||
return marks_3d_conners
|
||
|
||
def print_diff(aurco_pt1, aurco_pt2, lidar_pt1, lidar_pt2):
|
||
aurco_distance = np.linalg.norm(aurco_pt1 - aurco_pt2)
|
||
lidar_distance = np.linalg.norm(lidar_pt1 - lidar_pt2)
|
||
print(aurco_distance)
|
||
print(lidar_distance)
|
||
print(abs(aurco_distance - lidar_distance))
|
||
|
||
|
||
# 测试2d角点分割算法
|
||
def _test_select_conners_from_mark_cloud_2d_2():
|
||
pass
|
||
|
||
|
||
if __name__ == '__main__':
|
||
marks_3d_conners = []
|
||
ply_file_path = os.path.dirname(__file__) + "\\samples\\calibration\\11\\output.ply"
|
||
marks_pcd = seg_mark_cloud_simple(ply_file_path, kk)
|
||
for i in range(4):
|
||
mark_ply_file_path = conf_temp_cloud_path + str(i) + '.ply'
|
||
mark_pcd = o3d.io.read_point_cloud(mark_ply_file_path)
|
||
plane_model, inlier_cloud, inlier_pcd_correct = correct_mark_cloud(mark_pcd)
|
||
# pcd_conners_2d = select_conners_from_mark_cloud_2d(mark_pcd, plane_model)
|
||
pcd_conners_2d = select_conners_from_mark_cloud_3d_v2(inlier_pcd_correct)
|
||
|
||
|
||
# image_project = cv2.imread("../caowei/new/output.png")
|
||
# # if image_project is None:
|
||
# # print("image_project is None!")
|
||
# # exit()
|
||
|
||
# # # 分割标定板
|
||
# ply_file_path = '../caowei/new/20241227175529496.ply'
|
||
|
||
# marks_pcd = seg_mark_cloud(ply_file_path, kk)
|
||
# if len(marks_pcd) != MARK_COUNT:
|
||
# exit()
|
||
|
||
# LEFT = 0
|
||
# TOP = 1
|
||
# RIGHT = 2
|
||
# BOTTOM = 3
|
||
|
||
# frame, marks_aurco = aurco_3d_detect.cal_marks_3d_conners("../caowei/new/output.png")
|
||
|
||
# marks_pcd = cal_marks_3d_conners()
|
||
# marks_pcd_new = []
|
||
# # marks_pcd_new.append(marks_pcd[2].points)
|
||
# # marks_pcd_new.append(marks_pcd[3].points)
|
||
# # marks_pcd_new.append(marks_pcd[1].points)
|
||
# # marks_pcd_new.append(marks_pcd[0].points)
|
||
|
||
# tmmp = []
|
||
# for i in range(4):
|
||
# tmmp.append(marks_pcd[2].points[i])
|
||
# marks_pcd_new.append(tmmp)
|
||
# tmmp = []
|
||
# for i in range(4):
|
||
# tmmp.append(marks_pcd[3].points[i])
|
||
# marks_pcd_new.append(tmmp)
|
||
# tmmp = []
|
||
# for i in range(4):
|
||
# tmmp.append(marks_pcd[1].points[i])
|
||
# marks_pcd_new.append(tmmp)
|
||
# tmmp = []
|
||
# for i in range(4):
|
||
# tmmp.append(marks_pcd[0].points[i])
|
||
# marks_pcd_new.append(tmmp)
|
||
|
||
# print("======== 0 left - 1 right =========")
|
||
# print_diff(marks_aurco[0][LEFT] , marks_aurco[1][RIGHT], marks_pcd_new[0][LEFT], marks_pcd_new[1][RIGHT])
|
||
# print("======== 0 left - 2 right =========")
|
||
# print_diff(marks_aurco[0][LEFT] , marks_aurco[2][RIGHT], marks_pcd_new[0][LEFT], marks_pcd_new[2][RIGHT])
|
||
# print("======== 0 left - 3 right =========")
|
||
# print_diff(marks_aurco[0][LEFT] , marks_aurco[3][RIGHT], marks_pcd_new[0][LEFT], marks_pcd_new[3][RIGHT])
|
||
# print("======== 0 right - 1 left =========")
|
||
# print_diff(marks_aurco[0][RIGHT] , marks_aurco[1][LEFT], marks_pcd_new[0][RIGHT] , marks_pcd_new[1][LEFT])
|
||
# print("======== 0 top - 3 bottom =========")
|
||
# print_diff(marks_aurco[0][TOP] , marks_aurco[3][BOTTOM], marks_pcd_new[0][TOP] , marks_pcd_new[3][BOTTOM])
|
||
# print("======== 2 left - 1 right =========")
|
||
# print_diff(marks_aurco[2][LEFT] , marks_aurco[1][RIGHT], marks_pcd_new[2][LEFT] , marks_pcd_new[1][RIGHT])
|
||
# print("======== 2 left - 3 right =========")
|
||
# print_diff(marks_aurco[2][LEFT] , marks_aurco[3][RIGHT], marks_pcd_new[2][LEFT] , marks_pcd_new[3][RIGHT])
|
||
|
||
# exit() |