手眼标定实现

 手眼标定基于Tsai的两步法标定，是经典的Ax = xB 求解模型。

Tsai的两步法标定是基于径向校正约束；第一步：利用最小二乘法求解线性方程组，得出相机的外参数；第二步：根据获得的相机外参数，求取相机的内参数；如果无透视畸变，可以使用一个线性方程求出。         

罗第6、7章；特别：P164-166）

1.手眼标定就是对机械手和相机的位置关系进行标定，这样根据识别得到的像素位置去引导机械手去抓取。

2.eye-in-hand:通过相机标定确定相机坐标系和世界坐标系之间的关系；即P&&R。这时如果知道相机坐标系和机械手基础坐标系之间的关系，即可得到物体在机械手坐标系中的坐标。

3.eye-to-hand:通过相机标定确定相机坐标系和世界坐标系之间的关系；即P&&R;因为相机和机械手基础坐标系之间的关系是固定的，即：只要求出相机坐标系在基础坐标系中的位置，即可获得物体在机械手基础坐标系中的位置。

****************手眼标定执行流程
**创建数据模型
create_calib_data ('calibration_object', 1, 1, CalibDataID)

***设置相机参数

set_calib_data_cam_param (CalibDataID, 0, 'area_scan_division', [])

**设置标定板描述文件
set_calib_data_calib_object (CalibDataID, 0, 'calplate.cpd')

**循环读取标定板图像
for index := 1 to 10 by 1
    read_image (Image, 'fabrik')
   
    *读取机械手法兰盘在基础坐标系中的位置姿态
    read_pose ('campose.dat', Pose)
   
    *将机械手法兰盘在基础坐标系中的位置姿态添加到标定数据模型中
    set_calib_data (CalibDataID, 'model', 'general', 'reference_camera', Pose)
   
    **获取标定对象，并添加到数据模型中
    find_calib_object (Image, CalibDataID, 0, 0, 0, [], [])
    *获得世界坐标系和相机坐标系的相对位置关系
    get_calib_data_observ_pose (CalibDataID, 0, 0, 0, ObjInCameraPose)
endfor

calibrate_hand_eye (CalibDataID, Errors)

*获得机械手基础坐标系在摄像机坐标系下的坐标
get_calib_data (CalibDataID, 'camera', 0, 'params', DataValue)

**获得摄像机坐标系在机械手基础坐标系下的坐标
pose_invert (ObjInCameraPose, PoseInvert)

*根据摄像机在机械手基础坐标系下的姿态和目标在摄像机坐标系下的姿态，求解目标在摄像机基础坐标系下的姿态
pose_compose (PoseInvert, PoseInvert, PoseCompose)