基于IBVS，利用UR5实现简单视觉抓取

动机

在读完了RVC之后，我对于IBVS方法非常感兴趣。实验室内有一台UR5，所以我希望可以快速实践一下看看具体应用中汇出什么幺蛾子。
对于UR5的控制，如果直接采用MATLAB方法必然是最快捷的（因为可以借助RVC的工具箱），但是RVC的工具箱是PUMA560这个经典型的机器人为例子的。而且是以仿真为主的。而我目前开发机器人运动规划算法一般是基于MoveIt，所以希望尝试将RVC所述的IBVS算法移植到ROS上来（我搜了一下，目前ROS中似乎还没有类似的软件包）。充分利用Moveit中写好的防碰撞功能。

思路

IBVS算法在《RVC》的457页，截至到这页的算法只考虑了成像输入和相机位姿的世界坐标变换的输出。这就是我需要实现算法的第一部分，本质上这部分是完成机器人感知世界。该算法的流程为：

当前图像&位姿

交互矩阵

位姿调整量

分析

1. 由于IBVS求取交互矩阵需要：
   稳定可靠地提取图像特征点的像素坐标（理想情况下，整个伺服过程中特征点对应的世界坐标系位置不应当发生变化。当然这也就要求了在伺服过程中，特征不可以被遮挡，更不应当跑到相机成像平面外面去！）
2. 特征点的数量要求：
   这部分在书中的阐述较为简略。我其实一直有一个问题，就是由于上面所说的遮挡问题，有一些特征点可能会被遮挡，不过也不能排除可以部分之前被遮挡的特征点会显露出来。能否利用这个特点，构造一种各项异性的冗余特征群，即：在保证可以正确同期望图像匹配的前提下，利用冗余点实现更在有遮挡存在可能性的情况下，依然保证提取得到的特征点数量满足交互矩阵约束（一个要求秩为6的Jaccobian矩阵）。这可以在一定程度上提高程序的鲁棒性。
3. 深度计算：
   这里可以直接使用RGBD相机获取像素深度，不过书中也提到了可以基于匹配思想和一个巧妙的深度滤波器（458页）实现一种基于单目相机的深度计算方法。这里可以直接使用RGBD相机获取像素深度，不过书中也提到了可以基于匹配思想和一个巧妙的深度滤波器（458页）实现一种基于单目相机的深度计算方法。（这里我先偷个懒，暂时用真实数据替代（仿真验证过程）、后续在最开始联调机械臂的时候，我会先利用realsense验证算法正确之后，改用单目相机实现一下深度滤波器）
4. 边缘控制器：
   这其实是一个应用级别的话题，目的是将特征点控制在图像内部，说白了就是在特征点接近相机边缘之前，把相机往后拉。（当然，不完全是后拉）

第一步做什么（CentralCamera）

作为一个“大项目”的开胃菜，第一步一般一保证自己算法构建的世界是安全稳定的。这很重要，否则你可能某天会发现自己程序一直崩溃的原因是自己把重力方向设成了水平。
对于本项目来说，要先保证，模拟相机的投影和反投影过程必须不能出幺蛾子！这直接决定了模拟相机所看到的和真实相机图像是否能保证基本一致。所以首先要维护的类为：CentralCamera类，我认为相机的位姿（也就是相机的外参）应该是交给它自己来管理的。这样每次更新位姿对于对象来说只需要维护一次私有变量，不需要担心变量的安全性。
因此CentralCamera类的成员变量为：

• private：intrinsic 内参
• private：extrinsic 外参 存储相机的位姿

CentralCamera类中两个比较重要的方法分别为：

• project() ：实现模拟相机将世界坐标系下点$A^W$投影到像素平面上的操作
• reproject() ：重投影（这是一个以验证为目的的操作，像素坐标上的某个相机坐标系下的点$A^C$，reproject得到的相机坐标系下坐标应当在$A^C$的附近。进一步，一个世界坐标系下的点$A^W$，被project方法转换到像素坐标后，reproject得到的相机坐标系下坐标转换到世界坐标系中，应当在点$A^W$的附近。于此即可验证模拟相机是否是正确的）

ROS的可视化非常好用。因此，期望配合Rviz实现一个CentralCamera的验证。