落叶无痕
0%

3D_定向边界框变得简单

发表于 分类于 Changyan：

自动摘要: 在上一篇文章中，我们研究了围绕某些数据/点簇生成2D边界框的过程。但是在使用LIDARs时，即点云数据分别，我们希望边界框像数据一样在3维。 2D定向边界框回顾 作为快速回顾，生成2D定向 ……..

在上一篇文章中，我们研究了围绕某些数据/点簇生成2D边界框的过程。但是在使用LIDARs时，即点云数据分别，我们希望边界框像数据一样在3维。

2D定向边界框回顾

作为快速回顾，生成2D定向框所涉及的步骤如下-

  1. 转换数据以使均值与原点匹配；
  2. 计算特征向量；
  3. 找到主成分的倾斜角/取向角，即具有较高特征值的特征向量；
  4. 使用在步骤3中计算的角度将数据/特征向量与笛卡尔基向量对齐；
  5. 通过确定每个维度上的最大值、最小值来计算边界框；
  6. 撤消对数据和边界框坐标的旋转和平移；

关于3D定向边界框的程序差异的注释

在这里，我们注意到特征向量、平移和旋转任务起着主要作用。计算特征向量和执行平移在3D中也是一项直接的任务，但旋转不是。因为，围绕正交轴的旋转是不可交换的，即魔方层旋转的顺序很重要。这就是改变层的旋转顺序将导致魔方上出现完全不同的颜色图案。旋转的这种非交换性质如下所示（如果您已经意识到这一点，请随时跳到下一节）

处理一些导入并为随机生成器设置种子

1
2
3
4
5
%matplotlib widget
import matplotlib.pyplot as plt
import numpy as np
import numpy.linalg as LA
from mpl\_toolkits.mplot3d import Axes3D

滚动、俯仰和偏航矩阵

在这里，我们定义了偏航 (绕z轴旋转)，俯仰 (绕y轴旋转) 和滚动 (绕x轴旋转) 矩阵，以查看旋转矢量 (2，0,0)，并证明旋转本质上不是可交换的。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
def yaw(theta):
    theta \= np.deg2rad(theta)
    return np.array(\[\[np.cos(theta), \-np.sin(theta), 0\],
                      \[np.sin(theta),  np.cos(theta), 0\],
                      \[            0,              0, 1\]\])

def pitch(theta):
    theta \= np.deg2rad(theta)
    return np.array(\[\[np.cos(theta) , 0, np.sin(theta)\],
                      \[             0, 1,             0\],
                      \[\-np.sin(theta), 0, np.cos(theta)\]\])

def roll(theta):
    theta \= np.deg2rad(theta)
    return np.array(\[\[1,              0,             0\],
                      \[0,  np.cos(theta), np.sin(theta)\],
                      \[0, \-np.sin(theta), np.cos(theta)\]\])
1
2
3
4
5
6
7
8
9
10
def CreateFigure():
    """
 Helper function to create figures and label axes
 """
    fig \= plt.figure()
    ax \= fig.add\_subplot(111, projection\='3d')
    ax.set\_xlabel('x')
    ax.set\_ylabel('y')
    ax.set\_zlabel('z')
    return ax

Yaw（偏航）

将矢量 (2,0，0) 绕z轴逆时针旋转90度

1
2
3
x \= np.matmul(yaw(90), np.array(\[\[2\],
                                   \[0\],
                                   \[0\]\]))
1
2
3
4
5
6
ax \= CreateFigure()

ax.plot(\[0, 2\], \[0, 0\], \[0, 0\], label\="before rotation")
ax.plot(\[0, x\[0,0\]\], \[0, x\[1,0\]\], \[0, x\[2,0\]\], label\="after rotation")
ax.plot(2 \* np.cos(np.linspace(0, np.pi/2, 50)), 2 \* np.sin(np.linspace(0, np.pi/2, 50)), 0, linestyle\="dashed", label\="rotation trail")
ax.legend()

Roll（滚动）

将矢量 (2，0，0) 绕x轴逆时针旋转90度

1
2
3
x \= np.matmul(roll(90), np.array(\[\[0\],
                                    \[2\],
                                    \[0\]\]))
1
2
3
4
5
6
ax \= CreateFigure()

ax.plot(\[0, 0\], \[0, 2\], \[0, 0\], label\="before rotation")
ax.plot(\[0, x\[0,0\]\], \[0, x\[1,0\]\], \[0, x\[2,0\]\], label\="after rotation")
ax.plot(np.zeros(50), 2 \* np.cos(np.linspace(0, np.pi/2, 50)), \-2 \* np.sin(np.linspace(0, np.pi/2, 50)), linestyle\="dashed", label\="rotation trail")
ax.legend()

Pitch（俯仰）

绕y轴逆时针旋转点 (2,0) 90度

1
2
3
x \= np.matmul(pitch(90), np.array(\[\[2\],
                                     \[0\],
                                     \[0\]\]))
1
2
3
4
5
6
ax \= CreateFigure()

ax.plot(\[0, 2\], \[0, 0\], \[0, 0\], label\="before rotation")
ax.plot(\[0, x\[0,0\]\], \[0, x\[1,0\]\], \[0, x\[2,0\]\], label\="after rotation")
ax.plot(2 \* np.cos(np.linspace(0, np.pi/2, 50)), np.zeros(50), \-2 \* np.sin(np.linspace(0, np.pi/2, 50)), linestyle\="dashed", label\="rotation trail")
ax.legend()

滚动，俯仰和偏航以及可视化的旋转的非交换性质

1
2
3
4
5
basis \= np.array(\[\[1,0,0\],
                    \[0,1,0\],
                    \[0,0,1\]\])
x1 \= np.matmul(yaw(90) @ pitch(90), np.array(\[\[2\], \[0\], \[0\]\]))
x2 \= np.matmul(pitch(90) @ yaw(90), np.array(\[\[2\], \[0\], \[0\]\]))
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
def SetAxisProp(ax):
    ax.set\_xlabel('x')
    ax.set\_ylabel('y')
    ax.set\_zlabel('z')
    ax.set\_xlim(\-2, 2)
    ax.set\_ylim(\-2, 2)
    ax.set\_zlim(\-2, 2)

fig \= plt.figure(figsize\=(10,6))
ax1 \= fig.add\_subplot(121, projection\='3d')
SetAxisProp(ax1)
ax2 \= fig.add\_subplot(122, projection\='3d')
SetAxisProp(ax2)

ax1.set\_title("Yaw x Pitch")
ax1.plot(\[0, 2\], \[0, 0\], \[0, 0\], label\="before rotation")
ax1.plot(\[0, x1\[0,0\]\], \[0, x1\[1,0\]\], \[0, x1\[2,0\]\], label\="after rotation")
ax1.legend()

ax2.set\_title("Pitch x Yaw")
ax2.plot(\[0, 2\], \[0, 0\], \[0, 0\], label\="before rotation")
ax2.plot(\[0, x2\[0,0\]\], \[0, x2\[1,0\]\], \[0, x2\[2,0\]\], label\="after rotation")
ax2.legend()

正如我们在上图中看到的，只需更改偏航和俯仰矢量的顺序，生成的矢量就完全不同了！

因此，在这篇文章中，与上一篇涉及面向2D的边界框的帖子相比，主要区别在于旋转以及我们将数据与笛卡尔基数对齐的方式。

快速浏览即将发生的事情

让我们从代码和数学开始，以生成边界框

1
2
3
4
5
6
7
8
9
10
11
12
x \= np.linspace(3, 8, 100) + np.random.normal(0, 0.2, 100)
y \= np.linspace(3, 8, 100) + np.random.normal(0, 0.2, 100)
z \= np.linspace(1, 3, 100) + np.random.normal(0, 0.2, 100)
data \= np.vstack(\[x, y, z\])

fig \= plt.figure()
ax \= fig.add\_subplot(111, projection\='3d')
ax.scatter(data\[0,:\], data\[1,:\], data\[2,:\], label\="original data")
ax.set\_xlabel('x')
ax.set\_ylabel('y')
ax.set\_zlabel('z')
ax.legend()

1
2
3
4
means \= np.mean(data, axis\=1)
cov \= np.cov(data)
eval, evec \= LA.eig(cov)
eval, evec
1
2
3
4
(array(\[4.74618325, 0.0445827 , 0.03395297\]),
 array(\[\[-0.68725458, -0.52695506, -0.4999995 \],
         \[-0.67558502,  0.71661269,  0.1733526 \],
         \[-0.26695696, -0.45692954,  0.84849831\]\]))

检查特征向量之间的角度

由于 LA.eig 返回的特征向量是预先归一化的，因此我们可以使用点积确定特征向量之间的角度

1
np.rad2deg(np.arccos(np.dot(evec\[:,0\], evec\[:,1\])))
1
np.rad2deg(np.arccos(np.dot(evec\[:,1\], evec\[:,2\])))
1
np.rad2deg(np.arccos(np.dot(evec\[:,2\], evec\[:,0\])))
1
centered\_data \= data \- means\[:,np.newaxis\]
1
2
3
4
5
6
7
8
9
10
11
12
13
fig \= plt.figure()
ax \= fig.add\_subplot(111, projection\='3d')
ax.scatter(data\[0,:\], data\[1,:\], data\[2,:\], label\="original data")
ax.scatter(centered\_data\[0,:\], centered\_data\[1,:\], centered\_data\[2,:\], label\="centered data")
ax.legend()
\# cartesian basis
ax.plot(\[0, 1\],  \[0, 0\], \[0, 0\], color\='b', linewidth\=4)
ax.plot(\[0, 0\],  \[0, 1\], \[0, 0\], color\='b', linewidth\=4)
ax.plot(\[0, 0\],  \[0, 0\], \[0, 1\], color\='b', linewidth\=4)
\# eigen basis
ax.plot(\[0, evec\[0, 0\]\],  \[0, evec\[1, 0\]\], \[0, evec\[2, 0\]\], color\='r', linewidth\=4)
ax.plot(\[0, evec\[0, 1\]\],  \[0, evec\[1, 1\]\], \[0, evec\[2, 1\]\], color\='g', linewidth\=4)
ax.plot(\[0, evec\[0, 2\]\],  \[0, evec\[1, 2\]\], \[0, evec\[2, 2\]\], color\='k', linewidth\=4)

蓝色的轴正交轴是笛卡尔基，红色、绿色和黑色的轴是正交特征向量

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
def draw3DRectangle(ax, x1, y1, z1, x2, y2, z2):
    \# the Translate the datatwo sets of coordinates form the apposite diagonal points of a cuboid
    ax.plot(\[x1, x2\], \[y1, y1\], \[z1, z1\], color\='b') \# | (up)
    ax.plot(\[x2, x2\], \[y1, y2\], \[z1, z1\], color\='b') \# -->
    ax.plot(\[x2, x1\], \[y2, y2\], \[z1, z1\], color\='b') \# | (down)
    ax.plot(\[x1, x1\], \[y2, y1\], \[z1, z1\], color\='b') \# <--

    ax.plot(\[x1, x2\], \[y1, y1\], \[z2, z2\], color\='b') \# | (up)
    ax.plot(\[x2, x2\], \[y1, y2\], \[z2, z2\], color\='b') \# -->
    ax.plot(\[x2, x1\], \[y2, y2\], \[z2, z2\], color\='b') \# | (down)
    ax.plot(\[x1, x1\], \[y2, y1\], \[z2, z2\], color\='b') \# <--

    ax.plot(\[x1, x1\], \[y1, y1\], \[z1, z2\], color\='b') \# | (up)
    ax.plot(\[x2, x2\], \[y2, y2\], \[z1, z2\], color\='b') \# -->
    ax.plot(\[x1, x1\], \[y2, y2\], \[z1, z2\], color\='b') \# | (down)
    ax.plot(\[x2, x2\], \[y1, y1\], \[z1, z2\], color\='b') \# <--

计算每个维度的最小值和最大值，并绘制矩形

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
xmin, xmax, ymin, ymax, zmin, zmax \= np.min(centered\_data\[0, :\]), np.max(centered\_data\[0, :\]), np.min(centered\_data\[1, :\]), np.max(centered\_data\[1, :\]), np.min(centered\_data\[2, :\]), np.max(centered\_data\[2, :\])
\# Code repeat start
fig \= plt.figure()
ax \= fig.add\_subplot(111, projection\='3d')
ax.scatter(data\[0,:\], data\[1,:\], data\[2,:\], label\="original data")
ax.scatter(centered\_data\[0,:\], centered\_data\[1,:\], centered\_data\[2,:\], label\="centered data")
ax.legend()
\# cartesian basis
ax.plot(\[0, 1\],  \[0, 0\], \[0, 0\], color\='b', linewidth\=4)
ax.plot(\[0, 0\],  \[0, 1\], \[0, 0\], color\='b', linewidth\=4)
ax.plot(\[0, 0\],  \[0, 0\], \[0, 1\], color\='b', linewidth\=4)
\# eigen basis
ax.plot(\[0, evec\[0, 0\]\],  \[0, evec\[1, 0\]\], \[0, evec\[2, 0\]\], color\='r', linewidth\=4)
ax.plot(\[0, evec\[0, 1\]\],  \[0, evec\[1, 1\]\], \[0, evec\[2, 1\]\], color\='g', linewidth\=4)
ax.plot(\[0, evec\[0, 2\]\],  \[0, evec\[1, 2\]\], \[0, evec\[2, 2\]\], color\='k', linewidth\=4)
\# Code repeat end
draw3DRectangle(ax, xmin, ymin, zmin, xmax, ymax, zmax)

旋转!定向边界框生成过程的关键

如前所述，特征向量是预归一化的，并且当我们检查它们之间的角度时，它们彼此正交，实际上，我们手中有一组新的基向量。我们可以使用此基向量将坐标转换为使用任何其他基向量定义的坐标。

也就是说，在我们的例子中，特征向量矩阵evec可用于变换在基向量 (1,0，0) 、 (0,1，0) 、 (0,0，1) 中定义的坐标。让我们将笛卡尔基向量中的坐标表示为C以特征向量为基础的坐标为E。

  • 我们可以从C到E通过乘以evec基向量矩阵
  • 我们可以从E到C通过乘以**逆 (evec)**基向量矩阵

由于特征向量被归一化，因此逆 (evec)= =转置 (evec)(下面的代码证明了这一点)

1
np.allclose(LA.inv(evec), evec.T)

旋转数据，即将特征向量与笛卡尔基对齐

1
aligned\_coords \= np.matmul(evec.T, centered\_data)
1
2
3
4
5
6
7
8
fig \= plt.figure()
ax \= fig.add\_subplot(111, projection\='3d')
ax.scatter(aligned\_coords\[0,:\], aligned\_coords\[1,:\], aligned\_coords\[2,:\], color\='g', label\="rotated/aligned data")
ax.scatter(centered\_data\[0,:\], centered\_data\[1,:\], centered\_data\[2,:\], color\='orange', label\="centered data")
ax.legend()
ax.set\_xlabel('x')
ax.set\_ylabel('y')
ax.set\_zlabel('z')

计算每个维度的最小值和最大值，并绘制矩形

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
xmin, xmax, ymin, ymax, zmin, zmax \= np.min(aligned\_coords\[0, :\]), np.max(aligned\_coords\[0, :\]), np.min(aligned\_coords\[1, :\]), np.max(aligned\_coords\[1, :\]), np.min(aligned\_coords\[2, :\]), np.max(aligned\_coords\[2, :\])

fig \= plt.figure()
ax \= fig.add\_subplot(111, projection\='3d')
ax.scatter(aligned\_coords\[0,:\], aligned\_coords\[1,:\], aligned\_coords\[2,:\], color\='g', label\="rotated/aligned data")
ax.scatter(centered\_data\[0,:\], centered\_data\[1,:\], centered\_data\[2,:\], color\='tab:orange', label\="centered data")
ax.legend()
ax.set\_xlabel('x')
ax.set\_ylabel('y')
ax.set\_zlabel('z')
\# cartesian basis
ax.plot(\[0, 1\],  \[0, 0\], \[0, 0\], color\='b', linewidth\=4)
ax.plot(\[0, 0\],  \[0, 1\], \[0, 0\], color\='b', linewidth\=4)
ax.plot(\[0, 0\],  \[0, 0\], \[0, 1\], color\='b', linewidth\=4)
\# eigen basis
ax.plot(\[0, evec\[0, 0\]\],  \[0, evec\[1, 0\]\], \[0, evec\[2, 0\]\], color\='r', linewidth\=4)
ax.plot(\[0, evec\[0, 1\]\],  \[0, evec\[1, 1\]\], \[0, evec\[2, 1\]\], color\='g', linewidth\=4)
ax.plot(\[0, evec\[0, 2\]\],  \[0, evec\[1, 2\]\], \[0, evec\[2, 2\]\], color\='k', linewidth\=4)

draw3DRectangle(ax, xmin, ymin, zmin, xmax, ymax, zmax)

撤消对数据和边界框坐标的旋转和平移

1
2
3
4
5
6
7
8
9
rectCoords \= lambda x1, y1, z1, x2, y2, z2: np.array(\[\[x1, x1, x2, x2, x1, x1, x2, x2\],
                                                        \[y1, y2, y2, y1, y1, y2, y2, y1\],
                                                        \[z1, z1, z1, z1, z2, z2, z2, z2\]\])

realigned\_coords \= np.matmul(evec, aligned\_coords)
realigned\_coords += means\[:, np.newaxis\]

rrc \= np.matmul(evec, rectCoords(xmin, ymin, zmin, xmax, ymax, zmax))
\# rrc = rotated rectangle coordinates

平移边界框的坐标

1
rrc += means\[:, np.newaxis\]

绘制数据和边界框

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
fig \= plt.figure()
ax \= fig.add\_subplot(111, projection\='3d')
ax.scatter(realigned\_coords\[0,:\], realigned\_coords\[1,:\], realigned\_coords\[2,:\], label\="rotation and translation undone")
ax.legend()

\# z1 plane boundary
ax.plot(rrc\[0, 0:2\], rrc\[1, 0:2\], rrc\[2, 0:2\], color\='b')
ax.plot(rrc\[0, 1:3\], rrc\[1, 1:3\], rrc\[2, 1:3\], color\='b')
ax.plot(rrc\[0, 2:4\], rrc\[1, 2:4\], rrc\[2, 2:4\], color\='b')
ax.plot(rrc\[0, \[3,0\]\], rrc\[1, \[3,0\]\], rrc\[2, \[3,0\]\], color\='b')

\# z2 plane boundary
ax.plot(rrc\[0, 4:6\], rrc\[1, 4:6\], rrc\[2, 4:6\], color\='b')
ax.plot(rrc\[0, 5:7\], rrc\[1, 5:7\], rrc\[2, 5:7\], color\='b')
ax.plot(rrc\[0, 6:\], rrc\[1, 6:\], rrc\[2, 6:\], color\='b')
ax.plot(rrc\[0, \[7, 4\]\], rrc\[1, \[7, 4\]\], rrc\[2, \[7, 4\]\], color\='b')

\# z1 and z2 connecting boundaries
ax.plot(rrc\[0, \[0, 4\]\], rrc\[1, \[0, 4\]\], rrc\[2, \[0, 4\]\], color\='b')
ax.plot(rrc\[0, \[1, 5\]\], rrc\[1, \[1, 5\]\], rrc\[2, \[1, 5\]\], color\='b')
ax.plot(rrc\[0, \[2, 6\]\], rrc\[1, \[2, 6\]\], rrc\[2, \[2, 6\]\], color\='b')
ax.plot(rrc\[0, \[3, 7\]\], rrc\[1, \[3, 7\]\], rrc\[2, \[3, 7\]\], color\='b')

结论

使用特征向量，我们能够在执行平移，旋转和共轭运算的同时为手头的数据生成一个边界框。

欢迎关注我的其它发布渠道