python - 如何使用Intel RealSense D435创建高度图

Question

对不起，我的英语不好，我也是新来的。

我有一个英特尔d435深度相机，一个覆盆子pi和一个diy rc风格的汽车，我在车上使用覆盆子pi来运行。我将aruco标记放置在房间的特定位置，并使用这些标记和车载摄像头获得位置反馈。我编写了一个脚本，使用aruco标记并使用标记的数据的预定位置将汽车移至所需位置。因此，使汽车从a行驶到b并不是问题(它沿着一条直线)。

但是现在我希望它从a点到b点避免静态障碍。

我刚刚了解了Astar alogorithm。
据我了解，有可能绘制从确定的起点到确定的终点的路径(通常来说)。

所以我想用我的intel d435相机生成我房间的高度图，并使用matplotlib使用3d散点图绘制它。我认为这是有可能的，因为d435可以输出点云数据。但是我的编程技巧很差并且找不到任何显示相似情况的示例。
然后可以计划到达一帧，从3d数组获取高度数据，运行A *，选择所需的节点并驾驶汽车直到到达目的地，然后重复直到到达目的地。

因此，总而言之，我的问题是如何使用3d数组和matplotlib散点图函数从intel d435绘制无纹理的点云。

import cv2                                
import numpy as np                       
import pyrealsense2 as rs
import matplotlib as mpl
import matplotlib.pyplot as plt          
import time
from mpl_toolkits.mplot3d import Axes3D
fig1 = plt.figure(1)
ar = fig1.gca(projection='3d')
ar.set_xlim(-1, 1)
ar.set_ylim(-1, 1)
ar.set_zlim(-1, 1)


# Configure depth and color streams
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)


# Start streaming
pipe_profile =pipeline.start(config)

points =[[[]]]
def get_3dPoints():#Function to be written
    pass    

try:
    while True:
        # Wait for a coherent pair of frames: depth and color
        frames = pipeline.wait_for_frames()
        depth_frame = frames.get_depth_frame()
        color_frame = frames.get_color_frame()

        if not depth_frame or not color_frame: #or not infrared_frame:
            continue
        # Convert images to numpy arrays
        depth_image = np.asanyarray(depth_frame.get_data())
        color_image = np.asanyarray(color_frame.get_data())

        cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
        colorizer = rs.colorizer()
        depth_colormap = np.asanyarray(colorizer.colorize(depth_frame).get_data())
        cv2.imshow('RealSense', depth_colormap)
        """
        points = get_3dPoints()
        tgt = ar.scatter(points, 'red')
        ar.set_xlabel('X - axis')
        ar.set_ylabel('Y - axis')
        ar.set_zlabel('Z - axis')
        plt.pause(0.00000000000000001)
        tgt.remove()
        """
        cv2.waitKey(1)

finally:

    # Stop streaming#img = cv2.resize(img, (1280, 720), interpolation=cv2.INTER_AREA)
    pipeline.stop()

Answer 1

查看此仓库有关如何获取热图link

官方的英特尔repo here中也有很多资源