双目相机 深度图和点云生成物体3D包围盒 生成抓取姿态

基于 Python+Open3D 的完整实现方案，专门适配双目相机输出的深度图 + 点云，流程包含：点云预处理 → 物体分割 → 3D 包围盒计算 → 抓取姿态生成 → 可视化验证。

整体流程（工程标准）

1. 双目深度图 → 生成 / 加载点云
2. 地面移除 + 去噪 + 聚类分割出目标物体
3. 生成定向 3D 包围盒（Oriented Bounding Box, OBB）
4. 基于包围盒计算稳定抓取姿态（6D 位姿：位置 + 方向）
5. 输出可用于机械臂的抓取点 + 旋转矩阵 / 四元数

---

完整可运行代码

依赖安装

bash

运行

pip install open3d numpy opencv-python scipy

主代码（直接用）

python

运行

import numpy as np
import open3d as o3d
import cv2
from scipy.spatial.transform import Rotation as R

# ====================== 1. 基础配置 ======================
# 双目相机内参（替换成你的相机真实内参）
CAMERA_INTRINSIC = {
    "fx": 600.0,
    "fy": 600.0,
    "cx": 320.0,
    "cy": 240.0,
}
# 深度图有效范围（米）
MIN_DEPTH = 0.2
MAX_DEPTH = 1.5

# ====================== 2. 深度图 → 点云 ======================
def depth2pointcloud(depth_img, intrinsic):
    """
    双目深度图 转 彩色点云（若有RGB图可叠加）
    :param depth_img: 双目输出的深度图 (H, W)，单位：米
    :param intrinsic: 相机内参
    :return: o3d.geometry.PointCloud
    """
    h, w = depth_img.shape
    xyz = []
    
    for v in range(h):
        for u in range(w):
            d = depth_img[v, u]
            if d < MIN_DEPTH or d > MAX_DEPTH:
                continue
            # 像素坐标 → 相机坐标系3D点
            x = (u - intrinsic["cx"]) * d / intrinsic["fx"]
            y = (v - intrinsic["cy"]) * d / intrinsic["fy"]
            z = d
            xyz.append([x, y, z])
    
    xyz = np.array(xyz)
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(xyz)
    pcd = pcd.voxel_down_sample(voxel_size=0.005)  # 降采样提速
    return pcd

# ====================== 3. 点云预处理：去地面+去噪 ======================
def preprocess_pointcloud(pcd):
    # 1. 去噪
    pcd, _ = pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=1.0)
    
    # 2. 分割地面（RANSAC平面拟合）
    plane_model, inliers = pcd.segment_plane(
        distance_threshold=0.01, ransac_n=3, num_iterations=1000
    )
    object_pcd = pcd.select_by_index(inliers, invert=True)  # 保留非地面点
    return object_pcd

# ====================== 4. 物体聚类 + 3D包围盒 ======================
def get_object_3d_bbox(object_pcd):
    """
    聚类提取最大物体 → 计算最优3D包围盒
    :return: 最大物体点云, OBB包围盒, 包围盒中心点
    """
    # 欧式聚类分割物体
    labels = np.array(object_pcd.cluster_dbscan(eps=0.02, min_points=10))
    max_label = labels.max()
    
    # 取点数最多的物体
    object_indices = np.where(labels == 0)[0] if max_label < 0 else np.where(labels == np.argmax(np.bincount(labels[labels >= 0])))[0]
    target_pcd = object_pcd.select_by_index(object_indices)
    
    # 计算 定向包围盒 OBB（最佳拟合，比轴对齐AABB更适合抓取）
    obb = target_pcd.get_oriented_bounding_box()
    center = obb.center  # 包围盒中心 (x,y,z)
    return target_pcd, obb, center

# ====================== 5. 生成抓取姿态（核心！） ======================
def generate_grasp_pose(obb, center):
    """
    基于3D包围盒生成机械臂抓取姿态
    规则：
    1. 抓取点 = 包围盒中心
    2. 抓取方向 = 包围盒最短边法向（最稳定夹持）
    3. 末端姿态垂直于夹持方向
    :return: grasp_pos (抓取点), grasp_rot (旋转矩阵), grasp_quat (四元数)
    """
    # 包围盒三个主轴（单位向量）
    axes = obb.R
    extents = obb.extent  # 包围盒长宽高 (w,h,d)
    
    # 找到最短轴 → 作为夹持方向（两指夹取）
    min_axis_idx = np.argmin(extents)
    grasp_dir = axes[:, min_axis_idx]  # 夹持方向
    
    # 构建末端执行器姿态
    z_axis = grasp_dir  # 夹爪闭合方向
    x_axis = np.array([1, 0, 0])
    if abs(np.dot(x_axis, z_axis)) > 0.9:
        x_axis = np.array([0, 1, 0])
    y_axis = np.cross(z_axis, x_axis)
    x_axis = np.cross(y_axis, z_axis)
    
    # 正交化旋转矩阵
    rot_mat = np.vstack([x_axis, y_axis, z_axis]).T
    rot_mat = o3d.geometry.get_rotation_matrix_from_xyz(
        R.from_matrix(rot_mat).as_euler('xyz')
    )
    
    # 转四元数（机械臂常用）
    quat = R.from_matrix(rot_mat).as_quat()  # [x,y,z,w]
    
    return center, rot_mat, quat

# ====================== 6. 可视化 ======================
def visualize(pcd, obb, grasp_pos, grasp_rot):
    coord = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.1)
    # 抓取姿态坐标系
    grasp_coord = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.05)
    grasp_coord.translate(grasp_pos)
    grasp_coord.rotate(grasp_rot)
    
    o3d.visualization.draw_geometries([
        pcd, obb, coord, grasp_coord
    ])

# ====================== 主函数 ======================
if __name__ == "__main__":
    # ========== 输入：双目深度图 ==========
    depth_image = cv2.imread("depth.png", cv2.IMREAD_UNCHANGED)  # 加载双目深度图
    depth_image = depth_image.astype(np.float32) / 1000.0  # 若单位是毫米 → 转米
    
    # 1. 深度图 → 点云
    pcd = depth2pointcloud(depth_image, CAMERA_INTRINSIC)
    
    # 2. 预处理
    object_pcd = preprocess_pointcloud(pcd)
    
    # 3. 获取3D包围盒
    target_pcd, obb, center = get_object_3d_bbox(object_pcd)
    
    # 4. 生成抓取姿态
    grasp_pos, grasp_rot, grasp_quat = generate_grasp_pose(obb, center)
    
    # 5. 输出结果
    print("===== 3D包围盒信息 =====")
    print(f"包围盒中心: {center.round(4)}")
    print(f"包围盒尺寸(宽高深): {obb.extent.round(4)}")
    
    print("\n===== 抓取姿态 =====")
    print(f"抓取位置 (x,y,z): {grasp_pos.round(4)} 米")
    print(f"旋转矩阵:\n{grasp_rot.round(4)}")
    print(f"四元数 [x,y,z,w]: {grasp_quat.round(4)}")
    
    # 可视化
    visualize(target_pcd, obb, grasp_pos, grasp_rot)

---

关键模块说明（你必须改的部分）

1. 相机内参

把代码里的CAMERA_INTRINSIC换成你双目相机标定后的真实内参，否则点云坐标完全错误。

2. 深度图输入

• 双目相机输出的深度图一般是16 位单通道图，单位毫米
• 代码中已做/1000转米处理
• 直接替换cv2.imread("depth.png")为你的深度图路径

3. 抓取姿态规则（最稳定）

这套方案采用工业机器人通用抓取逻辑：

1. 抓取点：物体 3D 包围盒几何中心
2. 抓取方向：包围盒最短边法向（两指夹取最稳）
3. 姿态：夹爪垂直于物体表面，无倾斜

---

输出结果（直接给机械臂用）

最终输出6D 抓取姿态：

1. 3D 位置 ：grasp_pos → XYZ 世界 / 相机坐标
2. 3D 方向 ：
   • grasp_rot：旋转矩阵（控制夹爪朝向）
   • grasp_quat：四元数（ROS / 机械臂控制器标准格式）

---

进阶优化（可选）

1. 叠加 RGB 图：把 RGB 颜色赋值给点云，分割更精准
2. 多物体抓取：遍历所有聚类结果，生成多个抓取姿态
3. 碰撞检测：检查抓取姿态是否与桌面 / 其他物体碰撞
4. 最优抓取评分：根据稳定性、可达性排序抓取姿态

---

总结

1. 这套代码直接适配双目相机，输入深度图就能输出 3D 包围盒 + 6D 抓取姿态
2. 用定向包围盒 (OBB) 比轴对齐盒更贴合物体，抓取更稳定
3. 输出格式是机械臂标准格式，可直接下发控制器执行抓取
4. 只需修改相机内参 和深度图路径就能跑通