用VISP+Opencv做相机到机械臂的标定（hand eye calibration）
首先要解决的问题如下图，需要知道的是camera到robot base(world)的变换矩阵：
然后可以简化成下面的图：
需要知道的是，
求 TB
机械臂endeffector到世界坐标系的变换可以通过关节转角+forward kinematics得到。因为我是直接通过tf读出来的，就不多说了。所以可以得到：
变换
求TA
这里我用的是opencv的solvePnP()函数，求出一个物体(这里是标定板）的3D空间点到2D图像点的变换，分为tvec(平移部分)和rvec(旋转部分)。
然后调用Rodrigues(rvec, rvecdst)函数, 可以得到最终的变换：
这个变换可以把一个点在相机坐标系下的坐标映射到标定板坐标系。
即：
注意，相机坐标系很可能和opencv的坐标系不一样！！！
一般情况下opencv的坐标系是：x 指向右，y向下，z向前
这里我们用的Kinect的坐标系是：x 指向左，y向上，z向前 （<-这里被坑了好久……）
所以还需要增加一个绕z轴的180度旋转变换：
其中
求TX
VISP提供了一个hand eye calibration的package。不过这里的相机是固定在机械臂的手上，标定板和世界坐标系固连，我们是相机和世界坐标系固连，标定板在机械臂end-effector上，然后通过移动机械臂采集多个变换。具体原理我也不是很清楚，可以参考网上的文档比如这里和这里。
根据这个包的说明需要的input是：
- a list of camera-to-object transformations （对我们来说是object-to-camera）
- a list of world to hand transformations
output是：
- hand to camera transformation
但是这个”camera-to-object” transformation的具体定义也没有说清楚，于是搜了一下visp的文档，确认了这里指的frame A to frame B的变换是坐标系的变换矩阵。
总之接下来就是移动机械臂采集多个点（两个以上，我用了10个点），分别发送到：
world_effector (visp_hand2eye_calibration/TransformArray)
和
camera_object (visp_hand2eye_calibration/TransformArray)
两个topic下。
通过
compute_effector_camera (visp_hand2eye_calibration/compute_effector_camera) 这个service得到
最终要求的
总的来说因为开源资源很完善，流程还是比较清楚的，当时觉得比较坑的两个地方就是opencv的坐标系和Kinect的不一样；以及visp的坐标变换定义。