419 lines
17 KiB
Python
419 lines
17 KiB
Python
"""
|
||
本模块包含了坐标系转换相关的功能类和函数，主要实现以下功能：
|
||
1. 像素坐标系到机械臂基座坐标系的转换
|
||
2. 相机坐标系到机械臂末端坐标系的转换
|
||
3. 机械臂末端坐标系到机械臂基座坐标系的转换
|
||
4. 坐标系转换过程中的深度处理和验证
|
||
"""
|
||
|
||
import copy
|
||
import cv2
|
||
import numpy as np
|
||
import math
|
||
import yaml
|
||
from pathlib import Path
|
||
from scipy.spatial.transform import Rotation as R
|
||
from scipy.interpolate import CubicSpline
|
||
|
||
from tools.log import CustomLogger
|
||
|
||
try:
|
||
from .vector import SimpleNormalVector
|
||
except:
|
||
from vector import SimpleNormalVector
|
||
|
||
class CoordinateTransformer:
|
||
def __init__(self, intrinsics, color_image, depth_image, position, quaternion, vtxdb=None):
|
||
"""
|
||
初始化坐标转换器
|
||
|
||
Args:
|
||
intrinsics: 相机内参
|
||
color_image: 彩色图像
|
||
depth_image: 深度图像
|
||
position: 机械臂位置
|
||
quaternion: 机械臂姿态四元数
|
||
"""
|
||
# 基础参数初始化
|
||
self.logger = CustomLogger(propagate=True)
|
||
self.intrinsics = intrinsics
|
||
self.color_image = color_image
|
||
self.depth_image = depth_image
|
||
self.color_visualize = None
|
||
self.position = position
|
||
self.quaternion = quaternion
|
||
self.insert_queue = False
|
||
|
||
# 坐标范围初始化
|
||
self.x_range = (-0.3, 0.5)
|
||
self.y_range = (-0.5, 1.7)
|
||
self.z_range = (0.65, 1.0)
|
||
|
||
# 相机坐标系手动偏置初始化
|
||
self.camera_offset_x = 0 # 毫米
|
||
self.camera_offset_y = 0 # 毫米
|
||
self.camera_offset_z = 0 # 毫米
|
||
|
||
self.vtxdb = vtxdb
|
||
|
||
# 初始化相机参数
|
||
self._init_camera_params()
|
||
|
||
# 初始化PoseVector
|
||
self._init_pose_vector()
|
||
self.camera_matrix = None
|
||
self.camera_trans = None
|
||
|
||
self.camera_matrix = np.array(self.vtxdb.get("robot_config","camera.camera_matrix"))
|
||
self.camera_trans = np.array(self.vtxdb.get("robot_config","camera.camera_trans"))
|
||
|
||
camera_offset = self.vtxdb.get("robot_config","camera.camera_offset")
|
||
if camera_offset:
|
||
self.set_camera_offset(camera_offset[0],camera_offset[1],camera_offset[2])
|
||
|
||
def _init_camera_params(self):
|
||
"""初始化相机参数，从配置文件读取"""
|
||
file_path = Path('/home/jsfb/jsfb_ws/global_config/camera_config.yaml')
|
||
|
||
# 设置默认参数
|
||
default_params = {
|
||
'bias_x': 20,
|
||
'bias_y': 20,
|
||
'camera_Upper_limit': 55,
|
||
'camera_Lower_limit': 404
|
||
}
|
||
|
||
if not file_path.exists():
|
||
self._set_camera_params(default_params)
|
||
self.logger.log_warning("相机配置文件不存在，使用默认参数")
|
||
return
|
||
|
||
try:
|
||
with open(file_path, 'r') as file:
|
||
camera_config = yaml.safe_load(file)
|
||
if camera_config.get('camera') == 'new':
|
||
params = {
|
||
'bias_x': 20,
|
||
'bias_y': 0,
|
||
'camera_Upper_limit': 30,
|
||
'camera_Lower_limit': 375
|
||
}
|
||
self._set_camera_params(params)
|
||
self.logger.log_info("使用新相机参数配置")
|
||
elif camera_config.get('camera') == 'old':
|
||
self._set_camera_params(default_params)
|
||
self.logger.log_info("使用旧相机参数配置")
|
||
else:
|
||
self._set_camera_params(default_params)
|
||
self.logger.log_warning("未知相机类型，使用默认参数")
|
||
except yaml.YAMLError as e:
|
||
self._set_camera_params(default_params)
|
||
self.logger.log_error(f"读取相机配置文件出错: {e}")
|
||
|
||
def _set_camera_params(self, params):
|
||
"""设置相机参数"""
|
||
self.bias_x = params['bias_x']
|
||
self.bias_y = params['bias_y']
|
||
self.camera_Upper_limit = params['camera_Upper_limit']
|
||
self.camera_Lower_limit = params['camera_Lower_limit']
|
||
|
||
self.logger.log_info(f"相机参数设置完成: bias_x={self.bias_x}, bias_y={self.bias_y}")
|
||
self.logger.log_info(f"相机限制设置完成: Upper={self.camera_Upper_limit}, Lower={self.camera_Lower_limit}")
|
||
|
||
def _init_pose_vector(self):
|
||
# """初始化PoseVector"""
|
||
# # 保存深度图像为npy文件
|
||
# output_npy_path = 'aucpuncture2point/configs/using_img/result_depth.npy'
|
||
# np.save(output_npy_path, self.depth_image)
|
||
|
||
# self.vector = PoseVector(
|
||
# output_npy_path,
|
||
# self.intrinsics,
|
||
# self.x_range,
|
||
# self.y_range,
|
||
# self.z_range,
|
||
# 'aucpuncture2point/configs/using_img/color.png',
|
||
# is_human_mask=False
|
||
# )
|
||
self.vector = SimpleNormalVector('aucpuncture2point/configs/using_img/depth.png', 'aucpuncture2point/configs/using_img/color.png', self.intrinsics)
|
||
|
||
def pixel_to_robotarm(self, target_pixel_points_list, number):
|
||
"""
|
||
将像素坐标转换为机械臂基座坐标系坐标
|
||
|
||
Args:
|
||
target_pixel_points_list: 目标像素点列表
|
||
number: 图像序号，用于保存可视化结果
|
||
|
||
Returns:
|
||
final_to_arm_point: 转换后的机械臂坐标点列表，如果验证失败返回None
|
||
"""
|
||
# 验证输入点的有效性
|
||
if not self._validate_points(target_pixel_points_list):
|
||
print("target pixel is not valid")
|
||
return None
|
||
|
||
# 处理每个像素点
|
||
points, points_fxl, processed_image = self._process_pixel_points(target_pixel_points_list)
|
||
if self.insert_queue == True:
|
||
return None
|
||
print(f"-------------{number}")
|
||
# 保存可视化结果
|
||
self._save_visualization(processed_image, number)
|
||
|
||
# 计算最终坐标
|
||
return self._calculate_final_coordinates(points, points_fxl)
|
||
|
||
def _validate_points(self, points):
|
||
"""验证输入点的有效性"""
|
||
if points[0][1] <= self.camera_Upper_limit:
|
||
self.logger.log_error(f"传入第一个点在y像素{self.camera_Upper_limit}以下，没有深度，提示用户躺下一点")
|
||
return False
|
||
|
||
# 限制y坐标范围
|
||
for point in points:
|
||
if point[1] > self.camera_Lower_limit:
|
||
point[1] = self.camera_Lower_limit
|
||
|
||
return True
|
||
|
||
# def _process_pixel_points(self, target_pixel_points_list):
|
||
# """处理像素点列表"""
|
||
# color_image = copy.deepcopy(self.color_image)
|
||
# depth_image = copy.deepcopy(self.depth_image)
|
||
# points = []
|
||
# points_fxl = []
|
||
# last_depth = 800
|
||
|
||
# for coordinate in target_pixel_points_list:
|
||
# x, y = int(round(coordinate[0])), int(round(coordinate[1]))
|
||
# depth = self._get_depth(x, y, depth_image, last_depth)
|
||
|
||
# # 在图像上绘制处理点
|
||
# cv2.circle(color_image, (x*2, y*2), 3, (51,102,255), 4)
|
||
|
||
# # 坐标转换
|
||
# camera_coordinate = self.deproject_pixel_to_point(self.intrinsics, (x, y), depth)
|
||
# arm_coordinate = self.calculate_arm_coords(camera_coordinate)
|
||
|
||
# points.append({"coordinates": arm_coordinate, "label": ''})
|
||
# points_fxl.append({"coordinates": camera_coordinate/1000.0, "label": ''})
|
||
|
||
# return points, points_fxl, color_image
|
||
def smooth_curve_with_spline(self,points):
|
||
"""使用三次样条插值平滑曲线."""
|
||
points = np.array(points)
|
||
x = points[:, 0]
|
||
y = points[:, 1]
|
||
|
||
t = np.linspace(0, 1, len(points))
|
||
cs_x = CubicSpline(t, x)
|
||
cs_y = CubicSpline(t, y)
|
||
|
||
t_dense = np.linspace(0, 1, 300)
|
||
x_smooth = cs_x(t_dense)
|
||
y_smooth = cs_y(t_dense)
|
||
|
||
smooth_points = np.array([x_smooth, y_smooth]).T.astype(np.int32)
|
||
return smooth_points
|
||
|
||
def _process_pixel_points(self, target_pixel_points_list):
|
||
"""处理像素点列表"""
|
||
color_image = copy.deepcopy(self.color_image)
|
||
depth_image = copy.deepcopy(self.depth_image)
|
||
if self.color_visualize is not None:
|
||
color_visualize = copy.deepcopy(self.color_visualize)
|
||
else:
|
||
color_visualize = color_image
|
||
points = []
|
||
points_fxl = []
|
||
last_depth = 800
|
||
|
||
# 获取原始图像和可视化图像的尺寸
|
||
h_orig, w_orig = color_image.shape[:2]
|
||
h_visual, w_visual = color_visualize.shape[:2] # 假设 color_visualize 是类属性
|
||
|
||
# 计算缩放比例
|
||
scale_x = w_visual / w_orig
|
||
scale_y = h_visual / h_orig
|
||
draw_points = []
|
||
for coordinate in target_pixel_points_list:
|
||
if self.insert_queue == True:
|
||
return None
|
||
x, y = int(round(coordinate[0])), int(round(coordinate[1]))
|
||
depth = self._get_depth(x, y, depth_image, last_depth)
|
||
|
||
# 在 color_visualize 上绘制点（调整缩放倍数）
|
||
x_scaled = int(round(x * scale_x))
|
||
y_scaled = int(round(y * scale_y))
|
||
if len(target_pixel_points_list) < 2: #只有一个点 直接绘制
|
||
cv2.circle(color_visualize, (x_scaled, y_scaled), 3, (47,255,173), 4) #BGR 173 255 47
|
||
else:
|
||
draw_points.append([x_scaled,y_scaled])
|
||
# print(f"draw points----{(x_scaled,y_scaled)}")
|
||
|
||
# 坐标转换
|
||
camera_coordinate = self.deproject_pixel_to_point(self.intrinsics, (x, y), depth)
|
||
|
||
offset_camera_coordinate = copy.deepcopy(camera_coordinate)
|
||
|
||
# 应用相机坐标系手动偏置
|
||
offset_camera_coordinate[0] += self.camera_offset_x # X轴偏置
|
||
offset_camera_coordinate[1] += self.camera_offset_y # Y轴偏置
|
||
offset_camera_coordinate[2] += self.camera_offset_z # Z轴偏置
|
||
|
||
arm_coordinate = self.calculate_arm_coords(offset_camera_coordinate)
|
||
|
||
points.append({"coordinates": arm_coordinate, "label": ''})
|
||
points_fxl.append({"coordinates": camera_coordinate/1000.0, "label": ''})
|
||
|
||
# 绘制样条插值曲线的图像 (保持不变)
|
||
if len(draw_points) > 0:
|
||
# image_curve_spline = np.zeros((h_visual, h_visual, 3), dtype=np.uint8)
|
||
smooth_points_spline = self.smooth_curve_with_spline(draw_points)
|
||
points_array_spline = smooth_points_spline.reshape((-1, 1, 2))
|
||
cv2.polylines(color_visualize, [points_array_spline], isClosed=False, color=(47, 255, 173), thickness=2, lineType=cv2.LINE_AA)
|
||
|
||
return points, points_fxl, color_visualize # 返回可视化后的图像
|
||
|
||
def _save_visualization(self, image, number):
|
||
"""保存可视化结果"""
|
||
# cv2.imwrite('aucpuncture2point/configs/using_img/Pixel_Visualization.jpg', image)
|
||
cv2.imwrite(f"aucpuncture2point/configs/using_img/Pixel_Visualization_{number}.jpg", image)
|
||
|
||
def _calculate_final_coordinates(self, points, points_fxl):
|
||
"""计算最终坐标"""
|
||
# 计算法向量
|
||
normals_data = self.vector.run(points_fxl, visualize=True)
|
||
|
||
# 合并结果
|
||
combined_results = []
|
||
for i, data in enumerate(normals_data):
|
||
normal_arm = self.calculate_arm_normal(data)
|
||
normals_position = points[i]["coordinates"] + normal_arm["transformed_normal"]
|
||
combined_results.append({
|
||
"label": points[i]["label"],
|
||
"normals_position": normals_position
|
||
})
|
||
|
||
# 计算最终的机械臂坐标
|
||
point = self.calculate_target_Euler(combined_results)
|
||
return [item['position'] for item in point]
|
||
|
||
def _get_depth(self, x, y, depth_image, last_depth):
|
||
"""获取深度值，处理无效深度的情况"""
|
||
if depth_image[y, x] != 0:
|
||
return depth_image[y, x]
|
||
|
||
non_zero_depths = depth_image[max(0, y-5):y+5, max(0, x-5):x+5]
|
||
non_zero_depths = non_zero_depths[non_zero_depths != 0]
|
||
return np.median(non_zero_depths) if non_zero_depths.size > 0 else last_depth
|
||
|
||
def deproject_pixel_to_point(self, intrinsics, pixel, depth):
|
||
"""将像素坐标和深度值转换为相机坐标系下的3D坐标"""
|
||
x, y = pixel
|
||
fx, fy = intrinsics['fx'], intrinsics['fy']
|
||
cx, cy = intrinsics['cx'], intrinsics['cy']
|
||
|
||
X = (x - cx) * depth / fx
|
||
Y = (y - cy) * depth / fy
|
||
Z = depth
|
||
|
||
return np.array([X, Y, Z])
|
||
|
||
# def calculate_arm_coords(self, camera_coordinate):
|
||
# """将相机坐标系转换为机械臂坐标系"""
|
||
# # 相机坐标系到机械臂末端坐标系
|
||
# arm_coordinate = [
|
||
# (camera_coordinate[1] - self.bias_y) / 1000, # x
|
||
# (camera_coordinate[0] - self.bias_x) / 1000, # y
|
||
# (-camera_coordinate[2] - 172) / 1000 # z
|
||
# ]
|
||
|
||
# # 机械臂末端坐标系到基座坐标系
|
||
# return (R.from_quat(self.quaternion).as_matrix() @ arm_coordinate + self.position).tolist()
|
||
|
||
def calculate_arm_coords(self, camera_coordinate):
|
||
"""
|
||
将相机坐标系转换为机械臂坐标系。
|
||
:param camera_coordinate: list, 相机坐标系中的坐标
|
||
:return: list, 机械臂坐标系中的坐标
|
||
"""
|
||
camera_coordinate = np.array(camera_coordinate)
|
||
# 将相机坐标系转换到机械臂末端坐标系
|
||
arm_coordinate = [0, 0, 0]
|
||
if self.camera_matrix is not None and self.camera_trans is not None:
|
||
camera_matrix = copy.deepcopy(self.camera_matrix)
|
||
camera_trans = copy.deepcopy(self.camera_trans)
|
||
else:
|
||
camera_matrix = np.array([[-0.04169144,0.99888175,-0.02229508],[0.99912694,0.04174075,0.00175093],[0.00267958,-0.02220262,-0.9997499]])
|
||
camera_trans = np.array([-0.00209053,-0.01021561,-0.16877656])
|
||
arm_coordinate = camera_matrix @ (camera_coordinate/1000) + camera_trans
|
||
# 将机械臂末端坐标系转为机械臂基座坐标系
|
||
targetPosition = R.from_quat(self.quaternion).as_matrix() @ arm_coordinate + self.position
|
||
return targetPosition.tolist()
|
||
|
||
def calculate_arm_normal(self, data):
|
||
"""将相机坐标系中的法向量转换为机械臂基座坐标系中的法向量"""
|
||
normal_vector = data['normal']
|
||
|
||
# 相机坐标系到机械臂末端坐标系
|
||
arm_normal = [normal_vector[1], normal_vector[0], -normal_vector[2]]
|
||
|
||
# 机械臂末端坐标系到基座坐标系
|
||
transformed_normal = R.from_quat(self.quaternion).as_matrix() @ arm_normal
|
||
|
||
return {
|
||
'label': data['label'],
|
||
'transformed_normal': transformed_normal.tolist()
|
||
}
|
||
|
||
def calculate_target_Euler(self, points):
|
||
"""计算目标欧拉角"""
|
||
converted_points = []
|
||
|
||
for point_data in points:
|
||
point = np.array(point_data['normals_position'], dtype=np.float64).reshape(1, -1)
|
||
temp_point = np.zeros((1, 6), dtype=np.float64)
|
||
|
||
# 复制位置坐标
|
||
temp_point[:, :3] = point[:, :3]
|
||
|
||
# 计算欧拉角
|
||
# temp_point[0][3] = -math.asin(point[0][4])
|
||
# temp_point[0][4] = -math.atan2(-point[0][3], point[0][5])
|
||
# temp_point[0][5] = 0.0
|
||
|
||
temp_point[0][3] = -math.asin(point[0][4])
|
||
temp_point[0][4] = math.atan2(point[0][3], point[0][5])
|
||
temp_point[0][5] = -np.pi/2
|
||
|
||
# 确保角度在[-pi, pi]范围内
|
||
if temp_point[0][5] > np.pi:
|
||
temp_point[0][5] -= 2*np.pi
|
||
|
||
converted_points.append({
|
||
'label': point_data['label'],
|
||
'position': temp_point[0].tolist()
|
||
})
|
||
|
||
return converted_points
|
||
|
||
def set_camera_offset(self, offset_x=0, offset_y=0, offset_z=0):
|
||
"""
|
||
设置相机坐标系下的手动偏置，限制在±50mm范围内
|
||
|
||
Args:
|
||
offset_x: X轴偏置(毫米)，范围±50mm
|
||
offset_y: Y轴偏置(毫米)，范围±50mm
|
||
offset_z: Z轴偏置(毫米)，范围±50mm
|
||
"""
|
||
# 限制偏置值在±50mm范围内
|
||
offset_x = max(-50, min(50, offset_x))
|
||
offset_y = max(-50, min(50, offset_y))
|
||
offset_z = max(-50, min(50, offset_z))
|
||
|
||
self.camera_offset_x = offset_x
|
||
self.camera_offset_y = offset_y
|
||
self.camera_offset_z = offset_z
|
||
self.logger.log_info(f"设置相机坐标系偏置: X={offset_x}mm, Y={offset_y}mm, Z={offset_z}mm") |