def test_calc_fovx(self):
fovx = PinholeCameraModel.calc_fovx(53.8, 1080, 1920)
testing.assert_almost_equal(
fovx, 84.1, decimal=1)
fovx = PinholeCameraModel.calc_fovx(45.0, 480, 640)
testing.assert_almost_equal(
fovx, 57.8, decimal=1)
def save_roi(camera_info, output_file):
"""Save roi
save as [xmin, xmax, ymin, ymax] order
cm.roi is [top, left, bottom, right]
"""
cm = PinholeCameraModel.from_camera_info(camera_info)
ymin, xmin, ymax, xmax = cm.roi
clip_info = np.array([xmin, xmax, ymin, ymax])
np.save(output_file, clip_info)
def test__binning_y(self):
cm = PinholeCameraModel.from_yaml_file(kinect_v2_camera_info())
cm.roi = [0, 0, 100, 100]
cm.target_size = (640, 480)
cm.binning_y = 0.5
self.assertEqual(cm.target_size, (640, 200))
cm.target_size = (640, 480)
cm.roi = [0, 0, 97, 100]
cm.binning_y = 2
self.assertEqual(cm.target_size, (640, 48))
def test_rectify_point(self):
cm = PinholeCameraModel.from_yaml_file(kinect_v2_camera_info())
testing.assert_equal(cm.rectify_point([0, 0]).shape, (2,))
testing.assert_equal(cm.rectify_point((0, 0)).shape, (2,))
testing.assert_equal(cm.rectify_point(np.array((0, 0))).shape, (2,))
testing.assert_equal(cm.rectify_point([[0, 0]]).shape, (1, 2))
testing.assert_equal(
cm.rectify_point(np.array([[0, 0]])).shape, (1, 2))
testing.assert_equal(
cm.rectify_point(np.zeros((10, 2))).shape, (10, 2))
def test_points_roi(self):
cm = PinholeCameraModel.from_yaml_file(kinect_v2_camera_info())
points = [[874.5, 680],
[875, 680],
[875, 679.5],
[1072, 680],
[1072.5, 680],
[1072, 859],
[1072, 869.5]]
testing.assert_equal(
cm.points_in_roi(points),
[False, True, False, True, False, True, False])
def test__target_size(self):
cm = PinholeCameraModel.from_yaml_file(kinect_v2_camera_info())
org_roi = copy.deepcopy(cm.roi)
cm.target_size = (640, 480)
self.assertEqual(cm.target_size, (640, 480))
resize_K = cm.K.copy()
resize_P = cm.P.copy()
cm.roi = [0, 0, 100, 100]
cm.target_size = (640, 480)
cm.roi = org_roi
testing.assert_equal(resize_K, cm.K)
testing.assert_equal(resize_P, cm.P)
def load_camera_info(self):
rospy.loginfo('loading camera model')
self.camera_info = rospy.wait_for_message(
self.camera_info_msg, CameraInfo)
# self.camera_model.fromCameraInfo(self.camera_info)
self.camera_model = PinholeCameraModel.from_camera_info(self.camera_info)
# self.intrinsic = o3d.camera.PinholeCameraIntrinsic()
# self.intrinsic.set_intrinsics(
# self.camera_model.width,
# self.camera_model.height,
# self.camera_model.fx(),
# self.camera_model.fy(),
# self.camera_model.cx(),
# self.camera_model.cy())
#print(self.camera_model.width)
print('load camera model')
def test__roi(self):
cm = PinholeCameraModel.from_yaml_file(kinect_v2_camera_info())
org_roi = copy.deepcopy(cm.roi)
org_K = cm.K.copy()
org_P = cm.P.copy()
cm.roi = [0, 0, 100, 100]
testing.assert_almost_equal(org_K[:3, :2], cm.K[:3, :2])
testing.assert_almost_equal(cm.K[:2, 2], [951.8467, 506.9212],
decimal=3)
testing.assert_almost_equal(org_P[:3, :2], cm.P[:3, :2])
testing.assert_almost_equal(cm.P[:2, 2], [951.8467, 506.9212],
decimal=3)
cm.roi = org_roi
testing.assert_almost_equal(org_K, cm.K)
testing.assert_almost_equal(org_P, cm.P)
def test_draw_roi(self):
cm = PinholeCameraModel.from_yaml_file(kinect_v2_camera_info())
img_org = kinect_v2_image()
img = img_org.copy()
cm.draw_roi(img, copy=True)
testing.assert_equal(img, img_org)
pil_img = Image.fromarray(img_org)
gray_pil_img = pil_img.convert("L")
gray_img_org = np.array(gray_pil_img, dtype=np.uint8)
gray_img = gray_img_org.copy()
cm.draw_roi(gray_img, copy=False)
testing.assert_equal(gray_img, gray_img_org)
cm.draw_roi(gray_img, copy=True) # ignore copy=True
testing.assert_equal(gray_img, gray_img_org)
alpha_pil_img = pil_img.convert("RGBA")
alpha_img_org = np.array(alpha_pil_img, dtype=np.uint8)
alpha_img = alpha_img_org.copy()
cm.draw_roi(alpha_img, copy=True)
testing.assert_equal(alpha_img, alpha_img_org)
def save_camera_info(camera_info, output_file):
cm = PinholeCameraModel.from_camera_info(camera_info)
cm.dump(output_file)
def test_rectify_image(self):
cm = PinholeCameraModel.from_yaml_file(kinect_v2_camera_info())
img = kinect_v2_image()
cm.rectify_image(img)
def test_from_yaml_file(self):
PinholeCameraModel.from_yaml_file(camera_info_path)
PinholeCameraModel.from_yaml_file(ros_camera_info_path)
def test_from_fovy(self):
PinholeCameraModel.from_fovy(45, 480, 640)
def test_calc_f_from_fov(self):
f = PinholeCameraModel.calc_f_from_fov(90, 480)
testing.assert_almost_equal(f, 240)
def setUpClass(cls):
cls.cm = PinholeCameraModel.from_fovy(45, 480, 640)
def setUpClass(cls):
cls.cm = PinholeCameraModel.from_yaml_file(kinect_v2_camera_info())
def test_kinect_v2_camera_info(self):
info_path = kinect_v2_camera_info()
PinholeCameraModel.from_yaml_file(info_path)
