0.前置
- 机器人持续学习基准LIBERO系列1——基本介绍与安装测试
- 机器人持续学习基准LIBERO系列2——路径与基准基本信息
- 机器人持续学习基准LIBERO系列3——相机画面可视化及单步移动更新
- 机器人持续学习基准LIBERO系列4——robosuite最基本demo
- 机器人持续学习基准LIBERO系列5——获取显示深度图
- 机器人持续学习基准LIBERO系列6——获取并显示实际深度图
1.前置代码
- 机器人持续学习基准LIBERO系列6——获取并显示实际深度图
2.重新获取真实深度信息
- 之前的由于要显示，进行了整数化处理，所以重新获取一下原始真实深度信息
from robosuite.utils.camera_utils import get_real_depth_map
agentview_depth_real = get_real_depth_map(env.sim, agentview_depth)
3.获取图像尺寸
h,w = env_args['camera_heights'], env_args['camera_widths']
4.创建像素点序列和颜色序列
i = np.zeros([h*w,2])#(点数，像素点二维坐标）
colors = np.zeros([h*w,3])#(点数，像素点对应的RGB值)
for x in range(h):
for y in range(w):
i[x*h+y] = [x,y]
colors[x*h+y] = agentview_image[x,y]
5.获取相机内外参
- robosuite官方文档有对应函数get_camera_intrinsic_matrix,get_camera_extrinsic_matrix
from robosuite.utils.camera_utils import get_camera_extrinsic_matrix,get_camera_intrinsic_matrix
camera_intrinsic_matrix_ = np.linalg.inv(get_camera_intrinsic_matrix(env.sim,'agentview', env_args['camera_heights'], env_args['camera_widths']))
camera_extrinsic_matrix_ = np.linalg.inv(get_camera_extrinsic_matrix(env.sim,'agentview'))
6.计算世界坐标系下三维点坐标
- 相机内外参使用参考公式
points = np.zeros([i.shape[0],3])
for num,p in enumerate(i):
p_ = (camera_intrinsic_matrix_@np.array([[p[0],p[1],1]]).T).T
p_[0,2] = agentview_depth_real[int(p[0]),int(p[1])]
p_ = (camera_extrinsic_matrix_@np.array([p_[0,0],p_[0,1],p_[0,2],1]).T).T
points[num] = p_[:-1]
print(points)
7.关闭环境
env.close()
- 不关闭环境，就是用open3d显示的话，会报错
X Error of failed request: BadAccess (attempt to access private resource denied)
Major opcode of failed request: 152 (GLX)
Minor opcode of failed request: 5 (X_GLXMakeCurrent)
Serial number of failed request: 183
Current serial number in output stream: 183
8.open3d显示点云
import open3d as o3d
pcd_show = o3d.geometry.PointCloud()
pcd_show.points = o3d.utility.Vector3dVector(points[:, :3])
pcd_show.colors = o3d.utility.Vector3dVector(colors[:]/255)
o3d.visualization.draw_geometries([pcd_show])
