pybullet实现多深度摄像机点云融合

在本文中，你将了解如何在pybullet环境中设计多深度摄像机并将获得的点云图进行融合。

目录

准备工作：

代码下载：

内容讲解与注意事项：

左右手坐标系的转换：

旋转矩阵的采用：

投影矩阵：

---

准备工作：

在阅读本文前，你应当先参考以下几篇文章：

Pybullet获取RGB图像和深度图像构建点云（Open3D）

左右手坐标系的转换

详解MVP矩阵之ViewMatrix

代码下载：

代码

内容讲解与注意事项：

运行代码

dual_arm_test.py

后，依次敲击键盘a和z（如下所示），拍摄三张相片，依次对应三个摄像头：

图像的通道是颠倒的，但这不会有影响。之后生成如下的三维图

代码中有一些值得注意的事项：

左右手坐标系的转换：

在bullet里面的相机是采用左手系，因此拍摄的图像x轴翻转，因此在pc_cam2pc_world函数中，首先将点云的x轴翻转回来，从而保证点云融合配准。

旋转矩阵的采用：

如果你看了准备工作里的几篇文章，那么你应该知道点云融合需要对每一个点做一个齐次变换，这部分在pc_cam2pc_world函数中。但是要注意采用的矩阵应当是take_pic函数中的viewMatrix。这主要是因为，你的摄像机的姿态可以多种多样，但是应当以你拍摄的角度所构建的矩阵，将摄像机坐标系下的点云转换到世界坐标下。

投影矩阵：

如果你希望对摄像机的参数做修改，则在take_pic函数中修改相关的参数。之后通过：

projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far)

来获取投影矩阵，并将矩阵中的数值同步到make_3d函数中的

intrinsic.set_intrinsics

中，否则你构建的3d模型会发生畸变。

如果你希望把点云保存成stl等三维文件，可以按照下文所述：

3D点云表面重建【Python】

或使用以下代码

        merged_pcd=clouds[0]+clouds[1]+clouds[2]

        ######如果你想将其保存为一个模型文件，用下面的代码
        depth=np.asarray(merged_pcd.points)
        norm=np.array([[0,0,1]for i in range(len(depth))])
        merged_pcd.normals=o3d.utility.Vector3dVector(norm)

        merged_pcd = o3d.geometry.PointCloud.uniform_down_sample(merged_pcd, 10)
        o3d.visualization.draw_geometries([merged_pcd])
        distances = merged_pcd.compute_nearest_neighbor_distance()
        avg_dist = np.mean(distances)
        radius = 3 * avg_dist
        bpa_mesh = o3d.geometry.TriangleMesh.create_from_point_cloud_ball_pivoting(merged_pcd, o3d.utility.DoubleVector(
            [radius, radius * 2]))
        # bpa_mesh = bpa_mesh.simplify_quadric_decimation(10000)
        o3d.io.write_triangle_mesh("bpa_mesh.stl", bpa_mesh)

值得注意的是，应当对点云合理下采样，以保证程序快速的运行。

效果如下（在PPT里可以展示）：