PTZ-SLAM源代码阅读——generate_points.py

最新推荐文章于 2022-12-02 20:54:11 发布

qq_28829853

最新推荐文章于 2022-12-02 20:54:11 发布

阅读量398

点赞数

分类专栏：学习笔记文章标签： python

本文链接：https://blog.csdn.net/qq_28829853/article/details/112335702

版权

学习笔记专栏收录该内容

10 篇文章 0 订阅

订阅专栏

这段代码展示了如何使用Python生成随机的三维点并进行可视化。通过设定随机种子，可以控制点的分布。点的生成分为三种情况，分别对应不同的概率和分布。最后，使用matplotlib库创建3D散点图来展示这些点。代码还包含了将三维点转换为二维图像平面上的点的过程，用于绘制足球场线。

摘要由CSDN通过智能技术生成

generate_points

    def generate_points(num):
        list_pts = []

        # fix the random seed
        random.seed(1)
        for i in range(num):
            choice = random.randint(0, 6)
            if choice < 3:
                x_side = random.randint(0, 1)
                list_pts.append([x_side * random.gauss(0, 5) + (1 - x_side) * random.gauss(108, 5), random.uniform(0, 70), random.uniform(0, 10)])
            elif choice < 5:
                list_pts.append([random.uniform(0, 108), random.gauss(63, 2), random.uniform(0, 10)])
            else:
                tmp_x = random.gauss(54, 20)
                while tmp_x > 108 or tmp_x < 0:
                    tmp_x = random.gauss(54, 20)

                tmp_y = random.gauss(32, 20)
                while tmp_y > 63 or tmp_y < 0:
                    tmp_y = random.gauss(32, 20)

                list_pts.append([tmp_x, tmp_y, random.uniform(0, 1)])

        pts_arr = np.array(list_pts, dtype=np.float32)
        return pts_arr

这段代码是在合成场景中随机生成num个数量的三维点。

random.seed(1)

seed()方法改变随机数生成器的种子，可以在调用其他随机模块函数之前调用此函数

当seed()没有参数时，每次生成的随机数是不一样的，而当seed()有参数时，每次生成的随机数是一样的，同时选择不同的参数生成的随机数也不一样

choice = random.randint(0, 6)

random.randint(参数1，参数2)

参数1、参数2必须是整数
函数返回参数1和参数2之间的任意整数

randint()相当于返回一个序列，seed(1)规定好了，randint()每次生成的序列也是相同的。

具体参见https://www.pynote.net/archives/2217

list_pts.append([x_side * random.gauss(0, 5) + (1 - x_side) * random.gauss(108, 5), random.uniform(0, 70), random.uniform(0, 10)])

random.gauss(mu, sigma)均值为mu且标准偏差为sigma的高斯分布

添加三维空间点（x,y,z），x服从（0，5）的高斯分布或者（108，5）的高斯分布；y和z分别服从于（0，70）和（0，10）的均匀分布

for i in range(num):
            choice = random.randint(0, 6)
            if choice < 3:
                x_side = random.randint(0, 1)
                list_pts.append([x_side * random.gauss(0, 5) + (1 - x_side) * random.gauss(108, 5), random.uniform(0, 70), random.uniform(0, 10)])
            elif choice < 5:
                list_pts.append([random.uniform(0, 108), random.gauss(63, 2), random.uniform(0, 10)])
            else:
                tmp_x = random.gauss(54, 20)
                while tmp_x > 108 or tmp_x < 0:
                    tmp_x = random.gauss(54, 20)

                tmp_y = random.gauss(32, 20)
                while tmp_y > 63 or tmp_y < 0:
                    tmp_y = random.gauss(32, 20)

                list_pts.append([tmp_x, tmp_y, random.uniform(0, 1)])

这部分主要是1/2的概率生成的三维点服从于某个分布，1/3的概率生成的三维点服从于某个分布，1/6的概率生成三维点服从于某种分布。

猜测这样设计是避免对三维点的学习有规律可循。

draw_3d_model

 def draw_3d_model(self, features):
        plt.ion()
        fig = plt.figure(num=1, figsize=(10, 5))
        ax = fig.add_subplot(111, projection='3d')
        ax.set_xlim(0, 120)
        ax.set_ylim(0, 70)
        ax.set_zlim(0, 10)
        for i in range(len(self.line_index)):
            x = [self.points[self.line_index[i][0]][0], self.points[self.line_index[i][1]][0]]
            y = [self.points[self.line_index[i][0]][1], self.points[self.line_index[i][1]][1]]
            z = [0, 0]
            ax.plot(x, y, z, color='g')

        ax.scatter(features[:, 0], features[:, 1], features[:, 2], color='r', marker='o')
        plt.show()

这个函数主要用于画足球场的3d模型的。

plt.ion()

在使用matplotlib的过程中，常常会需要画很多图，但是好像并不能同时展示许多图。这是因为python可视化库matplotlib的显示模式默认为阻塞（block）模式。什么是阻塞模式那？我的理解就是在plt.show()之后，程序会暂停到那儿，并不会继续执行下去。如果需要继续执行程序，就要关闭图片。那如何展示动态图或多个窗口呢？这就要使用plt.ion()这个函数，使matplotlib的显示模式转换为交互（interactive）模式。即使在脚本中遇到plt.show()，代码还是会继续执行。

ax = fig.add_subplot(111, projection='3d')

fig.add_subplot(111)就是构成1x1子图，第一个子图，234就是2x3个图中第4的子图
projection是投影的意思
组合起来就可以在111图中画3D图

ax.set_xlim(0, 120)
ax.set_ylim(0, 70)
ax.set_zlim(0, 10)

从结果可以理解设置坐标系的范围

ax.scatter(features[:, 0], features[:, 1], features[:, 2], color='r', marker='o')

功能：绘制散点

draw_soccer_line

    def draw_soccer_line(self, img, p, t, f):
        k = np.array([[f, 0, self.u], [0, f, self.v], [0, 0, 1]])

        pan = radians(p)
        tilt = radians(t)

        rotation = np.dot(np.array([[1, 0, 0],
                                    [0, cos(tilt), sin(tilt)],
                                    [0, -sin(tilt), cos(tilt)]]),
                          np.array([[cos(pan), 0, -sin(pan)],
                                    [0, 1, 0],
                                    [sin(pan), 0, cos(pan)]]))
        rotation = np.dot(rotation, self.base_rotation)

        image_points = np.ndarray([len(self.points), 2])

        for j in range(len(self.points)):
            p = np.array([self.points[j][0], self.points[j][1], 0])
            p = np.dot(k, np.dot(rotation, p - self.proj_center))
            image_points[j][0] = p[0] / p[2]
            image_points[j][1] = p[1] / p[2]

        for j in range(len(self.line_index)):
            begin = self.line_index[j][0]
            end = self.line_index[j][1]
            cv.line(img, (int(image_points[begin][0]), int(image_points[begin][1])),
                    (int(image_points[end][0]), int(image_points[end][1])), (0, 0, 255), 5)

其中

rotation = np.dot(np.array([[1, 0, 0],
                                    [0, cos(tilt), sin(tilt)],
                                    [0, -sin(tilt), cos(tilt)]]),
                          np.array([[cos(pan), 0, -sin(pan)],
                                    [0, 1, 0],
                                    [sin(pan), 0, cos(pan)]]))
        rotation = np.dot(rotation, self.base_rotation)

这里我们补充三维旋转矩阵的一些知识

绕z轴旋转，

$R_{z}(\theta)=\left[\begin{array}{ccc} \cos \theta & -\sin \theta & 0 \\ \sin \theta & \cos \theta & 0 \\ 0 & 0 & 1 \end{array}\right]$

绕x轴旋转，

$R_{x}(\theta)=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos \theta & -\sin \theta \\ 0 & \sin \theta & \cos \theta \end{array}\right]$

绕y轴旋转，

$R_{y}(\theta)=\left[\begin{array}{ccc} \cos \theta & 0 & \sin \theta \\ 0 & 1 & 0 \\ -\sin \theta & 0 & \cos \theta \end{array}\right]$

以上旋转矩阵都是在右手坐标系下计算的。三维旋转矩阵就可由以上三个矩阵相乘得到。
这里的旋转矩阵是需要左乘的，而且以逆时针为正。R是一个旋转矩阵，X是一个三维列向量[x,y,z]’。

则最终的旋转矩阵应为

$M=R_{x}(\theta) R_{y}(\theta) R_{z}(\theta)$

从代码上可知，Pan对应的x轴，Tilt对应y轴，所以上面的代码写成如上样子。

值得注意的是，在真实的PTZ相机中，Pan往往对应的是Z轴，这里就要涉及到坐标转换，先把坐标系转换成与上面相对应的坐标系，然后再利用上面的工作。

这样的好处是避免程序员手动代换是发生混乱。

 for j in range(len(self.points)):
            p = np.array([self.points[j][0], self.points[j][1], 0])
            p = np.dot(k, np.dot(rotation, p - self.proj_center))
            image_points[j][0] = p[0] / p[2]
            image_points[j][1] = p[1] / p[2]

这里是将三维空间点的位置转换到相机成像面上。

具体原理参见相机矩阵（内参矩阵和外参矩阵）和相机标定原理

这里只拿出结论，即

p = np.dot(k, np.dot(rotation, p - self.proj_center))

可以转换为

其中， $R \dot{t}=t$

以上内容为我的猜测，否则也没有其他合理解释。如果有人有不同意见可以在评论区补充。

qq_28829853

关注

0
点赞
踩
2

收藏

觉得还不错? 一键收藏
0
评论
复制链接

分享到 QQ

分享到新浪微博

扫一扫

专栏目录