pybullet杂谈 :使用深度学习拟合相机坐标系与世界坐标系坐标变换关系(一)

211
0
2021年1月26日 13时00分

在开始之前,先送上本篇文章的彩蛋——我竟然在pybullet里放烟花!!!
声明:彩蛋与文章主要内容无关,纯粹图个开心。

 

pybullet杂谈 :使用深度学习拟合相机坐标系与世界坐标系坐标变换关系(一)插图

一、前言

 

在进行机械臂抓取的时候,需要获得相机坐标系与世界坐标系的转换关系,也就是需要进行相机标定。而在pybullet中,这项工作少有资料,不过还是有三个思路可供选择:

1)在使用getCameraImage()函数的时候,已经指定了相机的一些参数,所以应该可以直接使用这些参数来计算,只需要找到公式即可;

 

2)在pybullet_data文件夹中,有提供标定板,所以理论上是可以进行标定操作的,感兴趣的小伙伴可以尝试做一下;

 

3)在仿真中,我们不仅可以获取相机中的坐标,还可以人为指定世界坐标系中的坐标,所以理论上这种模式是非常适合使用深度学习进行训练的:相机中的坐标作为输入,模型预测世界坐标系的坐标,编程指定的世界坐标系中的坐标作为标签。这样就可以构成完整的训练资料。

 

因为我的研究方向是深度强化学习,所以自然倾向于第三种方法,如果有小伙伴使用其他方法操作成功,也麻烦告知我一下,或者写个技术博客,一同学习。

 

下面我们就具体来实现这个深度学习训练模型。(假设抓取的是小的四方体)
下文会用到很多pybullet提供的函数,文中不会解释函数的用法,所有的用法都可以从pybullet的文档中查看,文档地址:pybullet quickstart guide

 

二、获取世界坐标系坐标数据

 

首先需要准备的是可供训练的数据:即相机坐标系坐标数据和世界坐标系坐标数据。世界坐标系的坐标在获取数据阶段,我们需要编程指定,是为了获取对应的相机坐标系的坐标数据。

 

指定世界坐标系坐标是非常简单的,我们只需要让四方体沿着x和y方向遍历移动,就可以获得很多的坐标数据。在这里,我们模拟抓取时候的场景,将四方体放在一个托盘中,如图所示:

 

pybullet杂谈 :使用深度学习拟合相机坐标系与世界坐标系坐标变换关系(一)插图(1)

 

代码就非常简单了。首先将托盘加载到环境中来,并且更改以下外表颜色。为什么要更改外表颜色呢?这个地方留个悬念,来不及解释了,到目的地再解释。然后定义好托盘的中心位置和到边缘的距离。

 

TRAY_CENTER_X_POSITION=0.7  #为什么x中心在0.7呢,为什么不是0?因为抓取的时候机械臂在0
TRAY_CENTER_Y_POSITION=0.0
TRAY_CENTER_Z_POSITION=0.0
HALF_LENGTH=0.2
trayUid=p.loadURDF("tray/traybox.urdf", [TRAY_CENTER_X_POSITION, 0, 0]) 
p.changeVisualShape(trayUid,-1,rgbaColor=[0.1,0.1,0.1,1])

 

至于小伙伴会说,托盘中心位置好确定,那到边缘的距离怎么确定呢?

 

1)直接去看urdf的源码,看一下是怎样定义尺寸的。但是托盘的urdf中定义的是斜面上的尺寸,不是底面的尺寸,而我们的小四方体是放置在底面的,所以不是很直观;

 

2)又来一些骚操作,pybullet有一个很好用的函数addUserDebugLine(),这个函数可以添加一些线条,添加到你想要的位置。所以思路就是我们让这个线条竖直放置,然后让线条和托盘的边缘重合,看一下当前的坐标,就可以获得托盘底面的长度了。代码如下:

 

# The following code's function is to determine the edge of the box
# So now the box's size is 0.4x0.4 (m)
p.addUserDebugLine([0.7,0,0],[0.7,0,1])
p.addUserDebugLine([0.7+0.2,0,0],[0.7+0.2,0,1],[0,0,0])
p.addUserDebugLine([0.7,0.2,0],[0.7,0.2,1],[1,0,0])

 

如果对函数的参数困惑,函数的说明如下图:

 

pybullet杂谈 :使用深度学习拟合相机坐标系与世界坐标系坐标变换关系(一)插图(2)

 

修改参数的数值试几次,就可以确定托盘底面的尺寸大概是0.4×0.4(m),这也就是上面HALF_LENGTH=0.2这条代码的由来。

 

然后我们就让小四方体在托盘底部沿着x和y方向(也就是平面)遍历:

 

camera_coor_list,real_coor_list=[],[]   #coordination in camera and real
for i in arange(TRAY_CENTER_X_POSITION-HALF_LENGTH,TRAY_CENTER_X_POSITION+HALF_LENGTH,0.01):
    for j in arange(TRAY_CENTER_Y_POSITION-HALF_LENGTH,TRAY_CENTER_Y_POSITION+HALF_LENGTH,0.01):
        cube_small_Uid=p.loadURDF("cube_small.urdf", [i, j ,0])
        real_coor_list.append((i,j))  #add coordination to list
        camera_coor_list.append(recognize_rectangle_center_point())               
        p.removeBody(cube_small_Uid)

 

这段代码有几个值得注意的地方:

 

1)小四方体每次移动的距离是0.01m;

 

2) camera_coor_list.append(recognize_rectangle_center_point())这句的作用是将相机坐标系的坐标添加到camera_coor_list中,后面我们再说;

 

3)p.removeBody(cube_small_Uid)作用是删除当前位置的小四方体,否则添加过的物体是不会消失的,这样造成的效果就是下面这样,不过也确实很有趣呢!

 

pybullet杂谈 :使用深度学习拟合相机坐标系与世界坐标系坐标变换关系(一)插图(3)
pybullet杂谈 :使用深度学习拟合相机坐标系与世界坐标系坐标变换关系(一)插图

 

至此,世界坐标系的坐标数据就存到real_coor_list这个列表中了。

 

三、获取相机坐标系坐标数据

 

这是这部分要达到的目标:

 

pybullet杂谈 :使用深度学习拟合相机坐标系与世界坐标系坐标变换关系(一)插图(4)

 

pybullet杂谈 :使用深度学习拟合相机坐标系与世界坐标系坐标变换关系(一)插图(5)

 

相机坐标系坐标的获取稍微麻烦些,但是好在我们假设抓取的是规则的小四方体,对于识别四方体中心坐标还是很简单的。

 

首先呢,需要在pybullet中获取相机的图像,同时需要定义一些相机的参数:

 

# some camera parameters
width = 720
height = 720

fov = 60
aspect = width / height
near = 0.02
far = 1

CAMERA_X_POSITIOH=TRAY_CENTER_X_POSITION
CAMERA_Z_POSITION=0.1
CAMERA_Y_POSITION=0

view_matrix = p.computeViewMatrix([0.7, 0, 0.6], [0.7, 0, 0], [1, 0, 0])
projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far)

images = p.getCameraImage(width,
                          height,
                          view_matrix,
                          projection_matrix,
                          shadow=True,
                          lightDirection=[CAMERA_X_POSITIOH,CAMERA_Y_POSITION,CAMERA_Z_POSITION],
                          renderer=p.ER_BULLET_HARDWARE_OPENGL)

 

这些参数的设置和获取图像的写法,可以借鉴pybullet的example。pybullet良心的地方在于,不仅文档即为全面,example也即为全面,我们来看一下文档和example的面目,实则业界良心了:

 

pybullet杂谈 :使用深度学习拟合相机坐标系与世界坐标系坐标变换关系(一)插图(6)

 

pybullet杂谈 :使用深度学习拟合相机坐标系与世界坐标系坐标变换关系(一)插图(7)

 

获取到图像后,我们需要确定四方体中心的坐标,这就请出大名鼎鼎的OpenCV出场了。原理就是使用OpenCV的findContours()函数确定轮廓,然后确定质心的坐标,因为是质量分布均匀的四方体(正方体),可以认为质心的坐标就是形心的坐标。

 

稍微停一下,还记得前面留了一个疑问吗?为什么要将托盘的颜色调成近乎黑色呢?答案就在这里啊,因为调成近乎黑色后,OpenCV的轮廓识别更加稳定和正确,如果使用托盘本来的样子,识别稳定性是不能保证的,可以实验看一下。
代码如下:

 

    img_for_opencv=images[2]
    gray = cv2.cvtColor(img_for_opencv, cv2.COLOR_BGR2GRAY)
    blurred = cv2.GaussianBlur(gray, (5, 5), 0)
    thresh = cv2.threshold(blurred, 60, 255, cv2.THRESH_BINARY)[1]

    cnts = cv2.findContours(thresh.copy(), cv2.RETR_EXTERNAL,
        cv2.CHAIN_APPROX_SIMPLE)

    cnts = imutils.grab_contours(cnts)
    for c in cnts:
        # compute the center of the contour
        if cv2.contourArea(c) > 10:
            M = cv2.moments(c)
            cX=int(M["m10"]/M["m00"])
            cY=int(M["m01"]/M["m00"])
            # draw the contour and center of the shape on the image
            cv2.drawContours(img_for_opencv, [c], -1, (0, 255, 0), 2)
            cv2.circle(img_for_opencv, (cX, cY), 7, (255, 255, 255), -1)
            print('cx,cy=',cX,cY)
            cv2.putText(img_for_opencv, "center", (cX - 20, cY - 20),
                cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)
            # show the image
            cv2.imshow("Image", img_for_opencv)
            cv2.waitKey(1)

四、整合代码

 

上面说了挺多的代码,只是讲了每个部分该怎么实现,那么怎么整合起来让其正常的工作呢?下面我们来一步步展开。
首先看一下代码结构:

 

pybullet杂谈 :使用深度学习拟合相机坐标系与世界坐标系坐标变换关系(一)插图(8)

 

src文件夹是顶层文件夹,train-coor-network是主文件夹,utils文件夹是一些可复用的组件。train-coor-network文件夹下的get_coor.py文件是上面所讲内容的代码,utils文件夹下的recognize_rectangle.py文件是识别四方体中心位置的代码,因为可复用,将其抽象后放到utils文件夹。

 

get_coor.py文件内容如下:

 

# -*- coding: utf-8 -*-

#!/usr/bin/env python3

# get_coor.py

"""
Created on Mon Jan 11 14:48:13 2021

@author: dell
"""
import sys
sys.path.append('../')

import pybullet as p
import pybullet_data as pd
from numpy import arange
import pickle
from utils.recognize_rectangle import recognize_rectangle_center_point



camera_coor_pickle='camera_coor_saved_file_copy'
real_coor_pickle='real_coor_saved_file_copy'

TRAY_CENTER_X_POSITION=0.7
TRAY_CENTER_Y_POSITION=0.0
TRAY_CENTER_Z_POSITION=0.0
HALF_LENGTH=0.2

p.connect(p.GUI)
p.setAdditionalSearchPath(pd.getDataPath())


p.resetDebugVisualizerCamera(cameraDistance=1.7,cameraYaw=0,\
                                     cameraPitch=-40,cameraTargetPosition=[0,-0.35,0.7])

p.setGravity(0,0,-9.8)


trayUid=p.loadURDF("tray/traybox.urdf", [TRAY_CENTER_X_POSITION, 0, 0]) 
p.changeVisualShape(trayUid,-1,rgbaColor=[0.1,0.1,0.1,1])

camera_coor_list,real_coor_list=[],[]   #coordination in camera and real

for i in arange(TRAY_CENTER_X_POSITION-HALF_LENGTH,TRAY_CENTER_X_POSITION+HALF_LENGTH,0.01):
    for j in arange(TRAY_CENTER_Y_POSITION-HALF_LENGTH,TRAY_CENTER_Y_POSITION+HALF_LENGTH,0.01):
        cube_small_Uid=p.loadURDF("cube_small.urdf", [i, j ,0])
        real_coor_list.append((i,j))  #add coordination to list
# =============================================================================
#         p.changeVisualShape(cube_small_Uid,-1,rgbaColor=[random.uniform(0,1), \
#                                                      random.uniform(0,1), \
#                                                      random.uniform(0,1), \
#                                                      1])
# =============================================================================


        camera_coor_list.append(recognize_rectangle_center_point())               
        p.removeBody(cube_small_Uid)

camera_coor_file=open(camera_coor_pickle,'wb')
real_coor_file=open(real_coor_pickle,'wb')

pickle.dump(camera_coor_list,camera_coor_file)
camera_coor_file.close()
pickle.dump(real_coor_list,real_coor_file)
real_coor_file.close()

# =============================================================================
# The following code's function is to determine the edge of the box
# So now the box's size is 0.4x0.4 (m)
# p.addUserDebugLine([0.7,0,0],[0.7,0,1])
# p.addUserDebugLine([0.7+0.2,0,0],[0.7+0.2,0,1],[0,0,0])
# p.addUserDebugLine([0.7,0.2,0],[0.7,0.2,1],[1,0,0])
# =============================================================================
while True:
    p.stepSimulation()

 

So easy 哈,主要内容上面都已经说过。camera_coor_list.append(recognize_rectangle_center_point())这句代码调用utils中的recognize_rectangle.py文件实现获取相机中的坐标。我们看一下这个文件怎么写:
recognize_rectangle.py

 

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

# recognize_rectangle.py

"""
Created on Wed Jan 20 21:18:19 2021

@author: dell
"""
import pybullet as p
import cv2 
import imutils


TRAY_CENTER_X_POSITION=0.7
TRAY_CENTER_Y_POSITION=0.0
TRAY_CENTER_Z_POSITION=0.0
HALF_LENGTH=0.2
# some camera parameters
width = 720
height = 720

fov = 60
aspect = width / height
near = 0.02
far = 1

CAMERA_X_POSITIOH=TRAY_CENTER_X_POSITION
CAMERA_Z_POSITION=0.1
CAMERA_Y_POSITION=0


view_matrix = p.computeViewMatrix([0.7, 0, 0.6], [0.7, 0, 0], [1, 0, 0])
projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far)


def recognize_rectangle_center_point():
    images = p.getCameraImage(width,
                          height,
                          view_matrix,
                          projection_matrix,
                          shadow=True,
                          lightDirection=[CAMERA_X_POSITIOH,CAMERA_Y_POSITION,CAMERA_Z_POSITION],
                          renderer=p.ER_BULLET_HARDWARE_OPENGL)


    img_for_opencv=images[2]
    gray = cv2.cvtColor(img_for_opencv, cv2.COLOR_BGR2GRAY)
    blurred = cv2.GaussianBlur(gray, (5, 5), 0)
    thresh = cv2.threshold(blurred, 60, 255, cv2.THRESH_BINARY)[1]

    cnts = cv2.findContours(thresh.copy(), cv2.RETR_EXTERNAL,
        cv2.CHAIN_APPROX_SIMPLE)

    cnts = imutils.grab_contours(cnts)
    for c in cnts:
        # compute the center of the contour
        if cv2.contourArea(c) > 10:
            M = cv2.moments(c)
            cX=int(M["m10"]/M["m00"])
            cY=int(M["m01"]/M["m00"])
            # draw the contour and center of the shape on the image
            cv2.drawContours(img_for_opencv, [c], -1, (0, 255, 0), 2)
            cv2.circle(img_for_opencv, (cX, cY), 7, (255, 255, 255), -1)
            print('cx,cy=',cX,cY)
            cv2.putText(img_for_opencv, "center", (cX - 20, cY - 20),
                cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)
            # show the image
            cv2.imshow("Image", img_for_opencv)
            cv2.waitKey(1)

    return (cX,cY)

 

也是so easy。OpenCV识别四方体确定坐标的代码可以看一下OpenCV的教程。此外,get_coor.py文件中还有部分代码是关于将数据保存到pickle文件中,这个不算重点,但是必须要有。关于python对数据的处理、存储的更多的知识,大家可以参考《利用python进行数据分析》这本书,关于pickle、CSV 、MessagePack、HDF5、Feather、Parquet 等不用存储格式之间的区别,可以参考这篇文章The Best Format to Save Pandas Data

 


五、总结

 

好了,这就是本篇的全部内容,主要就是获取数据,深度学习训练的内容我们留到下节讲,写了这些markdown模式已经卡的不要不要的了,写不动了。下节就是训练和推断了。

 

pybullet杂谈 :使用深度学习拟合相机坐标系与世界坐标系坐标变换关系(一)插图(9)

 

发表评论

后才能评论