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 test_camera_intrinsics(self):
# setup
fx = 1.0
fy = 2.0
cx = 3.0
cy = 4.0
# test and assert
K = camera_intrinsics(fx, fy, cx, cy)
K_exp = np.array([[1.0, 0.0, 3.0], [0.0, 2.0, 4.0], [0.0, 0.0, 1.0]])
self.assertTrue((K == K_exp).all())
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_pixel2image(self):
# Setup camera model
K = camera_intrinsics(554.25, 554.25, 320.0, 320.0)
camera_model = PinholeCameraModel(640, 640, K)
# Test
pixel = np.array([[320.0], [320.0]])
imgpt = camera_model.pixel2image(pixel)
expected = np.array([[0.0], [0.0]])
# Assert
self.assertTrue(np.array_equal(imgpt, expected))
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 test_observed_features(self):
# Setup camera model
K = camera_intrinsics(554.25, 554.25, 320.0, 320.0)
camera_model = PinholeCameraModel(640, 640, K)
# Setup random 3d features
feature = np.array([[0.0], [0.0], [10.0]])
# Test
rpy = np.array([deg2rad(0.0), deg2rad(-10.0), 0.0])
t = np.array([0.0, 0.0, 0.0])
observed = camera_model.observed_features(feature, rpy, t)
# Assert
self.assertTrue(len(observed) > 0)
def __init__(self, **kwargs):
# Debug mode?
self.debug_mode = kwargs.get("debug_mode", False)
# Camera
K = camera_intrinsics(554.25, 554.25, 320.0, 320.0)
self.cam_model = PinholeCameraModel(640, 640, K)
# Features
self.nb_features = kwargs.get("nb_features", 1000)
self.feature_bounds = {"x": {"min": -10.0, "max": 10.0},
"y": {"min": -10.0, "max": 10.0},
"z": {"min": 5.0, "max": 20.0}}
self.features = rand3dfeatures(self.nb_features, self.feature_bounds)
# Simulation settings
self.dt = kwargs.get("dt", 0.01)
self.t = 0.0
# Linear model
self.a_B = np.array([[0.01], [0.0], [0.0]])
self.w_B = np.array([[0.0], [0.0], [0.0]])
self.pos = np.zeros((3, 1))
self.vel = np.zeros((3, 1))
self.acc = self.a_B
self.att = np.zeros((3, 1))
self.avel = np.zeros((3, 1))
# State history
self.time_true = np.array([0.0])
self.pos_true = np.zeros((3, 1))
self.vel_true = np.zeros((3, 1))
self.acc_true = self.a_B
self.att_true = np.zeros((3, 1))
# Counters
self.counter_frame_id = 0
self.counter_track_id = 0
# Feature tracks
self.features_tracking = []
self.features_buffer = {}
self.tracks_tracking = []
self.tracks_lost = []
self.tracks_buffer = {}
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_P(self):
# Setup camera model
K = camera_intrinsics(554.25, 554.25, 320.0, 320.0)
camera_model = PinholeCameraModel(640, 640, K)
# Test
R = np.eye(3)
t = np.array([0, 0, 0])
P = camera_model.P(R, t)
# Assert
feature = np.array([[0.0], [0.0], [10.0]])
x = np.dot(P, convert2homogeneous(feature))
expected = np.array([[320.0], [320.0], [1.0]])
# Normalize pixel coordinates
x[0] /= x[2]
x[1] /= x[2]
x[2] /= x[2]
x = np.array(x)
self.assertTrue(np.array_equal(x, expected))
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_constructor(self):
K = camera_intrinsics(554.25, 554.25, 320.0, 320.0)
camera = PinholeCameraModel(640, 640, K)
self.assertEqual(camera.image_width, 640)
self.assertEqual(camera.image_height, 640)
