计算点到线的横向距离

代码

import numpy as np
import torch


def find_projection(x, y, x0, y0, x1, y1):
    t = ((x - x0) * (x1 - x0) + (y - y0) * (y1 - y0)) / ((x1 - x0) ** 2 + (y1 - y0) ** 2)
    t = np.clip(t, 0, 1)
    v_x = x0 + (x1 - x0) * t
    v_y = y0 + (y1 - y0) * t
    return v_x, v_y, t


def projection(lines, points):
    # lines: [M * 4] [x_s, y_s, x_1, y_1]
    # points: [N * 2] [x, y]
    # return: [N * M * 6]
    #         r, s, l, proj_x, proj_y, distance
    vector_point = points[:, None] - lines[None, :, [0, 1]]
    vector_line = lines[:, [2, 3]] - lines[:, [0, 1]]
    square_line_length = torch.square(vector_line).sum(dim=-1)
    line_length = torch.sqrt(square_line_length + 1e-6)
    inner_product = (vector_point * vector_line[None, :]).sum(dim=-1)
    output_product = vector_line[None, :, 0] * vector_point[:, :, 1] - vector_line[None, :, 1] * vector_point[:, :, 0]

    s = inner_product / line_length
    l = output_product / line_length
    r = s / line_length
    t = r.clamp(0, 1)
    proj = lines[None, :, [0, 1]] + t[..., None] * vector_line[None]
    distance = torch.sqrt((points[:, None] - proj).square().sum(dim=-1) + 1e-6)
    return torch.cat([r[..., None], s[..., None], l[..., None], proj, distance[..., None]], dim=-1)


def projection_batch(lines, points, vectorized=True):
    # lines: [B * M * 4] [x_s, y_s, x_1, y_1]
    # points: [B * N * 2] [x, y]
    # return: [B * N * M * 6]
    #         r, s, l, proj_x, proj_y, distance
    assert points.shape[0] == lines.shape[0]
    B, M = lines.shape[:2]
    N = points.shape[1]
    if vectorized:
        vector_point = points[:, :, None] - lines[:, None, :, [0, 1]]  # [B, M, 2]
    else:
        vector_point = points[:, :, None] - torch.zeros_like(lines)[:, None, :, [0, 1]]
    vector_line = lines[:, :, [2, 3]] - lines[:, :, [0, 1]]  # [B, N, 2]
    square_line_length = torch.square(vector_line).sum(dim=-1)
    line_length = torch.sqrt(square_line_length + 1e-6)
    # A · B = |A||B|Cos(θ) ; A·B = x1*y2 + x2*y1
    inner_product = (vector_point * vector_line[:, None, :]).sum(dim=-1)
    # A x B = |A||B|Sin(θ) ; A*B = x1*y2-x2*y1
    output_product = vector_line[:, None, :, 0] * vector_point[:, :, :, 1] - vector_line[:, None, :, 1] * vector_point[:, :, :, 0]
    # points 向 lines 纵向投影
    s = inner_product / line_length[:, None]
    # points 向 lines 的横向投影
    l = output_product / line_length[:, None]
    # 这块应该除 vector_point 的 length 得到 Cos(θ)
    r = s / line_length[:, None]
    t = r.clamp(0, 1)
    proj = lines[:, None, :, [0, 1]] + t[..., None] * vector_line[:, None]
    distance = torch.sqrt((points[:, :, None] - proj).square().sum(dim=-1) + 1e-6)
    return torch.cat([r[..., None], s[..., None], l[..., None], proj, distance[..., None]], dim=-1)


def projection_pairwise_batch(lines, points):
    # lines: [B * N * 4] [x_s, y_s, x_1, y_1]
    # points: [B * N * 2] [x, y]
    # return: [B * N * 6]
    #         r, s, l, proj_x, proj_y, distance
    assert points.shape[0] == lines.shape[0]
    assert points.shape[1] == lines.shape[1]
    B, M = lines.shape[:2]

    vector_point = points - lines[:, :, [0, 1]]
    vector_line = lines[:, :, [2, 3]] - lines[:, :, [0, 1]]
    line_length = safe_norm(vector_line)
    inner_product = (vector_point * vector_line).sum(dim=-1)
    output_product = vector_line[:, :, 0] * vector_point[:, :, 1] - vector_line[:, :, 1] * vector_point[:, :, 0]

    s = inner_product / line_length
    l = output_product / line_length
    r = s / line_length
    t = r.clamp(0, 1)
    proj = lines[:, :, [0, 1]] + t[..., None] * vector_line
    distance = safe_norm(points - proj)
    return torch.cat([r[..., None], s[..., None], l[..., None], proj, distance[..., None]], dim=-1)


def projection_np(lines, points):
    # lines: [M * 4] [x_s, y_s, x_1, y_1]
    # points: [N * 2] [x, y]
    # return: [N * M * 6]
    #         r, s, l, proj_x, proj_y, distance
    vector_point = points[:, None] - lines[None, :, [0, 1]]
    vector_line = lines[:, [2, 3]] - lines[:, [0, 1]]
    square_line_length = np.square(vector_line).sum(axis=-1)
    line_length = np.sqrt(square_line_length + 1e-6)
    inner_product = (vector_point * vector_line[None, :]).sum(axis=-1)
    output_product = vector_line[None, :, 0] * vector_point[:, :, 1] - vector_line[None, :, 1] * vector_point[:, :, 0]

    s = inner_product / line_length
    l = output_product / line_length
    r = s / line_length
    t = r.clip(0, 1)
    proj = lines[None, :, [0, 1]] + t[..., None] * vector_line[None]
    distance = np.sqrt(np.square(points[:, None] - proj).sum(axis=-1) + 1e-6)
    return np.concatenate([r[..., None], s[..., None], l[..., None], proj, distance[..., None]], axis=-1)


def safe_norm(lines):
    # lines: [..., 2]
    # return: [...]
    return torch.sqrt(torch.sum(torch.square(lines), dim=-1) + 1e-6)