Windows下使用python realsense2获取点云全过程

---

前言

本文是在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]

需要注意的地方

1. 这里再记录一下points.get_texture_coordinates()返回的u,v的含义：
   https://github.com/IntelRealSense/librealsense/issues/3271

2. 官方生成点云的例程运行时可以通过d键控制深度图分辨率，有三类分辨率（160,120）、（320,240）、（640,480）

3. 整个例程总结：
   ① 获得深度图和彩色图
   ② 通过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二维渲染出来，这部分比较麻烦，直接用就好了

---

总结