利用深度图将其转化成点云数据

# ******************************************************************************
#  Copyright (c) 2023 Orbbec 3D Technology, Inc
#
#  Licensed under the Apache License, Version 2.0 (the "License");
#  you may not use this file except in compliance with the License.
#  You may obtain a copy of the License at
#
#      http:# www.apache.org/licenses/LICENSE-2.0
#
#  Unless required by applicable law or agreed to in writing, software
#  distributed under the License is distributed on an "AS IS" BASIS,
#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
#  See the License for the specific language governing permissions and
#  limitations under the License.
# ******************************************************************************
from pyorbbecsdk import Pipeline
from pyorbbecsdk import Config
from pyorbbecsdk import OBSensorType, OBFormat
from pyorbbecsdk import OBError
import cv2
import numpy as np
import open3d as o3d
from utils import frame_to_bgr_image
from ultralytics import YOLO
from ultralytics.utils import ops
ESC_KEY = 27
PRINT_INTERVAL = 1  # seconds
MIN_DEPTH = 500  # 20mm
MAX_DEPTH = 4000  # 10000mm


def depth2xyz(depth_map, depth_cam_matrix, flatten=False):
    fx, fy = depth_cam_matrix[0, 0], depth_cam_matrix[1, 1]
    cx, cy = depth_cam_matrix[0, 2], depth_cam_matrix[1, 2]
    h, w = np.mgrid[0:depth_map.shape[0], 0:depth_map.shape[1]]
    z = depth_map
    x = (w - cx) * z / fx
    y = (h - cy) * z / fy
    xyz = np.dstack((x, y, z)) if flatten == False else np.dstack((x, y, z)).reshape(-1, 3)
    # xyz=cv2.rgbd.depthTo3d(depth_map,depth_cam_matrix)
    return xyz


class TemporalFilter:
    def __init__(self, alpha):
        self.alpha = alpha
        self.previous_frame = None

    def process(self, frame):
        if self.previous_frame is None:
            result = frame
        else:
            result = cv2.addWeighted(frame, self.alpha, self.previous_frame, 1 - self.alpha, 0)
        self.previous_frame = result
        return result


def main():
    config = Config()
    pipeline = Pipeline()
    temporal_filter = TemporalFilter(alpha=0.5)
    try:
        profile_list = pipeline.get_stream_profile_list(OBSensorType.DEPTH_SENSOR)
        assert profile_list is not None
        try:
            depth_profile = profile_list.get_video_stream_profile(640, 0, OBFormat.Y16, 30)
        except OBError as e:
            print("Error: ", e)
            depth_profile = profile_list.get_default_video_stream_profile()
        assert depth_profile is not None
        print("depth profile: ", type(depth_profile))
        config.enable_stream(depth_profile)
    except Exception as e:
        print(e)
        return
    yolo = YOLO('./runs/segment/train10/weights/best.pt', task="segment")
    pipeline.start(config)
    camera_param = pipeline.get_camera_param()
    depth_cam_matrix = np.array([[camera_param.depth_intrinsic.fx, 0, camera_param.depth_intrinsic.cx],
                                 [0, camera_param.depth_intrinsic.fy, camera_param.depth_intrinsic.cy],
                                 [0, 0, 1]])
    while True:
        frames = pipeline.wait_for_frames(100)

        # 如果frames为None，则继续循环
        if frames is None:
            continue

        depth_frame = frames.get_depth_frame()
        color_frame = frames.get_color_frame()

        # 只有当frames、depth_frame和color_frame都不是None时，才跳出循环
        if depth_frame is not None and color_frame is not None:
            break
        # covert to RGB format
    width = depth_frame.get_width()
    height = depth_frame.get_height()
    scale = depth_frame.get_depth_scale()

    depth_data = np.frombuffer(depth_frame.get_data(), dtype=np.uint16)
    depth_data = depth_data.reshape((height, width))

    depth_data = depth_data.astype(np.float32) * scale
    depth_data = np.where((depth_data > MIN_DEPTH) & (depth_data < MAX_DEPTH), depth_data, 0)
    depth_data = depth_data.astype(np.uint16)
    # Apply temporal filtering
    depth_data = temporal_filter.process(depth_data)
    points = depth2xyz(depth_data, depth_cam_matrix, flatten=True)
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(points)
    pipeline.stop()


if __name__ == "__main__":
    main()