Question

我收到的点数为x，y和z的点数从0到65534不等。每隔t毫秒我会得到一个点样本并检查点是否位于自动定义的锥体之外。以下是我在算法中使用的步骤：

第一个点用于定义右圆锥的尖端（我将使用x_tip，y_tip和z_tip坐标来标记）。圆锥的高度 h 等于z_tip
使用提示的x_tip和y_tip，我会计算z = 0

给定角度a我使用毕达哥拉斯定理计算圆锥的半径r：

cos_angle = cos(a)
if cos_angle != 0:
  hypotenuse = h / cos_angle
  sin_angle = sin(a)
  if sin_angle != 0:
    r = hypotenuse * sin_angle

在计算高度和半径后，我可以应用两步测试来确定给定点（尖端旁边）是否在锥体外面（注意锥体内部和位于锥体表面的两个点）标记为有效，它是锥体的一部分）：
- 锥形上方的点 - 所有点在z和0之间都有65534，同样的范围也适用于圆锥的高度。因此，检查一个点是否位于锥体下方是没有意义的（并且是不可能的:)）。执行此检查是一个简单的一维检查，检查是否z_point > z_tip（如果true则不需要进一步检查，因为该点明显位于锥体上方）。事实上，该测试是作为样本集中着陆点的预先要求步骤完成的。因此，所有正在检查的点都处于或低于z_zip
- 指向内部，位于圆周上或圆锥体切片外部的高度 - 此测试是二维的，这里我使用欧几里德距离（注意我已经绘制了0..65534范围为0..1，因为我在我的终端打印这些东西，浮点数更容易在视觉上比较（omho））：
```
def pointInsideOrOnCircle(x, y, z):
  if r == 0:
    return False

  # Map the point that we need to validate to 0..1
  x = x/65534.
  y = y/65534.
  z = z/65534.

  # Map the first from the samples points (tip of cone) to 0..1
  x_tip = points[0, 0]/65534.
  y_tip = points[0, 1]/65534.

  # Map cone's radius and height
  h = h/65534.
  r = r/65534.

  # Calculate Euclidean distance from point to center of slice
  d = sqrt((x_tip - x)**2 + (y_tip - y)**2)

  # Calculate the radius of the cone's slice
  r_slice = (r*z)/h

  return d <= r_slice # If true, point lies inside or on surface of cone
```

我在想这是有效的，但后来我决定用它绘制......结果看起来很奇怪：

示例：

样本

[
    [65534,62471,54690],
    [65534,59646,52585],
    [65534,56317,50283],
    [65240,52776,47825],
    [64508,49397,45249],
    [63357,46581,42549],
    [61838,44648,39719],
    [60011,43742,36772],
    [57902,43845,33730],
    [55473,44884,30622],
    [52618,46840,27472],
    [49215,49783,24311],
    [45291,53734,21167],
    [41294,58283,18073],
    [37942,62512,15059],
    [35498,65534,12158],
    [33748,65534,9399],
    [32488,65534,6808],
    [31445,65534,4405],
    [30533,65534,2205]
]

我得到了

示例：

样本

[
    [39130,65534,50773],
    [39387,65534,47926],
    [39700,65534,44867],
    [39991,65534,41689],
    [40185,65534,38404],
    [40213,65534,35034],
    [40023,65534,31614],
    [39583,65534,28187],
    [37913,65534,21521],
    [36686,65534,18402],
    [35219,65534,15506],
    [33574,65534,12883],
    [31892,65534,10570],
    [30378,65534,8592],
    [28339,65534,5679],
    [27757,65534,4740],
    [27361,65534,4138],
    [27094,65534,3859],
    [26824,65534,4017],
    [26790,65534,5566]
]

我得到了

有效点标记为绿色圈，而无效点标记为红色圈。正如你所看到的东西没有加起来 - 我有一些标记为有效的点，即使它们位于锥形区域之外，而其他点位于锥形区域内但被标记为无效。我不知道我的错误在哪里 - 是算法，是一些舍入问题，是情节本身......

下面的完整代码有一些变量名称略有不同，但应该很容易从上面的描述中跟进这些变量。箭头是额外的可视化，让我能够轻松地看到一些东西（例如，锥形尖端处的红色箭头指向，锥形底部的蓝色箭头和样本集中的最后一点处的黄色箭头指向）如果它困扰你，你可以删除它们。

#!/usr/bin/env python3
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from matplotlib.patches import FancyArrowPatch
from mpl_toolkits.mplot3d import proj3d
import numpy as np
from math import pi, cos, sin, sqrt


class Arrow3D(FancyArrowPatch):
    def __init__(self, xs, ys, zs, *args, **kwargs):
        FancyArrowPatch.__init__(self, (0, 0), (0, 0), *args, **kwargs)
        self._verts3d = xs, ys, zs

    def draw(self, renderer):
        xs3d, ys3d, zs3d = self._verts3d
        xs, ys, zs = proj3d.proj_transform(xs3d, ys3d, zs3d, renderer.M)
        self.set_positions((xs[0], ys[0]), (xs[1], ys[1]))
        FancyArrowPatch.draw(self, renderer)

points = np.array(
[
    [65534,62471,54690],
    [65534,59646,52585],
    [65534,56317,50283],
    [65240,52776,47825],
    [64508,49397,45249],
    [63357,46581,42549],
    [61838,44648,39719],
    [60011,43742,36772],
    [57902,43845,33730],
    [55473,44884,30622],
    [52618,46840,27472],
    [49215,49783,24311],
    [45291,53734,21167],
    [41294,58283,18073],
    [37942,62512,15059],
    [35498,65534,12158],
    [33748,65534,9399],
    [32488,65534,6808],
    [31445,65534,4405],
    [30533,65534,2205]
]
)

fig = plt.figure()

ax = Axes3D(fig)
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('distance')

thetaLevels = np.arange(0, 2*pi + pi/50, pi/50)
cone_height = points[0, 2]
cone_angle = 20.
cone_radius = 0.

cos_cone_angle = cos(cone_angle)
if cos_cone_angle != 0:
    hypotenuse = cone_height/cos_cone_angle
    sin_cone_angle = sin(cone_angle)
    if sin_cone_angle != 0:
        cone_radius = hypotenuse * sin_cone_angle


def pointInsideCircle(x, y, z):
    if cone_radius == 0:
        return False

    # Normalize to scale 0..1
    x = x/65534.
    y = y/65534.
    z = z/65534.

    tip_x = points[0, 0]/65534.
    tip_y = points[0, 1]/65534.

    cone_radius_n = cone_radius/65534.
    cone_height_n = cone_height/65534.

    cone_slice_radius = (cone_radius_n * z)/cone_height_n
    d = cone_slice_radius**2 - (tip_x - x)**2 + (tip_y - y)**2
    d_2 = sqrt((tip_x - x)**2 + (tip_y - y)**2)

    print('''
    point[x=%f, y=%f, z=%f] | slice[x=%f, y=%f, z=%f]\n
    cone(radius=%f, height=%f)\n
    slice(radius=%f)\n
    d = %f\n
    d_2 = %f
    ''' % (x, y, z, tip_x, tip_y, z, cone_radius_n, cone_height_n, cone_slice_radius, d, d_2))

#    if d < 0:
#        return False
    return d_2 <= cone_slice_radius


for x, y, z in points:
    # Slice circumference
    # X,Y of each point lying on the circumference
    sliceCircX = z * np.array([cos(t) for t in thetaLevels]) + points[0, 0]
    sliceCircY = z * np.array([sin(t) for t in thetaLevels]) + points[0, 1]
    ax.plot(sliceCircX,
            sliceCircY,
            -z + points[0, 2],
            'b-')

    # Plot center of cone slice at height of point
    ax.scatter(points[0, 0], points[0, 1], z, s=60, c='k', lw=0)

    valid = pointInsideCircle(x, y, z)
    print('point[%d, %d, %d] %s cone' % (x, y, z, ('inside of/lies on' if valid else 'outside of')))

    # Plot point
    ax.scatter(x, y, z, s=60, c=('g' if valid else 'r'), lw=0)

    # Point to center of slice
    p2cs = Arrow3D([x, points[0, 0]],
                   [y, points[0, 1]],
                   [z, z],
                   mutation_scale=20,
                   lw=1, arrowstyle='-', color='b', linestyle='dotted')
    ax.add_artist(p2cs)

aStart = Arrow3D([0, points[0, 0]],
                 [0, points[0, 1]],
                 [0, points[0, 2]],
                 mutation_scale=20,
                 lw=3, arrowstyle="-|>", color="r")
ax.add_artist(aStart)

aEnd = Arrow3D([0, points[-1, 0]],
               [0, points[-1, 1]],
               [0, points[-1, 2]],
               mutation_scale=20,
               lw=3, arrowstyle="-|>", color="y")
ax.add_artist(aEnd)

base = [points[0, 0], points[0, 1], 0]
aBase = Arrow3D([0, base[0]],
                [0, base[1]],
                [0, base[2]],
                mutation_scale=20,
                lw=3, arrowstyle="-|>", color="c")
ax.add_artist(aBase)

tipToBase = [points[0, 0],
             points[0, 1],
             0]
aTipToBase = Arrow3D([points[0, 0], tipToBase[0]],
                     [points[0, 1], tipToBase[1]],
                     [points[0, 2], tipToBase[2]],
                     mutation_scale=20,
                     lw=3, arrowstyle="-|>", color="b")
ax.add_artist(aTipToBase)

fig.show()
input('Press any key to exit')

检查锥体内的点 - 值和结果图（使用matplotlib）是否不准确

0 个答案: