物体位姿估计精度验证实验（涉及位姿估计，相手眼标定，机械臂运动）
1.位姿估计
简单介绍，采用双目结构光相机，利用拍摄的点云数据和CAD模型点云进行ICP配准，获取物体在相机坐标系下的位姿（R，t）
2.手眼标定
本文采用的是Eye to Hand 方式，与opencv中讲解的Eye in Hand方式不同；因此花了点时间，首先还是先来看Opencv官方的手眼标定函数
Opencv 手眼标定函数calibrateHandEye()
（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的矩阵定义很迷。。。。。。。。不过反正是可以用了
其他标定函数
我自己用的是网上网友把opencv中的代码自己实现了一下
另外还有一个github的matlab版本，这个没试过
总之原理就这样
3.机械臂运动
我们设想的是去验证整个位姿估计框架的精度：
包含了物体到相机，相机到机器人基座两部分变换关系
实验方案：
大概就这样，实际中B是一个框，A是中间那部分，初始状态就是A放在B中
机器人位姿校正推导：
这里的坐标系我喜欢用自己的定义，不用opencv那里 的反人类定义
那么就有：
然后实验就是完事了