基于 PythonOpen3D的完整实现方案专门适配双目相机输出的深度图 点云流程包含点云预处理 → 物体分割 → 3D 包围盒计算 → 抓取姿态生成 → 可视化验证。整体流程工程标准双目深度图 → 生成 / 加载点云地面移除 去噪 聚类分割出目标物体生成定向 3D 包围盒Oriented Bounding Box, OBB基于包围盒计算稳定抓取姿态6D 位姿位置 方向输出可用于机械臂的抓取点 旋转矩阵 / 四元数完整可运行代码依赖安装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_size0.005) # 降采样提速 return pcd # 3. 点云预处理去地面去噪 def preprocess_pointcloud(pcd): # 1. 去噪 pcd, _ pcd.remove_statistical_outlier(nb_neighbors20, std_ratio1.0) # 2. 分割地面RANSAC平面拟合 plane_model, inliers pcd.segment_plane( distance_threshold0.01, ransac_n3, num_iterations1000 ) object_pcd pcd.select_by_index(inliers, invertTrue) # 保留非地面点 return object_pcd # 4. 物体聚类 3D包围盒 def get_object_3d_bbox(object_pcd): 聚类提取最大物体 → 计算最优3D包围盒 :return: 最大物体点云, OBB包围盒, 包围盒中心点 # 欧式聚类分割物体 labels np.array(object_pcd.cluster_dbscan(eps0.02, min_points10)) 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(size0.1) # 抓取姿态坐标系 grasp_coord o3d.geometry.TriangleMesh.create_coordinate_frame(size0.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. 抓取姿态规则最稳定这套方案采用工业机器人通用抓取逻辑抓取点物体 3D 包围盒几何中心抓取方向包围盒最短边法向两指夹取最稳姿态夹爪垂直于物体表面无倾斜输出结果直接给机械臂用最终输出6D 抓取姿态3D 位置grasp_pos→ XYZ 世界 / 相机坐标3D 方向grasp_rot旋转矩阵控制夹爪朝向grasp_quat四元数ROS / 机械臂控制器标准格式进阶优化可选叠加 RGB 图把 RGB 颜色赋值给点云分割更精准多物体抓取遍历所有聚类结果生成多个抓取姿态碰撞检测检查抓取姿态是否与桌面 / 其他物体碰撞最优抓取评分根据稳定性、可达性排序抓取姿态总结这套代码直接适配双目相机输入深度图就能输出 3D 包围盒 6D 抓取姿态用定向包围盒 (OBB)比轴对齐盒更贴合物体抓取更稳定输出格式是机械臂标准格式可直接下发控制器执行抓取只需修改相机内参和深度图路径就能跑通