grasp detection

两个开源工作做得比较好的二维平面抓取点检测项目。
gcnn, 文档

GR-ConvNet

1. gqcnn

Dex-Net是 Berkeley Auto Lab 维护的一个项目，旨在训练机器人学习抓取。项目主要包含两个部分：Dex-net用于生成数据集，GQ-CNN (Grasp Quality Convolutional Neural Networks)用于预测平行夹爪用候选抓取位姿从点云中成功抓取物体的可能性。
GQ-CNN 可用于快速规划抓取位姿。
gqcnn文档
本文旨在利用预训练的GQ-CNN 模型，进行grasp planning。

请注意：GQ-CNN 模型对数据生成过程中使用的以下参数敏感：

The robot gripper
The depth camera
The distance between the camera and workspace.

1.1 安装

建议在 Pip 和 ROS 安装中用 Conda 或 Virtualenv 创建一个新的 Python 环境。

1.1.1 ros 安装

# 下载项目代码
cd catkin_workspace/src
git clone https://github.com/BerkeleyAutomation/gqcnn.git#  配置环境
cd gqcnn
pip install .

注：pip install . 的安装方法很有可能不成功，可以找到gpu_requirements.txt文件，逐个安装。其中autolab_core需要安装为ros package，方法如下：

cd catkin_workspace/src
git clone https://github.com/BerkeleyAutomation/autolab_core.gitcd autolab_core
python setup.py install

编译功能包

cd catkin_workspace
catkin_make

1.1.2 下载预训练模型

./scripts/downloads/models/download_models.sh

预训练模型下载至 gqcnn/models文件夹，对于平行夹爪来说主要有两个模型：GQCNN-4.0-PJ和FC-GQCNN-4.0-PJ 。两者都是用PhotoNeo PhoXi S相机数据为平行夹爪训练的模型，图片中的物体杂乱摆放。

On-Policy Dataset Synthesis for Learning Robot Grasping Policies Using Fully Convolutional Deep Networks表明FC-GQCNN-4.0-PJ效率更高,精度也更高。 FC-GQ-CNN无需依赖交叉熵方法（CEM）来迭代的搜索抓取动作空间以获得最佳抓取策略，而是密集而有效地并行评估整个动作空间。它可以在 0.625s 内评估 5000x 此以上的 grasp，进而每小时可进行 296 次抓取.

1.2 测试

测试的预训练模型为：GQCNN-4.0-PJ 和 FC-GQCNN-4.0-PJ。测试用例在data/examples/clutter/phoxi/文件夹下，GQCNN-4.0-PJ模型测试图片在dex-net_4.0文件夹，FC-GQCNN-4.0-PJ测试图片在gqcnn 文件夹。

上述示例都是Phoxi S相机的图片，如果要测试其他输入数据，则需要修改cfg/examples/fc_gqcnn_pj.yaml文件下[“policy”][“metric”][“fully_conv_gqcnn_config”] 的高度和宽度配置。

1.2.1 python 测试

测试预训练的GQ-CNN 平行夹爪网络，需要使用的深度图和分割图（data/examples中有测试用例）

python examples/policy.py GQCNN-4.0-PJ --depth_image data/examples/clutter/phoxi/dex-net_4.0/depth_0.npy --segmask data/examples/clutter/phoxi/dex-net_4.0/segmask_0.png --camera_intr data/calib/phoxi/phoxi.intr

如果要测试 FC-GQ-CNN 网络，需要添加 --fully_conv 标记。例如：

python examples/policy.py FC-GQCNN-4.0-PJ --fully_conv --depth_image data/examples/clutter/phoxi/fcgqcnn/depth_0.npy --segmask data/examples/clutter/phoxi/fcgqcnn/segmask_0.png --camera_intr data/calib/phoxi/phoxi.intr

rgb原图及抓取点的预测图如下(GQCNN-4.0-PJ)：

也可以直接运行测试 the pre-trained parallel jaw FC-GQ-CNN model 的脚本：

 ./scripts/policies/run_all_dex-net_4.0_fc_pj_examples.sh

1.2.2 ros 测试

启动预训练的 Dex-Net 4.0 平行夹爪 ros 抓取规划服务：

roslaunch gqcnn grasp_planning_service.launch model_name:=GQCNN-4.0-PJ

model_name: Name of the GQ-CNN model to use. Default is GQCNN-4.0-PJ.
model_dir: GQ-CNN models 的存储路径. Default is models/. If you are using the provided download_models.sh script, you shouldn’t have to modify this.
fully_conv：注意与model_name相对应。如果采用了FC-GQCNN-4.0-PJ，fully_conv则为 True：

roslaunch gqcnn grasp_planning_service.launch model_name:=FC-GQCNN-4.0-PJ fully_conv:=True

启动示例 ROS policy，（使用预先保存的图片）

python examples/policy_ros.py --depth_image data/examples/clutter/phoxi/dex-net_4.0/depth_0.npy --segmask data/examples/clutter/phoxi/dex-net_4.0/segmask_0.png --camera_intr data/calib/phoxi/phoxi.intr
# 也可以只用深度图
python examples/policy_ros.py --depth_image data/examples/clutter/phoxi/dex-net_4.0/depth_0.npy --camera_intr data/calib/phoxi/phoxi.intr

depth_image_filename: Path to a depth image (float array in .npy format).
segmask_filename: Path to an object segmentation mask (binary image in .png format).
camera_intr_filename: 相机内参路径 (.intr file generated with BerkeleyAutomation’s perception package).

注：ros测试中segmask 图不是必须的。如果segmask is None，程序会根据深度图和rgb 图自动生成 segmask。但 python 测试中 segmask 图是必须的。

debug：

ModuleNotFoundError: No module named 'rospkg'
因为运行环境为python3。
解决方法: pip3 install rospkg。
cannot open shared object file
OSError: /home/ur/miniforge3/envs/grasp/lib/libgeos_c.so: cannot open shared object file: No such file or directory
解决办法：locate libgeos_c.so 找到 libgeos_c.so 的路径。然后将这三个文件复制到/home/ur/miniforge3/envs/grasp/lib/文件夹。
ImportError: dynamic module does not define module export function (PyInit_cv_bridge_boost)
这是因为ros 自带的 cv_bridge只支持 python2。想要使用Python3需要另外创建 ros 工作空间,自行编译cv_bridge包。然后在 python3 环境执行：

source ~/cvbridge_build_ws/install/setup.bash --extend

详细解决方法参见连接：Jetson AGX Xavier避坑指南(五)——环境搭建2.(python3、conda、ros、pytorch、tensorflow)

1.3 ros 代码导读

本文仅解读 FC-GQCNN-PJ 部分。PJ 表示平行夹爪。
输入深度图(.npy)和 sagmask 图(可选)，输出抓取点。
采用了ros service 通信机制。

1.3.1 客户端：ros_policy.py

客户端节点：grasp_planning_example。

主要输入：depth_image (.npy), segmask(也可以没有)，camera_intr(相机内参配置文件路径)。

订阅/grasp_planner和/grasp_planner_segmask服务。前者适用于仅有depth_image的情况，后者用于有 depth_image和 segmask 图的情况。下面我们仅查看不需要 segmask 图的/grasp_planner服务。注意：虽然服务端需要输入 color_image,但客户端并没有提供 color_image输入，而是以一个与深度图同尺寸的全零矩阵替代。

客户端调用/grasp_planner服务，得到grasp=grasp_resp.grasp数据。进而转换为抓取动作。
Grasp2D (gqcnn/grasping/grasp.py)：Parallel-jaw grasp in image space。其 pose()函数会返回 grasp 在相机坐标系中的坐标(type: autolab_core.RigidTransform)，该函数有一个输入参数grasp_approach_dir，是相对于相机坐标系的。
GraspAction:进一步封装grasp 和Grasp2D数据。

1.3.2 服务端：graspy_planner_node.py

main

创建Grasp_Sampler_Server node。
加载配置文件：fully_conv、parallel_jaw 类型的配置文件路径为 "cfg/examples/ros/fc_gqcnn_pj.yaml" 该文件引入了"cfg/examples/fc_gqcnn_pj.yaml"文件，并重新定义配置文件中的 visualization 部分（vis）。
声明一个Publiser： grasp_pose_publisher，用于发布抓取位姿信息（"/gqcnn_grasp/pose",PoseStamped）。
初始化抓取策略：grasping_policy = FullyConvolutionalGraspingPolicyParallelJaw(policy_cfg)。
初始化抓取规划器：grasp_planner = GraspPlanner(cfg, cv_bridge, grasping_policy, grasp_pose_publisher)。
声明一个 Service： grasp_planner，服务类型GQCNNGraspPlanner，回调函数grasp_planner.plan_grasp。

srv

srv类型为GQCNNGraspPlanner.srv：

# request params
sensor_msgs/Image color_image
sensor_msgs/Image depth_image
sensor_msgs/CameraInfo camera_info
---
# response params
GQCNNGrasp grasp

GQCNNGrasp.msg 定义如下

geometry_msgs/Pose pose
float64 q_valueuint8 PARALLEL_JAW=0
uint8 SUCTION=1
uint8 grasp_type# grasp representation in image space.
float64[2] center_px
float64 angle
float64 depth
sensor_msgs/Image thumbnail

grasp_planner服务的回调函数为 plan_grasp().接收参数为深度图、sagmask图、相机参数。
如果没有 sagmask 图，程序会根据深度图自动生成。

如果要用自己的相机，注意修改相机参数配置。配置文件在 data/calib/文件夹下，新建一个自己相机的文件名如 k4a 然后添加两个配置文件：k4a.intr，k4a_to_world.tf。注意修改 cfg/examples/fc_gqcnn_pj.yaml 中输入的图像尺寸参数im_height和im_width。（或resize 图像尺寸，并修改 k4a.intr参数）。

grasp planner

GraspPlanner类中的plan_grasp(req)函数是服务的回调函数。

plan_grasp(req)

首先调用self.read_images(req)函数，将输入数据用 cv_bridge解析为 color_im、depth_im 和 camera_intr，数据类别为 perception 定义的 ColorImage, DepthImage。

然后调用self._plan_grasp(color_im, depth_im, camera_intr)函数Planning Grasp。

self._plan_grasp()

如果没有segmask，该函数会用 perception 定义的 BinaryImage 根据 depth_im自动生成segmask图。
然后将rgbd_im, camera_intr,segmask封装为rgbd_state（RgbdImageState类）。

然后调用execute_policy函数。

execute_policy(rgbd_state, self.grasping_policy, self.grasp_pose_publisher, camera_intr.frame)

execute_policy()

grasp = grasping_policy(rgbd_state) 。（ graspy_planner_node.py 中有定义：grasping_policy = FullyConvolutionalGraspingPolicyParallelJaw(policy_cfg)。稍后我们会看一下FullyConvolutionalGraspingPolicyParallelJaw是如何运行的）

然后将数据解析并赋给GQCNNGrasp类型的gqcnn_grasp。

将 gqcnn_grasp.pose封装为PoseStamped类型的pose_stamped，然后grasp_pose_publisher.publish(pose_stamped)

最后返回gqcnn_grasp。

1.3.3 grasping policy

下面我们来看一下 grasping_policy是如何工作的。

所有的GQ-CNN 抓取策略都是 GraspingPolicy 的子类。输入 RgbdImageStates，返回 GraspAction。
FC-GQCNN-PJ模型的grasping_policy为FullyConvolutionalGraspingPolicyParallelJaw (定义在gqcnn/grasping/policy/fc_policy.py)，派生关系如下：

Policy -> GraspingPolicy -> FullyConvolutionalGraspingPolicy->FullyConvolutionalGraspingPolicyParallelJaw

Policy 是抽象基类，定义了抽象方法 action(state)，Returns an action for a given state, GraspingPolicy 类引入了grasp_quality_fn实例属性, 然后在 action() 函数中定义了抽象方法_action(state)。最后由FullyConvolutionalGraspingPolicy类实现_action(state)方法。

execute_policy() 函数中主要用到了grasping_policy的三个属性(或方法)：

.q_value
.grasp（type ：Grasp2D, 包含.pose()、.center、.depth、.angle等方法和属性）、
.image.rosmsg

1.3.1 Policy

gqcnn/grasping/policy/policy.py

Policy 是抽象基类，定义如下：

class Policy(ABC):"""Abstract policy class."""def __call__(self, state):"""Execute the policy on a state."""return self.action(state)@abstractmethoddef action(self, state):"""Returns an action for a given state."""pass

__call__()函数使类的实例可以当做函数调用。即：

grasping_policy = Policy() #实例化
grasp = grasping_policy(rgbd_state) #函数调用
# 等同于
grasp = grasping_policy.action(rgbd_state)

而 action(state)是抽象方法，需要在子类GraspingPolicy中实现。

1.3.2 GraspingPolicy

gqcnn/grasping/policy/policy.py

1.__init__(self,config)：
载入 policy config 数据，即"cfg/examples/ros/fc_gqcnn_pj.yaml"的 "policy"部分。

定义属性： self._grasp_quality_fn ，并允许作为 grasp_quality_fn 属性调用。

2.实现action(state)方法:
action 函数并没有实现啥功能，而是调用了_action(state) 函数，而 _action(state) 是抽象方法，在子类 FullyConvolutionalGraspingPolicy 中实现。

self._grasp_quality_fn

我们来看一下self._grasp_quality_fn干了什么。

self._grasp_quality_fn = GraspQualityFunctionFactory.quality_function( metric_type, self._metric_config)
然后quality_function返回了 FCGQCnnQualityFunction(config) 。 FCGQCnnQualityFunction类加载FCGQCNNTF网络模型，返回quality()。quality函数return FCGQCNNTF模型的predict(images, depths)。predict 根据深度图像预测抓取成功的概率和抓握姿势。

FCGQCnnQualityFunction类的父类是抽象基类GraspQualityFunction，GraspQualityFunction在 call 函数中 return self.quality(state, actions, params)。即可以将FCGQCnnQualityFunction类的实例当做方法调用，返回的是 quality 函数。

1.3.3 FullyConvolutionalGraspingPolicy

gqcnn/grasping/policy/fc_policy.py

实现_action(state)方法：

解包 state(type: RgbdImageState) 数据。然后提取 images 和 depths。
preds = self._grasp_quality_fn.quality(images, depths)：预测抓取成功概率。
_mask_predictions() ：用裁剪和降采样的 sagmask 图像，进一步筛选出在物体上的预测成功的grasps.
_sample_predictions()：采样 num_actions=1 个predictions。
_get_actions() :将返回数据封装为 GraspAction 类型。_get_actions() 函数为抽象方法定义在子类FullyConvolutionalGraspingPolicyParallelJaw中。

GraspAction

GraspAction 实例主要有四个属性，execute_policy() 函数用到了前三个。

q_value
grasp（type ：Grasp2D, 包含.pose()、.center、.depth、.angle等方法和属性）
image.rosmsg
policy_name

Grasp2D

pose(grasp_approach_dir=None):计算grasp 在相机坐标系的 3d 位姿。grasp_approach_dir是接近目标物体的方向(相机坐标系)。
depth grasp: 中心点的深度(3d)
center:图像坐标系下的grasp 中心点。
angle ：Grasp axis angle with the camera x-axis。

Grasp2D.pose()

pose(grasp_approach_dir=None)
计算grasp 在相机坐标系的 3d 位姿。
参数：grasp_approach_dir (numpy.ndarray) 是接近目标物体的方向(相机坐标系),如果未指定，则使用相机光轴方向。
在相机坐标系下抓取位姿矩阵：grasp_rot_camera的 x,y,z 轴定义如下：
grasp_x_camera = grasp_approach_dir （type: np.array:(3,)）
grasp_y_camera = grasp_axis_camera （np.array:(3,)）
grasp_z_camera = np.cross(grasp_x_camera, grasp_y_camera) ( np.array:(3,))
相机坐标系下抓取中心点坐标 grasp_center_camera通过下列函数计算

 grasp_center_camera = self.camera_intr.deproject_pixel(self.depth, center_px_im)

camera_intr.deproject_pixel函数主要内容为point_3d = depth * np.linalg.inv(self._K).dot(np.r_[pixel.data, 1.0]),即
[xyz]=depth∗[fxγcx0fycy001]−1∗[pixel.xpixel.x1]\left[\begin{matrix} x\\ y\\ z\end{matrix}\right] =depth * \left[\begin{array}{ccc}f_x & \gamma & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1\end{array}\right]^{-1} * \left[\begin{matrix} \text{pixel.x}\\ \text{pixel.x} \\ 1\end{matrix}\right] ⎣⎡xyz⎦⎤=depth∗⎣⎡fx00γfy0cxcy1⎦⎤−1∗⎣⎡pixel.xpixel.x1⎦⎤

Returns：从 grasp 到相机坐标系的转换。

T_grasp_camera = RigidTransform(rotation=grasp_rot_camera,translation=grasp_center_camera,from_frame="grasp", to_frame=self.camera_intr.frame)  # from_frame and to_frame just a name
return T_grasp_camera

Return type：autolab_core.RigidTransform

RigidTransform 似乎啥也没干，仅仅把输入的参数以各种输出形式（如四元数、欧拉角、geometry_msgs.msg.Pose）封装一下。

RigidTransform

autolab_core.RigidTransform
刚体从一个坐标系到另一个坐标系的变换。

重要属性

pose_msg
geometry_msgs.msg.Pose， The rigid transform as a geometry_msg pose.

机器人抓取（六）—— 抓取点检测（抓取位姿估计） gqcnn代码测试与解读相关推荐

从零实现无序抓取（四）如何获取点云位姿估计真值
前言这是一章预告. 上一章在进行点云位姿估计时,我们利用PCL库对原始点云进行了一个人工设定的变换矩阵,这个变换矩阵可以作为位姿估计的真值.但是这种方式只能很简单地将某个原始点云整体进行变换.也就是 ...
目标检测YOLO实战应用案例100讲-基于YOLOv5的目标检测与6D位姿估计算法研究
目录前言传统的目标检测算法基于深度学习的目标检测算法传统的6D位姿估计算法
杜国光博士，基于视觉的机器人抓取--物体定位，位姿估计到抓取估计课堂笔记
基于视觉的机器人抓取--物体定位,位姿估计到抓取估计课堂笔记杜国光博士在智东西公开课上讲了<基于视觉的机器人抓取--物体定位,位姿估计到抓取估计>的精彩课程满满的干货,记下来,后面慢慢 ...
python爬虫电影资源_【Python爬虫】第十六次 xpath整站抓取阳光电影网电影资源
[Python爬虫]第十六次 xpath整站抓取阳光电影网电影资源# 一.解析电影url # 请求15题构造出的每个电影菜单的分页url,解析出每个电影url # 二.xpath解析电影资源 # 对第 ...
SilhoNet：一种用于3D对象位姿估计和抓取规划的RGB方法
论文题目:SilhoNet:An RGB Method for 3D Object Pose Estimation and Grasp Planning 论文地址:公众号「3D视觉工坊」后台回复「si ...
php抓取标签内的内容,php抓取网页中的内容
以下就是几种常用的用php抓取网页中的内容的方法. 1.file_get_contents PHP代码代码如下:>>>>>>>>>>> ...
抓取百度页面html,百度会抓取页面代码中的注释内容吗
百度会抓取页面代码中的注释内容吗内容导读:百度会抓取页面代码中的注释内容吗?答案是百度会抓取,但是在提取正文的时候会直接忽略掉,也就是说页面代码的注释内容不会影响到页面质量,所以大家尽管放心. 问题 ...
selenium抓取_使用Selenium的网络抓取电子商务网站
selenium抓取 In this article we will go through a web scraping process of an E-Commerce website. I hav ...
android 监听本机网络请求_fiddler如何抓取https请求实现fiddler手机抓包-证书安装失败100%解决...
一.HTTP协议和HTTPS协议. (1) HTTPS协议=HTTP协议+SSL协议,默认端口:443 (2) HTTP协议(HyperText Transfer Protocol):超文本传输协议. ...

机器人抓取（六）—— 抓取点检测（抓取位姿估计） gqcnn代码测试与解读