自动生成相机标定轨迹

需求：在进行单双目标定时（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()

---

•