def test_camera_info(self): width = 640 height = 480 fov_y = np.pi / 4 focal_y = height / 2 / np.tan(fov_y / 2) focal_x = focal_y center_x = width / 2 - 0.5 center_y = height / 2 - 0.5 intrinsic_matrix = np.array([[focal_x, 0, center_x], [0, focal_y, center_y], [0, 0, 1]]) infos = [ mut.CameraInfo(width=width, height=height, fov_y=fov_y), mut.CameraInfo(width=width, height=height, focal_x=focal_x, focal_y=focal_y, center_x=center_x, center_y=center_y), ] for info in infos: self.assertEqual(info.width(), width) self.assertEqual(info.height(), height) self.assertEqual(info.focal_x(), focal_x) self.assertEqual(info.focal_y(), focal_y) self.assertEqual(info.center_x(), center_x) self.assertEqual(info.center_y(), center_y) self.assertTrue( (info.intrinsic_matrix() == intrinsic_matrix).all()) assert_pickle(self, info, mut.CameraInfo.intrinsic_matrix)
def construct_single(parent_id, X_PB): depth_camera = DepthRenderCamera( RenderCameraCore("renderer", mut.CameraInfo(width, height, np.pi / 6), ClippingRange(0.1, 6.0), RigidTransform()), DepthRange(0.1, 5.5)) return mut.RgbdSensor(parent_id=parent_id, X_PB=X_PB, depth_camera=depth_camera)
def construct(parent_id, X_PB): color_camera = ColorRenderCamera( RenderCameraCore("renderer", mut.CameraInfo(width, height, np.pi / 6), ClippingRange(0.1, 6.0), RigidTransform()), False) depth_camera = DepthRenderCamera(color_camera.core(), DepthRange(0.1, 5.5)) return mut.RgbdSensor(parent_id=parent_id, X_PB=X_PB, color_camera=color_camera, depth_camera=depth_camera)