生成场景碰撞标签

[kiwiwan]

首先感谢你们的杰出工作！
我尝试用数据集里的grasp_label及场景0000来生成场景的collision_label。原本我的想法是直接将整个场景所有物体的点级抓取构建为一个graspGroup，然后调用ModelFreeCollisionDetector函数得到colllision_mask，即为collision_labels。但是当我只将一个物体（id为0）的所有抓取构建为graspGroup，然后调用ModelFreeCollisionDetector函数时，遇到了内存不够的情况：
collision_detector.py", line 75, in detect targets = self.scene_points[np.newaxis,:,:] - T[:,np.newaxis,:] numpy.core._exceptions.MemoryError: Unable to allocate 19.5 TiB for an array with shape (49809600, 17901, 3) and data type float64
然后我将grasp_label里collision为True的抓取排除，都还剩下4332493个抓取，检测时还是会报超内存。这时我只能将这个graspGroup切片，每次50000左右，然后分别检测碰撞，这样会很慢，检测完一个物体都需要7，8个小时。
你们在生成场景的collision_label时，会碰到这样的问题吗？你们是怎么处理的呢？
另外，你们在生成场景的collision_label时ModelFreeCollisionDetector函数里场景点云的voxel_size设为多少呢？approach_dist，collision_thresh, empty_thresh，finger_width，finger_length等这些参数设为多少呢？

[Fang-Haoshu]

你好，是的，生成碰撞标签是很耗时的一项工作，我们在服务器上跑了很久。你使用的话应该直接用我们release的collision标签即可。

[kiwiwan]

感谢您的答复！
我这边想在您提供的数据集上，针对我的应用场景添加几个特定的物体，比如门把手之类的，以提高相应的成功率。
您能否指点一下，该怎么做才能加快这个过程呢？我这样做的速度基本是没法生成场景级的碰撞的，太慢了。

[Fang-Haoshu]

我们当时为了加速开了CPU多线程处理

[kiwiwan]

嗯，我尝试使用多进程处理，现在速度基本能满足需求。
但是我生成的碰撞结果与数据集里的有些差异：1.我生成的碰撞要多许多。2.我生成的挺多无碰撞抓取在桌子下面。3.我生成的抓取不会与机械臂所在区域碰撞，数据集里的无碰撞抓取会与机械臂所在区域碰撞。
下面图1，图2是数据集场景000里物体id0的无碰撞抓取正面和侧面视图。图3，图4是我生成的场景000里物体id0的无碰撞抓取正面和侧面视图。图5是我生成的碰撞与数据集里的碰撞统计情况。

图1

图2

图3

图4

图5
不知道您能否抽空帮我看看是哪里导致了我得到的结果和数据集里的结果之间的差异？
我用的方法大概为：a.将场景000的256个视角的点云拼接，根据各物体信息将该拼接点云去outlier点，然后以0.006下采样。b.对id0物体的所有无碰撞（物体级）抓取进行碰撞检测（场景级），一些参数设为height = 0.02，depth_base = 0.02，finger_width = 0.01，approach_dist = 0.03，collision_thresh=0.05。
gen_collision_test.txt

[chenxi-wang]

这是因为采用的场景点云不同，我们直接利用物体点云模型和6D pose重建了场景，而不是把不同视角的点云合在一起，6D pose的标注会影响最后的结果；相机拍到的桌子只是一个面，实际上应该是有厚度的，我们在做碰撞标注的时候也把桌面的等效模型加了进去；在重建场景时没有把机械臂的模型放进去，因而此部分碰撞无法得到。

[kiwiwan]

非常感谢您的解答！原来场景级的碰撞标注是这么做的，我再试试这样做的结果。

[kiwiwan]

@chenxi-wang ，您好，按照您给的方法测试了一下，比之前结果好了不少。但有个疑问是您的碰撞检测是用iou的方式吗？还是直接看有没有任何点云发生碰撞（如collision_detect()）？

[kiwiwan]

@chenxi-wang ，另外，还想请教一下生成grasp_label时确定抓取宽度的问题。
抓取宽度生成方式是否是取最小的无碰撞非空抓取？
具体操作为：确定手指夹取方向上点云y的最大值及最小值，以该最大值和最小值离抓取坐标系原点的距离极大者的两倍作为最小的无碰撞抓取宽度。若该抓取宽度大于某一最大阈值(猜测是0.15m)，或是该抓取为空抓取（两指之间物体的点云少于10个点），则将抓取宽度设为0。否则，该抓取宽度为最小的无碰撞非空抓取。

[chenxi-wang]

@chenxi-wang ，您好，按照您给的方法测试了一下，比之前结果好了不少。但有个疑问是您的碰撞检测是用iou的方式吗？还是直接看有没有任何点云发生碰撞（如collision_detect()）？

用的是后者的规则

[chenxi-wang]

@chenxi-wang ，另外，还想请教一下生成grasp_label时确定抓取宽度的问题。 抓取宽度生成方式是否是取最小的无碰撞非空抓取？ 具体操作为：确定手指夹取方向上点云y的最大值及最小值，以该最大值和最小值离抓取坐标系原点的距离极大者的两倍作为最小的无碰撞抓取宽度。若该抓取宽度大于某一最大阈值(猜测是0.15m)，或是该抓取为空抓取（两指之间物体的点云少于10个点），则将抓取宽度设为0。否则，该抓取宽度为最小的无碰撞非空抓取。

大致思路是一样的，有的物体表面变化较大，存在多个值，我们从小到大采样，取最小宽度。

[kiwiwan]

@chenxi-wang ，另外，还想请教一下生成grasp_label时确定抓取宽度的问题。 抓取宽度生成方式是否是取最小的无碰撞非空抓取？ 具体操作为：确定手指夹取方向上点云y的最大值及最小值，以该最大值和最小值离抓取坐标系原点的距离极大者的两倍作为最小的无碰撞抓取宽度。若该抓取宽度大于某一最大阈值(猜测是0.15m)，或是该抓取为空抓取（两指之间物体的点云少于10个点），则将抓取宽度设为0。否则，该抓取宽度为最小的无碰撞非空抓取。

大致思路是一样的，有的物体表面变化较大，存在多个值，我们从小到大采样，取最小宽度。

感谢您及时的回复！
您说的存在多个值的意思我有点没懂，手指夹取方向上点云y的最大值及最小值不是都只有一个吗？比如下图：

[chenxi-wang]

拿杯子举例，如果你从正上方杯壁上一个点来看，可以直接夹住杯壁抓取，也可以加大宽度到把整个杯子握住，如果物体表面起伏变化快，很容易出现多值的情况

[pange1802703882]

请问如果要添加新的物体的话，要如何生成scenes中的annotations,label,meta,cam0_wrt_table.npy,camera_poses.npy等文件呢，是用nerf之类的方法重建生成吗?

[Hriver-J]

感谢您的答复！ 我这边想在您提供的数据集上，针对我的应用场景添加几个特定的物体，比如门把手之类的，以提高相应的成功率。 您能否指点一下，该怎么做才能加快这个过程呢？我这样做的速度基本是没法生成场景级的碰撞的，太慢了。

@kiwiwan @chenxi-wang 你们好，我目前也在做相同的事情。想请问一下特定物体的grasp_label宽度计算(offsets的最后一列)是如何做的？

我目前是分这样的步骤去做的：

1. 抓取方向获取
   根据graspnetAPI中的球面采样函数获取到view数据，它的格式是(x, y, z)的三维坐标，含义是以某个点center为球心时，球面上某点的坐标。
   当使用此点坐标减去球心坐标，就可以得到夹爪接近被抓取点的向量V

2. 确定抓取平面
   以下图为例，我们定义抓取平面为：图a中的XOY平面，它的特点是位于夹爪末端平面(过点O)且与接近方向垂直
   
   首先按照点法式假设平面方程m(x-x0)+n(y-y0)+p(z-z0)=0
   其中(m, n, p)为平面法向量，即为1中的V；(x0, y0, z0)为平面上一点，即O点坐标。
   可以直接由O、obj两点共线、在法向量V方向上计算：O点坐标 = obj坐标+向量V乘深度。解出O点坐标后就得到了平面方程

3. 确定基准抓取轴
   基准抓取轴定义为图a的OY所在直线。抓取轴一定经过点O。
   任取一对x, y如x=1.0,y=1.0，可以解出对应的z坐标，此时x,y,z为平面上的任意一点P的坐标。
   相减得到OP向量，OP即为一个抓取轴。
   由于OP轴是随机选择的，并且后续旋转角度步长15°，所以OP轴的0-14°会影响后续抓取轴分布。为了找到最贴合数据集的抓取，我以1°的角度旋转OP轴，分15组数据。

4. 查找抓取轴上的点
   抓取轴确定后，遍历物体点云，查看是否有点在抓取轴上。
   比如物体上某点M，若MO向量与OP向量平行说明两者共线，即M在抓取轴上。做法为：将MO向量与OP向量归一化，判断两个向量的点积，若向量点积与1的差小于0.01，就认为两个向量共线

5. 遍历抓取点、抓取视角、深度、旋转角度

这样做完之后得到的宽度和grasp_labels.npz中的宽度相差很多，不知道是哪里出了问题

import numpy as np
from scipy.spatial.transform import Rotation
import open3d as o3d  # 用于点云处理
import time
import logging
import os
from datetime import datetime

def setup_logger(log_dir='logs'):
    """设置日志记录器"""
    # 创建日志目录
    if not os.path.exists(log_dir):
        os.makedirs(log_dir)
    
    # 生成日志文件名，包含时间戳
    timestamp = datetime.now().strftime('%Y%m%d_%H%M%S')
    log_file = os.path.join(log_dir, f'grasp_gen_{timestamp}.log')
    
    # 配置日志记录器
    logger = logging.getLogger('grasp_generator')
    logger.setLevel(logging.INFO)
    
    # 文件处理器
    file_handler = logging.FileHandler(log_file,encoding="utf-8")
    file_handler.setLevel(logging.INFO)
    
    # 控制台处理器
    console_handler = logging.StreamHandler()
    console_handler.setLevel(logging.INFO)
    
    # 设置日志格式
    formatter = logging.Formatter('%(asctime)s - %(levelname)s - %(message)s')
    file_handler.setFormatter(formatter)
    console_handler.setFormatter(formatter)
    
    # 添加处理器
    logger.addHandler(file_handler)
    logger.addHandler(console_handler)
    
    return logger

def sample_sphere_points(n_points, phi=(np.sqrt(5)-1)/2, center=np.zeros(3), R=1):
    """在球面上均匀采样视角点
    使用Fibonacci lattices方法进行球面均匀采样
    
    Args    : 
    n_points: 采样点数量
    phi     : 黄金分割比, 默认(sqrt(5)-1)/2 ≈ 0.618
    center  : 球心坐标,  默认原点
    R       : 球半径,   默认1
        
    Returns:
        points: (n_points, 3) 采样得到的点坐标
    """
    # 生成索引序列
    idxs = np.arange(n_points, dtype=np.float32)
    
    # 计算Z坐标 (-1 到 1)
    Z = (2 * idxs + 1) / n_points - 1
    
    # 计算X,Y坐标
    X = np.sqrt(1 - Z**2) * np.cos(2 * idxs * np.pi * phi)
    Y = np.sqrt(1 - Z**2) * np.sin(2 * idxs * np.pi * phi)
    
    # 组合坐标并应用半径和中心偏移
    points = np.stack([X, Y, Z], axis=1)
    points = R * points + center
    
    return points

def find_best_grasp_width(grasp_center, grasp_axis, points, min_width=0.01, max_width=0.15, min_points=10): 
    """计算最佳夹爪宽度
    在抓取轴上找点，计算点对之间的距离
    
    Args:
        grasp_center: 抓取中心点
        grasp_axis: 抓取轴方向（单位向量，在抓取平面上）
        points: 物体点云
        min_width: 最小夹爪宽度（0.01m）
        max_width: 最大夹爪宽度（0.15m）
        min_points: 最小支撑点数
        
    Returns:
        width: 计算得到的夹爪宽度，如果无效则返回0
    """
    #0. 找到抓取轴上的点
    points_on_axis = []

    for point in points: 
        vec = point - grasp_center
        vec = vec / np.linalg.norm(vec)
        grasp_axis = grasp_axis / np.linalg.norm(grasp_axis)
        if 1.0 - abs(np.dot(vec, grasp_axis)) < 0.008: 
            points_on_axis.append(point)
    if len(points_on_axis) < min_points: 
        # print("轴上点数不足")
        return 0.0
    
    # 1. 计算点在抓取轴上的投影距离
    points_to_center = points_on_axis - grasp_center
    projections = np.dot(points_to_center, grasp_axis)

    if len(projections) < min_points: 
        return 0.0
    
    # 2. 计算所有点对之间的距离
    diffs = []
    for i in range(len(projections)):
        for j in range(i+1, len(projections)):
            diff = abs(projections[i] - projections[j])
            diffs.append(diff)
    
    diffs = np.array(diffs)
    
    # 3. 找到有效范围内的最小距离
    valid_diffs = diffs[(diffs >= min_width) & (diffs <= max_width)]
    
    if len(valid_diffs) == 0:
        return 0.0
        
    return np.min(valid_diffs)

def generate_grasps_for_point(point, normal, points, n_views=300, n_depths=4, n_angles=12, base_angle_idx=0): 
    """为单个点生成抓取参数网格"""
    # 1. 在球面上采样视角点
    views = sample_sphere_points(n_views, center=point)
    
    # 2. 准备深度和角度网格
    depths = np.linspace(0.01, 0.04, n_depths)
    angles = np.linspace(0, np.pi, n_angles, endpoint=False)
    
    # 3. 初始化结果数组
    grasps = np.zeros((n_views, n_angles, n_depths, 3))
    
    # 4. 对每个视角生成抓取
    for view_idx, view in enumerate(views):
        approach = -(view - point)
        
        # 对每个深度生成抓取平面
        for depth_idx, depth in enumerate(depths):
            grasp_center = point + depth * approach
            
            # 在平面上选择一条基准直线
            # 确定抓取平面A(x - x0) + B(y - y0) + C(z - z0) = 0
            grasp_plane = [approach[0], approach[1], approach[2]]

            if (approach[2] - 0.0 < 1e-6): 
                plane_point = np.array([1.0, 1.0, 0.0])
            else: 
                #z = (A(x0 - x) + B(y0 - y)) / C + z0
                plane_point = np.array([1.0, 1.0, 
                (approach[0] * (grasp_center[0] - 1.0) + approach[1] * (grasp_center[1] - 1.0)) / approach[2] + grasp_center[2]])

            base = plane_point - grasp_center
            base = base / np.linalg.norm(base)

            base_angle = np.pi / 180 * base_angle_idx
            # 旋转基准向量
            # print(base_angle, "before rot: ", base)
            rotation_base = Rotation.from_rotvec(base_angle * approach)

            base = rotation_base.apply(base)
            # print(base_angle, "after rot: ", base)
            # 在平面内旋转基准直线
            for angle_idx, angle in enumerate(angles):
                rotation = Rotation.from_rotvec(angle * approach)
                rotated_direction = rotation.apply(base)
                
                # 计算抓取宽度
                width = find_best_grasp_width(
                    grasp_center, rotated_direction, points
                )

                # 存储参数
                grasps[view_idx, angle_idx, depth_idx] = [
                    angle,
                    depth,
                    width
                ]
    
    return grasps

def main():
    # 设置日志记录器
    logger = setup_logger()
    
    logger.info("开始加载数据...")
    start_time = time.time()
    
    # 定义参数
    n_views = 300
    n_depths = 4
    n_angles = 12
    
    # 1. 加载labels数据
    labels = np.load('examples/002/002_labels.npz')
    points = labels['points']
    original_offsets = labels['offsets']
    
    test_size = 2
    test_points = points[:test_size]
    original_offsets = original_offsets[:test_size]
    
    logger.info(f"数据加载完成，耗时: {time.time() - start_time:.2f}秒")
    logger.info(f"完整点云大小: {len(points)}个点")
    logger.info(f"测试点数量: {len(test_points)}个点")
    
    # 2. 计算点云法向量
    logger.info("计算点云法向量...")
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(test_points)
    pcd.estimate_normals()
    normals = np.asarray(pcd.normals)
    total_points = len(test_points)
    logger.info("开始生成抓取...")
    for rot_axis_idx in range(15): 
        # 3. 为每个点生成抓取并与原始标签对比
        differences = []
        logger.info(f"=========第{rot_axis_idx}个旋转轴=========")
        for point_idx in range(total_points):
            logger.info(f"\n处理点 {point_idx}/{total_points}")
            point = test_points[point_idx]
            normal = normals[point_idx]
            
            # 生成抓取，传入完整点云用于判断
            generated_grasps = generate_grasps_for_point(point, normal, points, base_angle_idx=rot_axis_idx)
            original_grasps = original_offsets[point_idx]
            
            logger.info(f"生成的抓取形状: {generated_grasps.shape}")
            logger.info(f"原始抓取形状: {original_grasps.shape}")
            logger.info(f"生成的非零抓取数量: {np.sum(generated_grasps[..., 2] > 0)}")
            logger.info(f"原始的非零抓取数量: {np.sum(original_grasps[..., 2] > 0)}")
            
            # 计算差异
            valid_mask = (generated_grasps[..., 2] > 0) & (original_grasps[..., 2] > 0)
            logger.info(f"有效对比数量: {np.sum(valid_mask)}")
            
            if np.any(valid_mask):
                rotation_diff = np.mean(np.abs(generated_grasps[valid_mask, 0] - original_grasps[valid_mask, 0]))
                depth_diff = np.mean(np.abs(generated_grasps[valid_mask, 1] - original_grasps[valid_mask, 1]))
                width_diff = np.mean(np.abs(generated_grasps[valid_mask, 2] - original_grasps[valid_mask, 2]))
                differences.append([rotation_diff, depth_diff, width_diff])
                
                # 详细打印每个点的宽度对比
                logger.info(f"\n点 {point_idx} 的宽度对比:")
                # 获取前5个有效抓取的索引
                valid_indices = np.where(valid_mask)
                for i in range(min(5, len(valid_indices[0]))):
                    v, a,         d = valid_indices[0][i], valid_indices[1][i], valid_indices[2][i]
                    gen_width  = float(generated_grasps[v, a, d, 2])
                    orig_width = float(original_grasps[v, a, d, 2])
                    logger.info(f"生成宽度: {gen_width:.6f}m vs 原始宽度: {orig_width:.6f}m")
        
        # 4. 统计分析
        if len(differences) > 0:
            differences = np.array(differences)
            logger.info("\n整体统计:")
            logger.info("旋转角度差异: ", np.mean(differences[:, 0]), "±", np.std(differences[:, 0]))
            logger.info("深度差异: ", np.mean(differences[:, 1]), "±", np.std(differences[:, 1]))
            logger.info("宽度差异: ", np.mean(differences[:, 2]), "±", np.std(differences[:, 2]))
        else:
            logger.info("\n没有找到有效的对比数据")
        logger.info(f"====================")
    logger.info(f"\n总耗时: {time.time() - start_time:.2f}秒")

if __name__ == "__main__":
    main()

——————————————————
offsets的计算是标注工具中的一部分。我目前已经使用了作者团队提供的标注工具解决这个问题，有需要的可以去baseline-issue33下填写表格申请。感谢作者们的贡献和耐心