其中相机的URDF文件中使用的相机插件设置的相机视角是57°，图像分辨率是1280x720，所以根据相机内参各个参数的的定义算出该相机模型的内参真实值。这个计算与`camera_info`这个topic中的信息一致。
接下来是移动标定板，程序中设定的是1秒钟产生一个随机位置并移动标定板，若相机能正确识别出标定板角点则将此时的照片保存，有深度图，红外相机的图还有RGB图存放在`save_checkboard_img`目录中。当你觉得采的图片足够多之后即可停止这个程序。
```
rosrun robot_sim camera_calibration
```
这里我们提供了一个python的脚本`camera_calibration.py`，位于`robot_sim/experiment/camera_calibration/scripts`，用于载入前面保存的图片以计算RGB相机与IR相机的内参并分别保存在`IR_cameraintrinsic_parameters.npz`与`RGB_cameraintrinsic_parameters.npz`中，直接python运行此脚本即可。你也可以自己写程序来计算内参，并与前面公式计算的结果像对比以验证你的标定算法的准确性与误差。
```
cd ~/your_catkin_ws/src/robot_sim/experiment/camera_calibration/scripts
python3 camera_calibration.py
```
## 在仿真环境中进行深度图配准
此实验将用到实验一中采集的两个相机拍摄的标定板的图片，这里我们提供了一个python脚本`depth_image_registration.py`来计算配准矩阵并将其前两行存放在`Registration_matrix.txt`中，因为实际remap深度图的时候也只会用到前两行。
```
cd ~/your_catkin_ws/src/robot_sim/experiment/depth_image_registration/scripts
python3 depth_image_registration.py
```
随后可以随便拿一对RGB图与深度图来观察配准矩阵对不对，因为后面会用到这个矩阵所以这里直接就是写成CPP了。
```
cd ../src
g++ ./depth_image_registration.cpp -o depth_image_registration $(pkg-config --cflags --libs opencv)
./depth_image_registration
```
下面是配准前与配准后的区别。
## 在仿真环境中进行手眼标定
这里使用的手眼系统属于眼在手上的情况，即eye on hand，首先将我们提供的机械臂的moveit功能包跑起来，其中加载了UR10机械臂、大寰机器人的AG-95二指抓手还有D435i RGBD相机安装在UR10机械臂的末端。这里的手的link是`yixiuge_ee_link`，眼的link是相机的RGB光学frame`camera_rgb_optical_frame`。标定用到的Aruco二维码的大小是0.2m，ID是582。
```
roslaunch yixiuge_ur10_moveit_config yixiuge_ur_moveit.launch
```
需要等moveit加载完之后再加载手眼标定包，因为大寰的二指抓手开起来有点费时，如果马上开hand_eye_calibration的话可能因为找不到moveit的group而报错。
```
roslaunch robot_sim hand_eye_calibration.launch
```
之后请参照视频中的操作进行操作最后可以算出手眼矩阵。设计的17个点一般情况下不会全部满足，可以通过RVIZ中的moveit的球拖动机械臂渠道其他位置以采集更多的数据从而使标定结果更加准确，下面是我们采集了37个点之后所计算出来的结果
```
translation:
x: -0.0171515439439
y: 0.129039200607
z: 0.146263405556
rotation:
x: 0.999995982804
y: 0.00268604823595
z: 0.000687382040816
w: 0.000589089946183
```
提供的机器人URDF文件中手眼矩阵的真实值如下，可以看出还是很准确的，误差在3,4毫米这样子。
```
translation:
x: -0.0175
y: 0.128
z: 0.1425
rotation:
x: 1
y: 0
z: 0
w: 0
```
手眼标定完成电机save后数据会保存在`~/.ros/easy_handeye/`的`easy_handeye_eye_on_hand.yaml`中，运行下面的launch文件来观察标定的结果
```
roslaunch robot_sim hand_eye_calibration_result.launch
```
## 在仿真环境中抓取
这里设计了两种方法的抓取，一种是使用传统的几何方法识别抓取点的抓取([参考论文](https://link.springer.com/article/10.1023/A:1008381314159))，一种是基于机器学习的抓取([GPD](https://github.com/atenpas/gpd))
### 基于几何方法的抓取
先把机械臂和抓取环境load进来随后启动抓取。
```
cd ~/your_catkin_ws/
roslaunch robot_sim geometric_method_grasp.launch
rosrun robot_sim geometric_method_grasp
```
我们这里提供的方法主要原理是先将物体从桌面分割，这里的分割使用的是直接将相机当前画面减去一个mask，然后设置阈值二值化。因为是仿真环境所以这样做还是比较稳定的，分割效果也是没有问题的。但这种超级简单的做法仅限于环境固定的情况，实际使用的话还是建议使用其他分割算法，比如用深度图生成掩模来提取物体。
分割出物体之后便是滤波然后提取轮廓，膨胀后再腐蚀以连接成闭合的轮廓，随后选择最大的轮廓作为机械臂要抓取的物体。计算轮廓的一阶矩得到物体的重心，这里假设物体的质量分布均匀。随后使用PCA计算轮廓的主方向和垂直主方向的副方向，这里也可使用轮廓的二阶矩来获得。随后计算轮廓的最小外接矩形，与副轴的两个交点连成一条直线，随后遍历这条直线上的点找到副轴与轮廓的两个交点作为抓取点，随后绘制抓取矩形框。详细实现请见代码。
### 基于机器学习方法的抓取
这里使用的是atenpas的[GPD](https://github.com/atenpas/gpd)这个方法，具体原理可见[论文](https://arxiv.org/abs/1706.09911)，这里不再赘述。在搭建GPD库的时候你可能会遇到某些问题，倘若GPD2.0.0版本出现问题则建议使用GPD1.5.0。这里在我们的仿真环境中成功运行了这一算法。我们提供的代码使用的是GPD的sample方式，使用的是caffe的cfg文件。再运行这部分代码的时候请注意调整cfg文件到正确的路径。此外这里的分割使用的是根据距离风格，而实际更为鲁棒的做法应该是使用RGB图生成掩模来分割点云。
```
roslaunch robot_sim GPD_method_grasp.launch
roslaunch robot_sim gpd_run.launch type:=2 topic:=/cloud_sample
```
### 关于gazebo中抓手抓取物体会莫名抖动
这是因为抓手的控制是位置控制而不是力控制所造成的。我们是使用JenniferBuehler编写的gazebo插件[gazebo-pkgs](https://github.com/JenniferBuehler/gazebo-pkgs)来解决这个问题的。具体原理是插件会检测手指与物体接触，设定一些阈值当达到条件之后将物体与抓手的相对位置进行固定并失能物体的collision属性从而解决这一抖动的问题。
## 在仿真环境中进行数据采集
主要原理是围绕物体生成若干个位姿，随后驱动机械臂运动到指定位姿之后拍照，保存数据
```
roslaunch robot_sim data_collection.launch
rosrun robot_sim data_collection
```
其中采样点的多少以及位置等都是可以认为调节的，以下是56个的采样点
