opengl 在pyBullet中模拟有失真的摄像机

ffdz8vbo  于 2023-02-08  发布在  其他
关注(0)|答案(2)|浏览(287)

我正尝试根据从校准真实摄像头获得的内在和外在参数在pyBullet中配置摄像头。

我拥有的

摄像头用OpenCV校准,给我一个摄像头矩阵

|f_x  0  c_x|
| 0  f_y c_y|
| 0   0   1 |

以及失真系数的向量

(k_1, k_2, p_1, p_2, k_3)

(我还有相机的姿势,但这与实际问题无关,所以我把它留在这里。)

我已经做了什么

不幸的是pyBullet的computeProjectionMatrix函数有一点局限性。它假设f_x = f_yc_x, c_y正好位于图像的中心,但这两个函数对我的相机来说都不正确。因此我自己计算投影矩阵如下(基于this):

projection_matrix = [
    [2/w * f_x,  0,  (w - 2c_x)/w,  0],
    [0,  2/h * f_y,  (2c_y - h)/h,  0],
    [0, 0, A, B],
    [0, 0, -1, 0],
]

其中w,h是图像的宽度和高度,A = (near + far)/(near - far)B = 2 * near * far / (near - far)nearfar定义图像中包含的z轴范围(参见pybullet.computeProjectionMatrix)。

还缺少什么(我的实际问题)

上面的方法已经给了我更好的效果,但是渲染出来的图像仍然和真实的图像不完全匹配,我怀疑其中一个原因可能是没有考虑到失真。
最后回到我的问题:

    • 如何使用从校准真实摄像机获得的参数实现模拟摄像机的失真?**

有没有办法把这个积分到投影矩阵中?如果没有,有没有别的办法?

wtlkbnrh

wtlkbnrh1#

正如在评论中指出的,非线性失真不能集成到矩阵中,我现在要做的是首先渲染没有失真的图像,然后在第二步中使用this answer中的代码扭曲结果图像。
由于失真,图像会缩小一些,所以当保持图像大小不变时,图像的边缘会有一些空白区域。为了弥补这一点,我将图像渲染为比需要的尺寸稍大一些的尺寸,然后在失真后进行裁剪。注意,当增加图像大小时,中心点(c_x, c_y)需要相应地调整。
用一些伪代码来说明:

desired_image_size = (width, height)

# add 10% padding on each size
padding = desired_image_size * 0.1
render_image_size = desired_image_size + 2 * padding

# shift the centre point accordingly (other camera parameters are not changed)
c_x += padding[0]
c_y += padding[1]

# render image using the projection_matrix as described in the question
image = render_without_distortion(projection_matrix, camera_pose)

image = distort_image(image)

# remove the padding
image = image[padding[0]:-padding[0], padding[1]:-padding[1]]

这将生成与真实的摄影机中的图像非常匹配的图像。
完整的实现可以在这里找到。

1dkrff03

1dkrff032#

我广泛地寻找一个简洁的答案来构建视图和投影矩阵使用校准的K和ROS TF外在姿势,但我的乐趣,我没有找到。
我编写并测试了以下两个函数,用于计算在pybullet中模拟真实的相机所需的矩阵。希望它会有用:

from pyquaternion import Quaternion
import numpy as np

def cvK2BulletP(K, w, h, near, far):
    """
    cvKtoPulletP converst the K interinsic matrix as calibrated using Opencv
    and ROS to the projection matrix used in openGL and Pybullet.

    :param K:  OpenCV 3x3 camera intrinsic matrix
    :param w:  Image width
    :param h:  Image height
    :near:     The nearest objects to be included in the render
    :far:      The furthest objects to be included in the render
    :return:   4x4 projection matrix as used in openGL and pybullet
    """ 
    f_x = K[0,0]
    f_y = K[1,1]
    c_x = K[0,2]
    c_y = K[1,2]
    A = (near + far)/(near - far)
    B = 2 * near * far / (near - far)

    projection_matrix = [
                        [2/w * f_x,  0,          (w - 2*c_x)/w,  0],
                        [0,          2/h * f_y,  (2*c_y - h)/h,  0],
                        [0,          0,          A,              B],
                        [0,          0,          -1,             0]]
    #The transpose is needed for respecting the array structure of the OpenGL
    return np.array(projection_matrix).T.reshape(16).tolist()

def cvPose2BulletView(q, t):
    """
    cvPose2BulletView gets orientation and position as used 
    in ROS-TF and opencv and coverts it to the view matrix used 
    in openGL and pyBullet.
    
    :param q: ROS orientation expressed as quaternion [qx, qy, qz, qw] 
    :param t: ROS postion expressed as [tx, ty, tz]
    :return:  4x4 view matrix as used in pybullet and openGL
    
    """
    q = Quaternion([q[3], q[0], q[1], q[2]])
    R = q.rotation_matrix

    T = np.vstack([np.hstack([R, np.array(t).reshape(3,1)]),
                              np.array([0, 0, 0, 1])])
    # Convert opencv convention to python convention
    # By a 180 degrees rotation along X
    Tc = np.array([[1,   0,    0,  0],
                   [0,  -1,    0,  0],
                   [0,   0,   -1,  0],
                   [0,   0,    0,  1]]).reshape(4,4)
    
    # pybullet pse is the inverse of the pose from the ROS-TF
    T=Tc@np.linalg.inv(T)
    # The transpose is needed for respecting the array structure of the OpenGL
    viewMatrix = T.T.reshape(16)
    return viewMatrix

上面的两个函数为您提供了从pybullet环境中获取图像所需的矩阵,如下所示:

projectionMatrix = cvK2BulletP(K, w, h, near, far)
viewMatrix = cvPose2BulletView(q, t)

_, _, rgb, depth, segmentation = b.getCameraImage(W, H, viewMatrix, projectionMatrix, shadow = True)

上面返回的图像没有失真。为此,您可以使用前面提供的答案。

相关问题