def setup_gimbal_camera(self):
image_width = 640
image_height = 480
fov = 120
fx, fy = focal_length(image_width, image_height, fov)
cx, cy = (image_width / 2.0, image_height / 2.0)
K = camera_intrinsics(fx, fy, cx, cy)
cam_model = PinholeCameraModel(image_width, image_height, K)
return cam_model
def setUp(self):
# Pinhole Camera model
image_width = 640
image_height = 480
fov = 60
fx, fy = focal_length(image_width, image_height, fov)
cx, cy = (image_width / 2.0, image_height / 2.0)
K = camera_intrinsics(fx, fy, cx, cy)
self.cam_model = PinholeCameraModel(image_width, image_height, K)
# Feature estimator
self.estimator = FeatureEstimator()
def test_project_pinhole_equi(self):
image_width = 640
image_height = 480
fov = 120
fx, fy = focal_length(image_width, image_height, fov)
cx, cy = (image_width / 2.0, image_height / 2.0)
K = camera_intrinsics(fx, fy, cx, cy)
D = np.array([0.0, 0.0, 0.0, 0.0])
X_c = np.array([0.0001, 0.0001, 1.0])
result = project_pinhole_equi(X_c, K, D)
print(result)
def setUp(self):
# Pinhole Camera model
image_width = 640
image_height = 480
fov = 60
fx, fy = focal_length(image_width, image_height, fov)
cx, cy = (image_width / 2.0, image_height / 2.0)
K = camera_intrinsics(fx, fy, cx, cy)
self.cam_model = PinholeCameraModel(image_width, image_height, K)
# MSCKF
self.msckf = MSCKF(n_g=0.001 * np.ones(3),
n_a=0.001 * np.ones(3),
n_wg=0.001 * np.ones(3),
n_wa=0.001 * np.ones(3),
ext_p_IC=np.array([0.0, 0.0, 0.0]),
ext_q_CI=np.array([0.5, -0.5, 0.5, -0.5]),
cam_model=self.cam_model)
def setUp(self):
# Generate random features
nb_features = 100
feature_bounds = {
"x": {
"min": -1.0,
"max": 1.0
},
"y": {
"min": -1.0,
"max": 1.0
},
"z": {
"min": 10.0,
"max": 20.0
}
}
self.features = rand3dfeatures(nb_features, feature_bounds)
# Pinhole Camera model
image_width = 640
image_height = 480
fov = 60
fx, fy = focal_length(image_width, image_height, fov)
cx, cy = (image_width / 2.0, image_height / 2.0)
K = camera_intrinsics(fx, fy, cx, cy)
self.cam = PinholeCameraModel(image_width, image_height, K)
# Rotation and translation of camera 0 and camera 1
self.R_0 = np.eye(3)
self.t_0 = np.zeros((3, 1))
self.R_1 = roty(deg2rad(10.0))
self.t_1 = np.array([1.0, 0.0, 0.0]).reshape((3, 1))
# Points as observed by camera 0 and camera 1
self.obs0 = self.project_points(self.features, self.cam, self.R_0,
self.t_0)
self.obs1 = self.project_points(self.features, self.cam, self.R_1,
self.t_1)
def test_estimate(self):
estimator = DatasetFeatureEstimator()
# Pinhole Camera model
image_width = 640
image_height = 480
fov = 60
fx, fy = focal_length(image_width, image_height, fov)
cx, cy = (image_width / 2.0, image_height / 2.0)
K = camera_intrinsics(fx, fy, cx, cy)
cam_model = PinholeCameraModel(image_width, image_height, K)
# Camera states
track_cam_states = []
# -- Camera state 0
p_G_C0 = np.array([0.0, 0.0, 0.0])
rpy_C0G = np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)])
q_C0G = euler2quat(rpy_C0G)
C_C0G = C(q_C0G)
track_cam_states.append(CameraState(0, q_C0G, p_G_C0))
# -- Camera state 1
p_G_C1 = np.array([1.0, 0.0, 0.0])
rpy_C1G = np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)])
q_C1G = euler2quat(rpy_C1G)
C_C1G = C(q_C1G)
track_cam_states.append(CameraState(1, q_C1G, p_G_C1))
# Feature track
p_G_f = np.array([[0.0], [0.0], [10.0]])
kp0 = KeyPoint(cam_model.project(p_G_f, C_C0G, p_G_C0)[0:2], 0)
kp1 = KeyPoint(cam_model.project(p_G_f, C_C1G, p_G_C1)[0:2], 0)
track = FeatureTrack(0, 1, kp0, kp1, ground_truth=p_G_f)
estimate = estimator.estimate(cam_model, track, track_cam_states)
self.assertTrue(np.allclose(p_G_f.ravel(), estimate.ravel(), atol=0.1))
def test_focal_length(self):
fx, fy = focal_length(640, 320, 60)
self.assertEqual(round(fx, 2), 554.26)
self.assertEqual(round(fy, 2), 277.13)