前言
本文是在windows下安装ros成功,继续装realsense-ros失败后的办法,使用pyrealsense2,直接使用python生成和可视化点云。
其实realsense2官方给了一些基础的python例程,如获取深度图、彩色图,对齐深度图与彩色图,Opencv可视化点云,Pyglet可视化点云等,但这些例子代码加上可视化后比较复杂,这里记录一下自己的代码得到点云的过程。贴官网链接。
一、获取realsense2内参
内参是相机求取三维坐标的一个重要参数,其构成了相机模型,这里不赘述原理,简单理解,通过针孔相机模型,其构成了相机坐标系到像素坐标系之间的转换,至于相机坐标系到世界坐标系,就是外参(平移、旋转)的事了。
除此之外,对于RGB-D相机,除了彩色相机有内参外,深度相机也有,那我们应该用哪个呢?答案是彩色相机。因为在获取点云之前,通常需要将深度图与彩色图配准,即将深度相机坐标系转换到彩色相机坐标系下,那么转换之后就使用的是彩色相机下的针孔模型了。话不多说贴代码:
import pyrealsense2 as rs
import numpy as np
import cv2
# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()
# Start streaming
pipeline.start(config)
# 创建对齐对象(深度对齐颜色)
align = rs.align(rs.stream.color)
try:
while True:
# # Wait for a coherent pair of frames: depth and color
frames = pipeline.wait_for_frames()
# 对齐后再获取
aligned_frames = align.process(frames)
aligned_depth_frame = aligned_frames.get_depth_frame()
color_frame = aligned_frames.get_color_frame()
# 获取颜色帧内参
color_profile = color_frame.get_profile()
cvsprofile = rs.video_stream_profile(color_profile)
color_intrin = cvsprofile.get_intrinsics()
color_intrin_part = [color_intrin.ppx, color_intrin.ppy, color_intrin.fx, color_intrin.fy]
print(color_intrin_part)
finally:
pipeline.stop()
二、Bonus:align与not_align的区别
截断距离为1m,即1m外的抹去。效果还是很明显的
三、点云生成与可视化
点云生成
这里会使用graspnetAPI中的函数【从深度图生成点云的函数,也可自己用针孔模型的公式写一写】,有兴趣的可以pip下载或github上找源码。
用graspnetAPI的画是用Open3D做可视化,我不太会用,单纯的o3d.visualization.draw_geometries([cloud])效果比较很差,根本看不出形状,而且还不知道怎么增添颜色信息,还是从官方给的生成点云的代码里抠出来用吧
这部分参考:
结合官方给出的例程,想得到点云很简单,但可视化比较麻烦。这里需要注意的是,上面链接以及官方opencv_pointcloud_viewer.py的例程是没有进行配准的,这一点也可截断距离来观察,如下:
这里和 二、 中不同,这里是点云了,上面还是像素。
点云滤波
这里主要为了方便之后的用于抓取的点云场景滤波,主要指固定深度点云的去除与固定颜色点云的去处。(这里发现点云的存储顺序是和彩色图相同的,如点云为(307200,3)彩色图为(480,640,3),将彩色图reshape为(307200,3)后,其与点云的点是一一对应的)
部分代码如下:
color_image_ravel = color_image.reshape(-1,3)
# print(color_image_ravel.shape)
index_color = ((color_image_ravel[:,0] + color_image_ravel[:,1] + color_image_ravel[:,2]) < 170)
depth_colormap = np.asanyarray(
colorizer.colorize(aligned_depth_frame).get_data())
if state.color: # 即绘制点云时包含彩色图
mapped_frame, color_source = aligned_depth_frame, color_image
else:
mapped_frame, color_source = aligned_depth_frame, depth_colormap
pc.map_to(mapped_frame) # pyrealsense2.pyrealsense2.pointcloud
points = pc.calculate(aligned_depth_frame) # pyrealsense2.pyrealsense2.points points.get_vertices() && points.get_texture_coordinates()
v, t = points.get_vertices(), points.get_texture_coordinates() # pyrealsense2.pyrealsense2.BufData
verts = np.asanyarray(v).view(np.float32).reshape(-1, 3)
verts = verts[index_color]
index_depth = verts[:, 2] < 1
verts = verts[index_depth]
print(verts.shape)
texcoords = np.asanyarray(t).view(np.float32).reshape(-1, 2)
texcoords = texcoords[index_color]
texcoords = texcoords[index_depth]
需要注意的地方
-
这里再记录一下points.get_texture_coordinates()返回的u,v的含义:
https://github.com/IntelRealSense/librealsense/issues/3271
-
官方生成点云的例程运行时可以通过d键控制深度图分辨率,有三类分辨率(160,120)、(320,240)、(640,480)
-
整个例程总结:
① 获得深度图和彩色图
② 通过realsense2库函数计算points,但这里的points仍是pyrealsense2.pyrealsense2.points类型,非numpy.array
③ points.get_vertices(),points.get_texture_coordinates()得到点和点对应的像素坐标(pyrealsense2.pyrealsense2.BufData),转换成numpy,就成了(xxxx, 3)和(xxxx,2)的样子了,注意texture_coordinates范围为0-1,如上面的issue中说的,应该是做了相邻像素的插值
④ 可视化。将三维点用opencv二维渲染出来,这部分比较麻烦,直接用就好了