pybullet+graspnet实现多相机点云融合与物体抓取（三）

最新推荐文章于 2025-09-25 09:38:47 发布

原创最新推荐文章于 2025-09-25 09:38:47 发布 · 1.6k 阅读

23 ·

CC 4.0 BY-SA版权

文章标签：

#数码相机

在本文中，除了简单讲一下近期收到的问题，也公布系列文章二里视频使用的代码。

代码链接：

csdn链接（免费）

github（注意，该文件中没有加knn和pointnet包，你需要自己添加）

问题1：如何获取相机内参：

首先通过以下代码定义一个pybullet相机：

    width = 1280  # 图像宽度
    height = 720   # 图像高度

    fov = 50  # 相机视角
    aspect = width / height  # 宽高比
    near = 0.01  # 最近拍摄距离
    far = 20  # 最远拍摄距离


    # 请自己定义以下参数
    # cameraPos = [0,0,0.3]  # 相机位置
    # targetPos = [0,0,0]  # 目标位置，与相机位置之间的向量构成相机朝向
    # cameraupPos = [0,1,0]  # 相机顶端朝向

    viewMatrix = p.computeViewMatrix(
        cameraEyePosition=cameraPos,
        cameraTargetPosition=targetPos,
        cameraUpVector=cameraupPos,
        physicsClientId=0
    )  # 计算视角矩阵
    projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, near, far)  # 计算投影矩阵

通过projection_matrix来获取相机的内参，如何做？直接打印即可，比如上述代码得到的是：

(1.2062851190567017, 0.0, 0.0, 0.0,
 0.0, 2.1445069313049316, 0.0, 0.0,
 0.0, 0.0, -1.0010005235671997, -1.0,
 0.0, 0.0, -0.020010003820061684, 0.0)

对应的是：

其中需要关注的是fx和fy两个参数，这些东西将被应用于graspnet中CameraInfo的填写。比如在代码的

grasp_detector.py

文件中，

process_data

函数里就有对CamerInfo的声明，其中的fx就是这个矩阵的（0，0）*图像宽/2，fy是（1,1）*图像高/2，比如：（其中scale=1000是因为graspnet用的是毫米而pybullet默认单位是米，差距1000倍）

camera = CameraInfo(width=1280., height=720., fx=1.206285 * 1280 / 2., fy=2.14450693 * 720 / 2., cx=1280 / 2., cy=720 / 2., scale=1000.0)

这是相机参数相关的设置

问题2：如何控制夹爪的位移

我的方法是在夹爪一端加上六个关节，通过逆运动学计算夹爪的轨迹。首先你在代码里

gripper85/urdf/simple_gripper.urdf

将这个文件放到这个地方

之后再运行panda test.py就可以使用代码了。移动夹爪的函数是pos

运行过程中，按z键拍照片，按a是移动机械臂并释放物块，按c是随机添加物体。

问题3：knn代码报错

RuntimeError: expected scalar type Int but found Long 这一报错是由代码knn_pytorch.knn(ref, query, inds)引起的
解决方法是把c文件里面所有的long改成int，重新build即可