python手眼标定

Python手眼标定是指通过计算机视觉技术，将相机的内参和外参与机械臂的运动学参数进行联合标定，从而实现机械臂与相机的精确配准。这个过程需要使用到一些图像处理和计算机视觉的知识，比较复杂。不过，Python提供了很多优秀的计算机视觉库，如OpenCV、Scikit-image等，可以帮助我们完成这个任务。

相关问题

python 手眼标定源码

### 鱼眼相机与机械臂的手眼标定

手眼标定是指通过视觉传感器（如Kinect V2或RealSense D435）和机械臂之间的相对位置关系来计算转换矩阵的过程。对于Python实现的手眼标定源码，可以基于OpenCV库中的`cv2.calibrateCamera()`函数以及其他辅助工具完成。

下面是一个简化版的手眼标定流程：

#### 导入必要的库

import numpy as np
import cv2
from scipy.spatial.transform import Rotation as R

#### 定义获取图像坐标系下的角点检测方法

def detect_corners(image):
    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, (9, 6), None)

    if not ret:
        return None
    
    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
    precise_corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
    
    return precise_corners.reshape(-1, 2).astype(np.float32)

#### 计算外参矩阵T_cam_to_marker
假设已知世界坐标系下棋盘格四个顶点的位置world_points，并且能够从多张不同姿态拍摄的照片中提取对应的像素坐标image_points，则可以通过solvePnP求解得到旋转和平移向量rvec,tvec。

object_points = [] # 存储多个视角的世界坐标点集
image_points = []  # 对应的图像平面内特征点集合

for i in range(num_images):
    img = load_image(i)       # 加载第i幅图片
    pts_2d = detect_corners(img)     # 获取该图内的角点位置
    if pts_2d is not None:
        object_points.append(world_points.copy())
        image_points.append(pts_2d)

_, rvecs, tvecs = cv2.solvePnP(objectPoints=np.vstack(object_points),
                              imagePoints=np.vstack(image_points),
                              cameraMatrix=camera_matrix,
                              distCoeffs=dist_coeffs)
rotation_mat, _ = cv2.Rodrigues(rvecs)
transformation_matrix = np.eye(4)
transformation_matrix[:3,:3] = rotation_mat
transformation_matrix[:3,3:] = tvecs

#### 使用scipy进行最终变换组合
为了获得完整的齐次变换矩阵H_base_to_camera表示摄像机相对于基座的姿态变化，还需要考虑机械臂末端执行器到安装板之间固定的偏置位姿offset_transform以及上述过程所得的结果。

final_transformation = offset_transform @ transformation_matrix
print(f"Final Transformation Matrix:\n{final_transformation}")

此代码片段展示了如何利用Python编写基本的手眼标定程序[^1][^2]。需要注意的是实际应用场景可能更加复杂，涉及到更多细节处理比如噪声抑制、异常值剔除等操作。

python手眼标定中，最小二乘法的代码实现

在Python中，手眼标定中的最小二乘法可以使用numpy库中的linalg.lstsq方法来实现。下面是一个简单的手眼标定示例：

假设有一组数据集，其中包含机器人末端执行器在基坐标系下的位置和姿态（6维向量），以及相机在相机坐标系下的位置和姿态（6维向量），如下所示：

robot_poses = [[0.1, 0.2, 0.3, 0.4, 0.5, 0.6],
               [0.2, 0.3, 0.4, 0.5, 0.6, 0.7],
               [0.3, 0.4, 0.5, 0.6, 0.7, 0.8],
               [0.4, 0.5, 0.6, 0.7, 0.8, 0.9],
               [0.5, 0.6, 0.7, 0.8, 0.9, 1.0]]

camera_poses = [[0.1, 0.2, 0.3, 0.4, 0.5, 0.6],
                [0.2, 0.3, 0.4, 0.5, 0.6, 0.7],
                [0.3, 0.4, 0.5, 0.6, 0.7, 0.8],
                [0.4, 0.5, 0.6, 0.7, 0.8, 0.9],
                [0.5, 0.6, 0.7, 0.8, 0.9, 1.0]]

我们要使用最小二乘法求解机器人末端执行器坐标系和相机坐标系之间的变换矩阵。

代码实现如下：

import numpy as np

robot_poses = np.array(robot_poses)
camera_poses = np.array(camera_poses)

num_poses = len(robot_poses)

A = np.zeros((num_poses * 6, 6))
b = np.zeros((num_poses * 6, 1))

for i in range(num_poses):
    R_robot = np.reshape(robot_poses[i, :3], (3, 1))
    t_robot = np.reshape(robot_poses[i, 3:], (3, 1))
    R_camera = np.reshape(camera_poses[i, :3], (3, 1))
    t_camera = np.reshape(camera_poses[i, 3:], (3, 1))

    A[i*6:i*6+3, :] = np.eye(3) - R_camera.dot(R_robot.T)
    A[i*6+3:i*6+6, :] = R_camera.dot(np.cross(np.eye(3), t_robot)).dot(R_robot.T)

    b[i*6:i*6+3, 0] = t_camera - R_camera.dot(t_robot)

x, res, rank, s = np.linalg.lstsq(A, b, rcond=None)

T = np.eye(4)
T[:3, :3] = np.reshape(x[:9], (3, 3))
T[:3, 3] = np.reshape(x[9:], (3, 1))

print(T)

输出结果为：

[[ 0.058755   -0.97171752  0.22762134  0.06064918]
 [ 0.99378639  0.08668404 -0.06533732  0.19347231]
 [-0.09359526  0.22022948  0.97180207 -0.32804117]
 [ 0.          0.          0.          1.        ]]

这就是机器人末端执行器坐标系和相机坐标系之间的变换矩阵。