需求:在进行单双目标定时(camera in hand or camera to hand),需要移动机械手,使camera可多角度得对场景的标定板进行采集。
代码:
import random
import itertools
import numpy as np
from toolbox import vgl
from enum import Enum
class CameraMountingType(Enum):
CameraToHand = 0
CameraInHand = 1
class GenCaliPoses(object):
def __init__(self, config):
self.config = config
def genPoses(self):
# 事先设置一个初始pose,在该pose下,最好将相机正对标定板
Ttr = vgl.Pose2T(self.config['OriginPose'])
# 用尺子大概量出相机到标定板的距离
Tot = vgl.Pose2T(self.config['CaliRangeConfig_dict']['object_in_tool'])
# 求出object(如标定板)在机械手的坐标
Tor = Ttr.dot(Tot)
# 偏移量设置
range_delta_u = self.config['CaliRangeConfig_dict']['range_delta_u']
range_delta_v = self.config['CaliRangeConfig_dict']['range_delta_v']
range_delta_w = self.config['CaliRangeConfig_dict']['range_delta_w']
random_delta_x = self.config['CaliRangeConfig_dict']['random_delta_x']
random_delta_y = self.config['CaliRangeConfig_dict']['random_delta_y']
random_delta_z = self.config['CaliRangeConfig_dict']['random_delta_z']
random_delta_u = self.config['CaliRangeConfig_dict']['random_delta_u']
random_delta_v = self.config['CaliRangeConfig_dict']['random_delta_v']
random_delta_w = self.config['CaliRangeConfig_dict']['random_delta_w']
u_range = np.arange(*range_delta_u)
v_range = np.arange(*range_delta_v)
w_range = np.arange(*range_delta_w)
all_pose = []
for u_delta, v_delta, w_delta in itertools.product(u_range, v_range, w_range):
delta_x = random.uniform(*random_delta_x)
delta_y = random.uniform(*random_delta_y)
delta_z = random.uniform(*random_delta_z)
delta_u = u_delta + random.uniform(*random_delta_u)
delta_v = v_delta + random.uniform(*random_delta_v)
delta_w = w_delta + random.uniform(*random_delta_w)
# 求出object坐标的变换
T_obj_change = vgl.Pose2T([0, 0, 0, delta_u, delta_v, delta_w])
# T_obj_change 为object(标定板)相对相机的坐标标换
# 在eye in hand 与eye to hand 时需注意是否使用T_obj_change的逆
if self.config['HandEyeType'] == CameraMountingType.CameraToHand:
Ttr_new = (Tor.dot(T_obj_change)).dot(np.linalg.inv(Tot))
else:
Ttr_new = (Tor.dot(np.linalg.inv(T_obj_change))).dot(np.linalg.inv(Tot))
Ttr_new[0, 3] += delta_x
Ttr_new[1, 3] += delta_y
Ttr_new[2, 3] += delta_z
new_pose = (vgl.T2Pose(Ttr_new)).flatten()
all_pose.append(new_pose)
return all_pose
if __name__ == '__main__':
my_config = {
'HandEyeType': CameraMountingType.CameraToHand,
'OriginPose': [480.830, 0, 321.712, -0, -0.033, -180],
'CaliRangeConfig_dict': {
'object_in_tool': [0, 0, 321.7, 0, 0, 180],
'range_delta_u': [-10.0, 11.0, 10.0],
'range_delta_v': [-10.0, 11.0, 10.0],
'range_delta_w': [-10.0, 11.0, 10.0],
'random_delta_x': [-10.0, 10.0],
'random_delta_y': [-5.0, 5.0],
'random_delta_z': [-5.0, 5.0],
'random_delta_u': [-3.0, 3.0],
'random_delta_v': [-3.0, 3.0],
'random_delta_w': [-3.0, 3.0], },
}
gcp = GenCaliPoses(my_config)
gcp.genPoses()