use in existing code of repo OpenPCDet

[Petros626]

Hello,

I came over your repo and would like to know, if this collision detection suits for 3d bb with a heading angle regarding to KITTI dataset. I use augmentation techniques from the repo: https://github.com/open-mmlab/OpenPCDet/blob/8cacccec11db6f59bf6934600c9a175dae254806/pcdet/datasets/augmentor/data_augmentor.py like random_local_rotation and must be sure, that after the rotation not boxes intersect with each other. I started creating a method for that, but I'm not sure if it's correct, so I thought about to use your code directly for this task.

Use case: check here that near 3d bb don't intersect.

def box_collision_test(boxes, qboxes):
    """Box collision test for 3D bounding boxes in the format [x, y, z, dx, dy, dz, heading].

    Args:
        boxes (np.ndarray): Current boxes, shape (N, 7).
        qboxes (np.ndarray): Boxes to avoid, shape (K, 7).

    Returns:
        np.ndarray: A boolean matrix of shape (N, K), where True indicates a collision.
    """
    # Get corners from box formats
    corners = box_utils.boxes_to_corners_3d(boxes)  # (N, 8, 3)
    qcorners = box_utils.boxes_to_corners_3d(qboxes)  # (K, 8, 3)

    N = corners.shape[0]
    K = qcorners.shape[0]
    ret = np.zeros((N, K), dtype=np.bool_)
    
    # Check for AABB collision first
    for i in range(N):
        for j in range(K):
            # AABB overlap check
            if (np.max(corners[i, :, 0]) >= np.min(qcorners[j, :, 0]) and
                np.min(corners[i, :, 0]) <= np.max(qcorners[j, :, 0]) and
                np.max(corners[i, :, 1]) >= np.min(qcorners[j, :, 1]) and
                np.min(corners[i, :, 1]) <= np.max(qcorners[j, :, 1]) and
                np.max(corners[i, :, 2]) >= np.min(qcorners[j, :, 2]) and
                np.min(corners[i, :, 2]) <= np.max(qcorners[j, :, 2])):
                
                # Check for actual edge intersections if AABBs overlap
                for k in range(8):  # Check all edges of box i
                    for l in range(8):  # Check all edges of box j
                        A = corners[i][k]
                        B = corners[i][(k + 1) % 8]
                        C = qcorners[j][l]
                        D = qcorners[j][(l + 1) % 8]
                        
                        # Using vector cross products to check for intersection
                        if lines_intersect(A, B, C, D):
                            ret[i, j] = True
                            break
                    if ret[i, j]:  # If a collision is found, no need to check further
                        print('Collision found after local rotation')
                        break
    return ret

def lines_intersect(A, B, C, D):
    """Check if line segments AB and CD intersect.

    Args:
        A, B, C, D (np.ndarray): Points defining the line segments.

    Returns:
        bool: True if the line segments intersect, False otherwise.
    """
    def ccw(P, Q, R):
        return (R[1] - P[1]) * (Q[0] - P[0]) > (Q[1] - P[1]) * (R[0] - P[0])

    #if clockwise:
    #    return ccw(A, C, D) != ccw(B, C, D) and ccw(A, B, C) != ccw(A, B, D)
    #else:
    return ccw(A, C, D) != ccw(B, C, D) and ccw(A, B, C) != ccw(A, B, D)

Thank you