物体位姿估计精度验证实验（涉及位姿估计，手眼标定，机械臂运动）
- 1.位姿估计
- 2.手眼标定
- Opencv 手眼标定函数calibrateHandEye()
- （1）Eye in Hand
- （1）Eye to Hand
- halcon 手眼标定
- 其他标定函数
- 3.机械臂运动
- 实验方案：
- 机器人位姿校正推导：
1.位姿估计
简单介绍，采用双目结构光相机，利用拍摄的点云数据和CAD模型点云进行ICP配准，获取物体在相机坐标系下的位姿（R，t）
2.手眼标定
本文采用的是Eye to Hand 方式，与opencv中讲解的Eye in Hand方式不同；因此花了点时间，首先还是先来看Opencv官方的手眼标定函数
Opencv 手眼标定函数calibrateHandEye()
先贴一个包含棋盘格检测的c++代码 github
（1）Eye in Hand
在opencv 3.4版本以上，提供了手眼标定函数，本文采用的是4.4版本，贴一个官方文档
基本输入输出描述：
看着这个旋转矩阵，有点迷，来看一下官方的图
可以看出，官方求解的是相机到夹爪的变换矩阵：gTc；
其两个输入：
- 夹爪到机器人基座变换矩阵bTg
- 目标物体（一般是棋盘格）到相机的变换矩阵cTt
这就感觉很反人类，机器人每次运动读出来的坑定是基座到机械臂末端的变换矩阵，按照opencv的表示方法应该是gTb；然后pnp算法可以获取得到相机每次拍照相对于棋盘格的外参cTt（这个参数和opencv的是相符的）；
- 所以Eye in Hand相机标定使用opencv的标定函数，应该是要把机器人的位姿估计去逆？？？？
- 那为啥广大csdn朋友都是直接用的？？？纳闷 （看到最后）
（1）Eye to Hand
这里我们还是按照opencv的定义吧；
根据target到base从红色路径走和蓝色路径走，一次拍摄可以获得一个等式
bTg · gTt = bTc · cTt
两次拍摄，有
bTg1 · gTt = bTc · cTt1 （1）
bTg2 · gTt = bTc · cTt2 （2）
用（1）消去（2）中的 gTt，有
bTg2 · inv(bTg1 ) · bTc · cTt1 = bTc · cTt2
挪一挪，成AX=XB的形式：
bTg2 · inv(bTg1 ) · bTc = bTc · cTt2 · inv(cTt1)
对比一下eye in hand：好像bTg逆了一下？？？
那根据opencv变换矩阵的定义，是不是对于eye to hand直接可以用机器人读出的基座到末端的矩阵？？？
我怀着好奇试了一下，发现。。。。嘤嘤嘤，错了；
几种情况都试了几下，发现需要求逆的是eye to hand中的bTg，也就是机器人末端位姿取个逆,或许这里能解释广大的csdn友eye in hand中机器人末端矩阵不取逆的原因？？？
所以我感觉opencv的矩阵定义很迷。。。。。。。。不过反正是可以用了
halcon 手眼标定
由于上述方式计算出的手眼标定矩阵没有评价指标，然后看到51halcon上有人晒出具有评价指标的手眼标定，实验室有halcon的标定板，因此尝试了一下
步骤：
- 下载halcon hdevelop软件，安装后使用试用许可证
- 官网看到eye to hand 手眼标定例程，原来csdn上抄的是halcon的图
- 打开对应标定的示例程序
代码里注释都很清楚了，主要是读取四部分数据：
- 标定板描述文件
CalTabFile := ‘caltab_100mm.descr’（包含圆形标定板尺寸信息）； - 机械臂末端位姿文件：
这里例程一个pose一个.dat文件，
这里涉及到halcon的位姿表示类型，一般是采用2对应上面：f=2，zyx欧拉角方式，按照要求写之 - 图片文件：
读取的时候不用后缀，但是有可能检测不到标定板外框，圆形，需要调节算子find_calib_object [‘alpha’], [0.2] - 相机内参文件：
两种方式生成相机内参
一个是读文件
read_cam_par (DataNameStart + ‘start_campar.dat’, StartCamParam)
一个是硬编码，有很多种参数类型，具体看文档
gen_cam_par_area_scan_division (1.17685423e-002, -2.54169782e+003, 7.32908724e-006, 7.40000000e-006, 1.2433252307808543e+03, 1.0420874394843283e+03, 2448, 2048, StartCamParam)
- 根据程序提示运行就完事了：
求解完了之后，可以用算子，pose_to_hom_mat3d把pose转mat；手眼标矩阵和opencv差不太多，但是实验中感觉精度也有问题
其他标定函数
我自己用的是网上网友把opencv中的代码自己实现了一下
另外还有一个github的matlab版本，这个没试过
总之原理就这样
3.机械臂运动
我们设想的是去验证整个位姿估计框架的精度：
包含了物体到相机，相机到机器人基座两部分变换关系
实验方案：
大概就这样，实际中B是一个框，A是中间那部分，初始状态就是A放在B中
机器人位姿校正推导：
这里的坐标系我喜欢用自己的定义，不用opencv那里 的反人类定义
那么就有：
然后实验就是完事了
