从零实现无序抓取（四）如何获取点云位姿估计真值

前言

这是一章预告。
上一章在进行点云位姿估计时，我们利用PCL库对原始点云进行了一个人工设定的变换矩阵，这个变换矩阵可以作为位姿估计的真值。但是这种方式只能很简单地将某个原始点云整体进行变换。也就是只能生成下图这种变化。

但是我们的实际的位姿估计算法不可能只在这种简单的场景下应用。一般来说，具体的抓取场景应该是有多个目标相互堆叠存在于场景中的。类似这样。

现实中我们可以利用点云相机采集这个场景的点云，然后估计目标的位姿，但是我们难以通过测量的方式得到每个零件的6DoF位姿，也就是说我们很难得到场景中目标的真值，没有真值我们就无法评价算法的误差，不能定量的评价误差，我们就没办法评估不同算法之间的精度，也就没办法发论文。那么，如何获得目标真值位姿呢？可以采用仿真的方式生成与实际场景类似的仿真数据。
我在这里采用了bullet物理仿真引擎进行了自由落体的仿真模拟。
先放上一段效果：

bullet自由落体仿真

然后是点云模拟的结果：

一、利用bullet进行自由落体仿真

Bullet是一个开源的物理模拟计算引擎，世界三大物理模拟引擎之一（另外两种是Havok和PhysX）。广泛应用于游戏开发和电影制作中。Bullet也是AMD开放物理计划成员之一。持Windows、Linux、MAC、Playstation3、XBOX360、Nintendo Wii。Bullet也整合到了Maya和Blender 3D中。

1.1 安装pybullet

现在Bullet已经可以很好的支持python了
安装方式简单粗暴直接
pip install pybullet
验证一下是否成功，打开命令行，输入

python -m pybullet_envs.examples.enjoy_TF_AntBulletEnv_v0_2017may

可以看见一只正在走路的四脚蜘蛛

1.2 导入模型

先看一个例子，如何初始化引擎并导入已有的模型。

import pybullet as p
import pybullet_data

# 连接引擎
_ = p.connect(p.GUI)
# 不展示GUI的套件
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)

# 添加资源路径
p.setAdditionalSearchPath(pybullet_data.getDataPath())

planeUid = p.loadURDF("plane.urdf", useMaximalCoordinates=True)  # 加载一个地面
trayUid = p.loadURDF("tray/traybox.urdf", basePosition=[0, 0, 0])  # 加载一个箱子，设置初始位置为（0，0，0）

# 开始渲染
while True:
    p.stepSimulation()

代码结果

代码不详细解释了，有兴趣可以去看看pybullet官网，或者建议看官方的教程进行入门学习。
链接：https://pan.baidu.com/s/1mwyAjckm1IR6F-I-trU_og
提取码：w2uh

1.3 urdf文件

代码中比较重要的部分，这两句是用来导入urdf文件的，这个文件需要自己编写，里面包含了你要加载的模型的视觉属性和碰撞属性。

planeUid = p.loadURDF("plane.urdf", useMaximalCoordinates=True)  # 加载一个地面
trayUid = p.loadURDF("tray/traybox.urdf", basePosition=[0, 0, 0])  # 加载一个箱子，设置初始

我们进入到bullet的资源文件路径里面找一下这两个文件，看看是什么样子

print(pybullet_data.getDataPath())

‘D:\Anaconda\Anaconda\envs\bullet\lib\site-packages\pybullet_data’
找到traybox.urdf文件，打开后可以看到

<robot name="tabletop">
  <link name="base_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0"/>
      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="tray_textured.obj" scale="5 5 5"/>
      </geometry>
      <material name="tray_material">
        <color rgba="1 1 1 1"/>
      </material>
    </visual>
     <collision>
      <origin rpy="0 0 0" xyz="0 0 0.005"/>
      <geometry>
	 			<box size=".6 .6 .02"/>
      </geometry>
    </collision>
     <collision>
      <origin rpy="0 0.575469961 0" xyz="0.25 0 0.059"/>
      <geometry>
	 			<box size=".02 .6 .15"/>
      </geometry>
    </collision>
		<collision>
      <origin rpy="0 -0.575469961 0" xyz="-0.25 0 0.059"/>
      <geometry>
	 			<box size=".02 .6 .15"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0.575469961 0 0" xyz="0 -0.25 0.059"/>
      <geometry>
	 			<box size=".6 .02 .15"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="-0.575469961 0 0" xyz="0 0.25 0.059"/>
      <geometry>
	 			<box size=".6 .02 .15"/>
      </geometry>
    </collision>

  </link>
</robot>

因为我们要导入的是一个单独的零件，所以不存在有多个link joint（不理解请返回阅读官方的教程），重点看

    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="tray_textured.obj" scale="5 5 5"/>
      </geometry>
      <material name="tray_material">
        <color rgba="1 1 1 1"/>
      </material>
    </visual>

这部分描述了物体的视觉属性，mesh filename就是我们自己的零件对应的.obj文件，如何画自己的.obj文件，可以参照我的另一篇从零实现无序抓取（二）制作属于自己的点云数据，此外可以这个里面还可以定义他的rgba颜色属性，还有material材料属性（非必须，这个文件就没有定义）。

然后另一个部分

    <collision>
      <origin rpy="-0.575469961 0 0" xyz="0 0.25 0.059"/>
      <geometry>
	 			<box size=".6 .02 .15"/>
      </geometry>
    </collision>

描述了模型的碰撞属性，官方这个文件是自己设置了一个box类型的碰撞属性。我们也可以设置成自己的obj，只需要把这一部分的内容和视觉属性部分保持一致就可以了。

1.4 制作自己的urdf文件

这里放上我自己写的一个urdf文件，part1.urdf

<robot name="blob000">
  <link name="random_obj_000">
    <contact>
      <lateral_friction value="1.0"/>
      <rolling_friction value="0.0"/>
      <inertia_scaling value="3.0"/>
      <contact_cfm value="0.0"/>
      <contact_erp value="1.0"/>
    </contact>
    <inertial>
      <origin rpy="0 0 0" xyz="0.6 0.05 0.4"/>
      <mass value="1"/>
      <inertia ixx="1" ixy="1" ixz="1" iyy="1" iyz="1" izz="1"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="part1.obj" scale="0.01 0.01 0.01"/>
      </geometry>
      <material name="blockmat">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="part1.obj" scale="0.01 0.01 0.01"/>
      </geometry>
    </collision>
  </link>
</robot>

这个文件就是将part1这个模型根据part1.obj进行了视觉属性和碰撞属性的定义，把这个模型加载后可以得到。

1.5 添加重力

现在只是把模型加载好，但是还没有设置重力，很简单

p.setGravity(0,0,-10)

把这句话加进去就行了。

最终的仿真效果，这里我同时加载了好几个模型，并让他们从不同的初始位置开始下落。

二、在bullet中采集RGB图和深度图

现在我们有了场景模型，那如何获得点云呢？bullet好像不能直接生成点云，他只能得到场景的点云图和深度图，后期的点云图需要用深度图合成出来。

width, height, rgbImg, depthImg, segImg 
= p.getCameraImage(width=1080, height=720,projectionMatrix=projectionMatrix)

获取深度图的方式也很简单，在渲染中加入这句话就行了。
返回的depthImg为numpy格式，自己把它保存下来就行了。

三、将深度图转换为点云图

有了深度图，我们还需要将深度图转换为点云数据，这里需要先获取相机的参数。

projectionMatrix = p.computeProjectionMatrixFOV(
    fov=50.0,               # 摄像头的视线夹角
    aspect=1.0,
    nearVal=0.001,            # 摄像头焦距下限
    farVal=20,               # 摄像头能看上限
)
print(projectionMatrix)

输出结果：
(2.1445069313049316, 0.0, 0.0, 0.0,
0.0, 2.1445069313049316, 0.0, 0.0,
0.0, 0.0, -1.0000998973846436, -1.0,
0.0, 0.0, -0.00200009997934103, 0.0)
这个矩阵就是openGL里面的投影矩阵了，不做详细解释了，可以自行去看看。
然后用深度图在这个矩阵上做变换，就可以得到点云数据了，这里我用matlab做了结果的可视化。