PyBullet calIntrinsicMatrix error, the value of f and cx, cy is wrong

[mushroom-x]

the origin version: pybullet_env.py

    def calIntrinsicMatrix(self):
        '''计算相机内参矩阵'''
        # 这里的其实是考虑的是无畸变且光心就在画面正中间的情况
        # 而且像素尺寸为正方向 fx = fy
        # FOV是视场角，单位° 
        # 注: FOV这里指的是画面对角线对应的视场角。
        # FOV视场角的一半: \phi= (FOV/2) * (pi/180°)
        # 图像对角线的一半 l = \sqrt((h/2)^2 + (w/2)^2)
        # tan(\phi) = l / f  f是等效焦距
        # f = l / tan(\phi)
        # TODO f求错了，不应该/2
        # 导致的问题就是FOV跟实际的FOV不匹配，要比预设的FOV大
        f = math.sqrt(self.width * self.width / 4.0 + self.height * self.height / 4.0) / 2.0 / math.tan(self.fov / 2.0 / 180.0 * math.pi)
        # cx = w/2
        # cy = h/2 
        # TODO 写错了， 这里cx, cy不应该有-0.5
        return (f, 0.0, self.width / 2.0 - 0.5, 0.0, f, self.height / 2.0 - 0.5, 0.0, 0.0, 1.0)

It is need to be modified as :

    def calIntrinsicMatrix(self):
        '''计算相机内参矩阵'''
        f = math.sqrt(self.width * self.width / 4.0 + self.height * self.height / 4.0) / math.tan(self.fov / 2.0 / 180.0 * math.pi)
        # cx = w/2
        # cy = h/2 
        # TODO 写错了， 这里cx, cy不应该有-0.5
        return (f, 0.0, self.width / 2.0 , 0.0, f, self.height / 2.0, 0.0, 0.0, 1.0)