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_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 test_prediction_update(self):
# Setup
data = RawSequence(RAW_DATASET, "2011_09_26", "0005")
K = data.calib_cam2cam["K_00"].reshape((3, 3))
cam_model = PinholeCameraModel(1242, 375, K)
# Initialize MSCKF
msckf = MSCKF(n_g=1e-6 * np.ones(3),
n_a=1e-6 * np.ones(3),
n_wg=1e-6 * np.ones(3),
n_wa=1e-6 * np.ones(3),
imu_q_IG=euler2quat(data.get_attitude(0)),
imu_v_G=data.get_inertial_velocity(0),
cam_model=cam_model,
ext_p_IC=np.array([0.0, 0.0, 0.0]),
ext_q_CI=np.array([0.5, -0.5, 0.5, -0.5]))
# Setup state history storage and covariance plot
pos_est = msckf.imu_state.p_G
vel_est = msckf.imu_state.v_G
att_est = quat2euler(msckf.imu_state.q_IG)
# Loop through data
for i in range(1, len(data.oxts)):
# MSCKF prediction and measurement update
a_m, w_m = data.get_imu_measurements(i)
dt = data.get_dt(i)
msckf.prediction_update(a_m, w_m, dt)
msckf.augment_state()
# Store history
pos = msckf.imu_state.p_G
vel = msckf.imu_state.v_G
att = quat2euler(msckf.imu_state.q_IG)
pos_est = np.hstack((pos_est, pos))
vel_est = np.hstack((vel_est, vel))
att_est = np.hstack((att_est, att))
# Plot
# debug = True
debug = False
if debug:
# Position
self.plot_position(data.get_local_position(), pos_est,
msckf.cam_states)
# Velocity
self.plot_velocity(data.get_timestamps(),
data.get_inertial_velocity(), vel_est)
# Attitude
self.plot_attitude(data.get_timestamps(), data.get_attitude(),
att_est)
# data.plot_accelerometer()
# data.plot_gyroscope()
plt.show()
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_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):
# 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_project(self):
# Load points
points_file = join(test.TEST_DATA_PATH, "house/house.p3d")
points = np.loadtxt(points_file).T
# Setup camera
K = np.eye(3)
R = np.eye(3)
t = np.array([0, 0, 0])
camera = PinholeCameraModel(320, 240, K)
x = camera.project(points, R, t)
# Assert
self.assertEqual(x.shape, (3, points.shape[1]))
self.assertTrue(np.all(x[2, :] == 1.0))
# Plot projection
debug = False
# debug = True
if debug:
plt.figure()
plt.plot(x[0], x[1], 'k. ')
plt.show()
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_measurement_update2(self):
# Setup
# data = RawSequence(RAW_DATASET, "2011_09_26", "0001")
data = RawSequence(RAW_DATASET, "2011_09_26", "0005")
# data = RawSequence(RAW_DATASET, "2011_09_26", "0046")
# data = RawSequence(RAW_DATASET, "2011_09_26", "0036")
K = data.calib_cam2cam["P_rect_00"].reshape((3, 4))[0:3, 0:3]
cam_model = PinholeCameraModel(1242, 375, K)
# Initialize MSCKF
v0 = data.get_inertial_velocity(0)
q0 = euler2quat(data.get_attitude(0))
msckf = MSCKF(n_g=4e-2 * np.ones(3),
n_a=4e-2 * np.ones(3),
n_wg=1e-6 * np.ones(3),
n_wa=1e-6 * np.ones(3),
imu_q_IG=q0,
imu_v_G=v0,
cam_model=cam_model,
ext_p_IC=np.zeros((3, 1)),
ext_q_CI=np.array([0.49921, -0.49657, 0.50291,
-0.50129]))
# cov_plot = PlotMatrix(msckf.P())
# plt.show(block=False)
# Initialize feature tracker
img = cv2.imread(data.image_00_files[0])
tracker = FeatureTracker()
tracker.update(img)
# Setup state history storage and covariance plot
pos_est = msckf.imu_state.p_G
vel_est = msckf.imu_state.v_G
att_est = quat2euler(msckf.imu_state.q_IG)
# Loop through data
# for i in range(1, 100):
for i in range(1, len(data.oxts)):
print("frame %d" % i)
# Track features
img = cv2.imread(data.image_00_files[i])
# tracker.update(img, True)
# key = cv2.waitKey(1)
# if key == 113:
# exit(0)
tracker.update(img)
tracks = tracker.remove_lost_tracks()
# Accelerometer and gyroscope and dt measurements
a_m, w_m = data.get_imu_measurements(i)
dt = data.get_dt(i)
# MSCKF prediction and measurement update
msckf.prediction_update(a_m, w_m, dt)
msckf.measurement_update(tracks)
# cov_plot.update(msckf.P())
# Store history
pos = msckf.imu_state.p_G
vel = msckf.imu_state.v_G
att = quat2euler(msckf.imu_state.q_IG)
pos_est = np.hstack((pos_est, pos))
vel_est = np.hstack((vel_est, vel))
att_est = np.hstack((att_est, att))
# Plot
debug = True
# debug = False
if debug:
# Position
self.plot_position(data.get_local_position(), pos_est,
msckf.cam_states)
# Velocity
self.plot_velocity(data.get_timestamps(),
data.get_inertial_velocity(), vel_est)
# Attitude
self.plot_attitude(data.get_timestamps(), data.get_attitude(),
att_est)
# data.plot_accelerometer()
# data.plot_gyroscope()
plt.show()
class MSCKFTest(unittest.TestCase):
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 plot_position(self, pos_true, pos_est, cam_states):
N = pos_est.shape[1]
pos_true = pos_true[:, :N]
pos_est = pos_est[:, :N]
# Figure
plt.figure()
plt.suptitle("Position")
# Ground truth
plt.plot(pos_true[0, :],
pos_true[1, :],
color="red",
label="Grouth truth")
# Estimated
plt.plot(pos_est[0, :], pos_est[1, :], color="blue", label="Estimated")
# Sliding window
cam_pos = []
for cam_state in cam_states:
cam_pos.append(cam_state.p_G)
cam_pos = np.array(cam_pos).reshape((len(cam_pos), 3)).T
plt.plot(cam_pos[0, :],
cam_pos[1, :],
color="green",
label="Camera Poses")
# Plot labels and legends
plt.xlabel("East (m)")
plt.ylabel("North (m)")
plt.axis("equal")
plt.legend(loc=0)
def plot_velocity(self, timestamps, vel_true, vel_est):
N = vel_est.shape[1]
t = timestamps[:N]
vel_true = vel_true[:, :N]
vel_est = vel_est[:, :N]
# Figure
plt.figure()
plt.suptitle("Velocity")
# X axis
plt.subplot(311)
plt.plot(t, vel_true[0, :], color="red", label="Ground_truth")
plt.plot(t, vel_est[0, :], color="blue", label="Estimate")
plt.title("x-axis")
plt.xlabel("Date Time")
plt.ylabel("ms^-1")
plt.legend(loc=0)
# Y axis
plt.subplot(312)
plt.plot(t, vel_true[1, :], color="red", label="Ground_truth")
plt.plot(t, vel_est[1, :], color="blue", label="Estimate")
plt.title("y-axis")
plt.xlabel("Date Time")
plt.ylabel("ms^-1")
plt.legend(loc=0)
# Z axis
plt.subplot(313)
plt.plot(t, vel_true[2, :], color="red", label="Ground_truth")
plt.plot(t, vel_est[2, :], color="blue", label="Estimate")
plt.title("z-axis")
plt.xlabel("Date Time")
plt.ylabel("ms^-1")
plt.legend(loc=0)
def plot_attitude(self, timestamps, att_true, att_est):
# Setup
N = att_est.shape[1]
t = timestamps[:N]
att_true = att_true[:, :N]
att_est = att_est[:, :N]
# Figure
plt.figure()
plt.suptitle("Attitude")
# X axis
plt.subplot(311)
plt.plot(t, att_true[0, :], color="red", label="Ground_truth")
plt.plot(t, att_est[0, :], color="blue", label="Estimate")
plt.title("x-axis")
plt.legend(loc=0)
plt.xlabel("Date Time")
plt.ylabel("radians")
# Y axis
plt.subplot(312)
plt.plot(t, att_true[1, :], color="red", label="Ground_truth")
plt.plot(t, att_est[1, :], color="blue", label="Estimate")
plt.title("y-axis")
plt.legend(loc=0)
plt.xlabel("Date Time")
plt.ylabel("radians")
# Z axis
plt.subplot(313)
plt.plot(t, att_true[2, :], color="red", label="Ground_truth")
plt.plot(t, att_est[2, :], color="blue", label="Estimate")
plt.title("z-axis")
plt.legend(loc=0)
plt.xlabel("Date Time")
plt.ylabel("radians")
def test_init(self):
self.assertEqual(self.msckf.N(), 1)
self.assertTrue(self.msckf.P_cam is not None)
self.assertTrue(self.msckf.P_imu_cam is not None)
self.assertEqual(self.msckf.P_cam.shape, (6, 6))
self.assertEqual(self.msckf.P_imu_cam.shape, (15, 6))
# Plot matrix
# debug = True
debug = False
if debug:
ax = plt.subplot(311)
ax.matshow(self.msckf.P())
ax = plt.subplot(312)
ax.matshow(self.msckf.P_cam)
ax = plt.subplot(313)
ax.matshow(self.msckf.P_imu_cam)
plt.show()
def test_P(self):
self.assertEqual(self.msckf.P().shape, (21, 21))
# Plot matrix
# debug = True
debug = False
if debug:
ax = plt.subplot(111)
ax.matshow(self.msckf.P())
plt.show()
def test_N(self):
self.assertEqual(self.msckf.N(), 1)
def test_H(self):
# Setup feature track
track_id = 0
frame_id = 3
data0 = KeyPoint(np.array([0.0, 0.0]), 21)
data1 = KeyPoint(np.array([0.0, 0.0]), 21)
track = FeatureTrack(track_id, frame_id, data0, data1)
# Setup track cam states
self.msckf.augment_state()
self.msckf.augment_state()
self.msckf.augment_state()
track_cam_states = self.msckf.track_cam_states(track)
# Feature position
p_G_f = np.array([[1.0], [2.0], [3.0]])
# Test
H_f_j, H_x_j = self.msckf.H(track, track_cam_states, p_G_f)
# Assert
self.assertEqual(H_f_j.shape, (4, 3))
self.assertEqual(H_x_j.shape, (4, 39))
# Plot matrix
debug = True
# debug = False
if debug:
plt.matshow(H_f_j)
plt.show()
plt.matshow(H_x_j)
plt.show()
def test_augment_state(self):
self.msckf.augment_state()
N = self.msckf.N()
self.assertTrue(self.msckf.P_cam is not None)
self.assertTrue(self.msckf.P_imu_cam is not None)
self.assertEqual(self.msckf.P_cam.shape, (N * 6, N * 6))
self.assertEqual(self.msckf.P_imu_cam.shape, (15, N * 6))
self.assertEqual(self.msckf.N(), 2)
self.assertTrue(
np.array_equal(self.msckf.cam_states[0].q_CG, self.msckf.ext_q_CI))
self.assertEqual(self.msckf.counter_frame_id, 2)
# Plot matrix
# debug = True
debug = False
if debug:
ax = plt.subplot(111)
ax.matshow(self.msckf.P())
plt.show()
def test_track_cam_states(self):
# Setup feature track
track_id = 0
frame_id = 3
data0 = KeyPoint(np.array([0.0, 0.0]), 21)
data1 = KeyPoint(np.array([0.0, 0.0]), 21)
track = FeatureTrack(track_id, frame_id, data0, data1)
# Push dummy camera states into MSCKF
self.msckf.augment_state()
self.msckf.augment_state()
self.msckf.augment_state()
self.msckf.augment_state()
# Test
track_cam_states = self.msckf.track_cam_states(track)
# Assert
self.assertEqual(len(track_cam_states), track.tracked_length())
self.assertEqual(track_cam_states[0].frame_id, track.frame_start)
self.assertEqual(track_cam_states[1].frame_id, track.frame_end)
def test_prediction_update(self):
# Setup
data = RawSequence(RAW_DATASET, "2011_09_26", "0005")
K = data.calib_cam2cam["K_00"].reshape((3, 3))
cam_model = PinholeCameraModel(1242, 375, K)
# Initialize MSCKF
msckf = MSCKF(n_g=1e-6 * np.ones(3),
n_a=1e-6 * np.ones(3),
n_wg=1e-6 * np.ones(3),
n_wa=1e-6 * np.ones(3),
imu_q_IG=euler2quat(data.get_attitude(0)),
imu_v_G=data.get_inertial_velocity(0),
cam_model=cam_model,
ext_p_IC=np.array([0.0, 0.0, 0.0]),
ext_q_CI=np.array([0.5, -0.5, 0.5, -0.5]))
# Setup state history storage and covariance plot
pos_est = msckf.imu_state.p_G
vel_est = msckf.imu_state.v_G
att_est = quat2euler(msckf.imu_state.q_IG)
# Loop through data
for i in range(1, len(data.oxts)):
# MSCKF prediction and measurement update
a_m, w_m = data.get_imu_measurements(i)
dt = data.get_dt(i)
msckf.prediction_update(a_m, w_m, dt)
msckf.augment_state()
# Store history
pos = msckf.imu_state.p_G
vel = msckf.imu_state.v_G
att = quat2euler(msckf.imu_state.q_IG)
pos_est = np.hstack((pos_est, pos))
vel_est = np.hstack((vel_est, vel))
att_est = np.hstack((att_est, att))
# Plot
# debug = True
debug = False
if debug:
# Position
self.plot_position(data.get_local_position(), pos_est,
msckf.cam_states)
# Velocity
self.plot_velocity(data.get_timestamps(),
data.get_inertial_velocity(), vel_est)
# Attitude
self.plot_attitude(data.get_timestamps(), data.get_attitude(),
att_est)
# data.plot_accelerometer()
# data.plot_gyroscope()
plt.show()
def test_prediction_update2(self):
# Setup
dataset = DatasetGenerator()
# Initialize MSCKF
msckf = MSCKF(n_g=1e-6 * np.ones(3),
n_a=1e-6 * np.ones(3),
n_wg=1e-6 * np.ones(3),
n_wa=1e-6 * np.ones(3),
imu_q_IG=dataset.vel,
imu_v_G=euler2quat(np.zeros((3, 1))),
cam_model=dataset.cam_model,
ext_p_IC=np.array([0.0, 0.0, 0.0]),
ext_q_CI=np.array([0.0, 0.0, 0.0, 1.0]))
# Loop through data
for i in range(1, 30):
# MSCKF prediction and measurement update
a_m, w_m = dataset.step()
dt = dataset.dt
msckf.prediction_update(a_m, w_m, dt)
# Plot
# debug = True
debug = False
if debug:
# Position
self.plot_position(dataset.pos_true, msckf.pos_est,
msckf.cam_states)
# Velocity
self.plot_velocity(dataset.time_true, dataset.vel_true,
msckf.vel_est)
# Attitude
self.plot_attitude(dataset.time_true, dataset.rpy_true,
msckf.att_est)
# data.plot_accelerometer()
# data.plot_gyroscope()
plt.show()
def test_track_residuals(self):
# Setup track cam states
# -- Camera state 0
self.msckf.imu_state.p_G = np.array([0.0, 0.0, 0.0]).reshape((3, 1))
self.msckf.augment_state()
# -- Camera state 1
self.msckf.imu_state.p_G = np.array([1.0, 1.0, 0.0]).reshape((3, 1))
self.msckf.augment_state()
# -- Camera state 2
self.msckf.imu_state.p_G = np.array([2.0, 2.0, 0.0]).reshape((3, 1))
self.msckf.augment_state()
# -- Camera state 3
self.msckf.imu_state.p_G = np.array([3.0, 3.0, 0.0]).reshape((3, 1))
self.msckf.augment_state()
# Setup feature
p_G_f = np.array([[10.0], [1.0], [0.0]])
# Setup feature track
C_C2G = C(self.msckf.cam_states[2].q_CG)
C_C3G = C(self.msckf.cam_states[3].q_CG)
p_G_C2 = self.msckf.cam_states[2].p_G
p_G_C3 = self.msckf.cam_states[3].p_G
kp1 = self.cam_model.project(p_G_f, C_C2G, p_G_C2.reshape((3, 1)))[0:2]
kp2 = self.cam_model.project(p_G_f, C_C3G, p_G_C3.reshape((3, 1)))[0:2]
track_id = 0
frame_id = 3
data0 = KeyPoint(kp1, 21)
data1 = KeyPoint(kp2, 21)
track = FeatureTrack(track_id, frame_id, data0, data1)
# Test
track_cam_states = self.msckf.track_cam_states(track)
r_j = self.msckf.track_residuals(self.cam_model, track,
track_cam_states, p_G_f)
# Assert
self.assertEqual(r_j.shape, (4, 1))
self.assertTrue(np.allclose(r_j, np.zeros((4, 1))))
def test_residualize_track(self):
# Camera 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)
# -- Camera state 1
p_G_C1 = np.array([1.0, 1.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)
# Features
landmark = np.array([0.0, 0.0, 10.0])
kp1 = self.cam_model.project(landmark, C_C0G, p_G_C0)[0:2]
kp2 = self.cam_model.project(landmark, C_C1G, p_G_C1)[0:2]
# Setup feature track
track_id = 0
frame_id = 1
data1 = KeyPoint(kp1, 21)
data2 = KeyPoint(kp2, 21)
track = FeatureTrack(track_id, frame_id, data1, data2)
# Setup track cam states
self.msckf.augment_state()
self.msckf.min_track_length = 2
self.msckf.cam_states[0].p_G = p_G_C0.reshape((3, 1))
self.msckf.cam_states[0].q_CG = q_C0G.reshape((4, 1))
self.msckf.cam_states[1].p_G = p_G_C1.reshape((3, 1))
self.msckf.cam_states[1].q_CG = q_C1G.reshape((4, 1))
print(self.msckf.cam_states[0])
print(self.msckf.cam_states[1])
# Test
# self.msckf.enable_ns_trick = False
H_o_j, r_o_j, R_o_j = self.msckf.residualize_track(track)
plt.matshow(H_o_j)
plt.show()
# plt.matshow(r_o_j)
# plt.show()
# plt.matshow(R_o_j)
# plt.show()
def test_calculate_residuals(self):
# Camera 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)
# -- Camera state 1
p_G_C1 = np.array([1.0, 1.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)
# Features
landmark = np.array([0.0, 0.0, 10.0])
kp1 = self.cam_model.project(landmark, C_C0G, p_G_C0)[0:2]
kp2 = self.cam_model.project(landmark, C_C1G, p_G_C1)[0:2]
# Setup feature track
track_id = 0
frame_id = 1
data1 = KeyPoint(kp1, 21)
data2 = KeyPoint(kp2, 21)
track = FeatureTrack(track_id, frame_id, data1, data2)
# Setup track cam states
self.msckf.augment_state()
self.msckf.min_track_length = 2
self.msckf.cam_states[0].p_G = p_G_C0.reshape((3, 1))
self.msckf.cam_states[0].q_CG = q_C0G.reshape((4, 1))
self.msckf.cam_states[1].p_G = p_G_C1.reshape((3, 1))
self.msckf.cam_states[1].q_CG = q_C1G.reshape((4, 1))
# Test
self.msckf.calculate_residuals([track])
def test_measurement_update(self):
# Setup
np.random.seed(0)
dataset = DatasetGenerator(dt=0.1)
# Initialize MSCKF
msckf = MSCKF(n_g=4.2e-2 * np.ones(3),
n_a=4.2e-2 * np.ones(3),
n_wg=1e-6 * np.ones(3),
n_wa=1e-6 * np.ones(3),
imu_q_IG=euler2quat(np.zeros((3, 1))),
imu_v_G=dataset.vel,
cam_model=dataset.cam_model,
ext_p_IC=np.array([0.0, 0.0, 0.0]),
ext_q_CI=np.array([0.5, -0.5, 0.5, -0.5]))
# feature_estimator=DatasetFeatureEstimator())
# msckf.enable_qr_trick = True
# msckf.enable_ns_trick = True
# cov_plot = PlotMatrix(msckf.P())
# gain_plot = PlotMatrix(msckf.K)
# plt.show(block=False)
# Setup state history storage and covariance plot
pos_est = msckf.imu_state.p_G
vel_est = msckf.imu_state.v_G
att_est = quat2euler(msckf.imu_state.q_IG)
dataset.step()
# Loop through data
for i in range(1, 100):
# Prediction update
a_m, w_m = dataset.step()
dt = dataset.dt
msckf.prediction_update(a_m, w_m, dt)
# msckf.imu_state.v_G = dataset.vel
# msckf.imu_state.p_G = dataset.pos
# Measurement update
tracks = dataset.remove_lost_tracks()
retval = msckf.measurement_update(tracks)
# if retval == 'q':
# break
# cov_plot.update(msckf.P())
# gain_plot.update(msckf.K)
# Record states
pos = msckf.imu_state.p_G
vel = msckf.imu_state.v_G
att = quat2euler(msckf.imu_state.q_IG)
pos_est = np.hstack((pos_est, pos))
vel_est = np.hstack((vel_est, vel))
att_est = np.hstack((att_est, att))
print("frame: %d, window size: %d, updated?: %r" %
(i, len(msckf.cam_states), retval))
# Plot
debug = True
# debug = False
if debug:
# Position
self.plot_position(dataset.pos_true, pos_est, msckf.cam_states)
# Velocity
self.plot_velocity(dataset.time_true, dataset.vel_true, vel_est)
# Attitude
self.plot_attitude(dataset.time_true, dataset.att_true, att_est)
# data.plot_accelerometer()
# data.plot_gyroscope()
plt.show()
def test_measurement_update2(self):
# Setup
# data = RawSequence(RAW_DATASET, "2011_09_26", "0001")
data = RawSequence(RAW_DATASET, "2011_09_26", "0005")
# data = RawSequence(RAW_DATASET, "2011_09_26", "0046")
# data = RawSequence(RAW_DATASET, "2011_09_26", "0036")
K = data.calib_cam2cam["P_rect_00"].reshape((3, 4))[0:3, 0:3]
cam_model = PinholeCameraModel(1242, 375, K)
# Initialize MSCKF
v0 = data.get_inertial_velocity(0)
q0 = euler2quat(data.get_attitude(0))
msckf = MSCKF(n_g=4e-2 * np.ones(3),
n_a=4e-2 * np.ones(3),
n_wg=1e-6 * np.ones(3),
n_wa=1e-6 * np.ones(3),
imu_q_IG=q0,
imu_v_G=v0,
cam_model=cam_model,
ext_p_IC=np.zeros((3, 1)),
ext_q_CI=np.array([0.49921, -0.49657, 0.50291,
-0.50129]))
# cov_plot = PlotMatrix(msckf.P())
# plt.show(block=False)
# Initialize feature tracker
img = cv2.imread(data.image_00_files[0])
tracker = FeatureTracker()
tracker.update(img)
# Setup state history storage and covariance plot
pos_est = msckf.imu_state.p_G
vel_est = msckf.imu_state.v_G
att_est = quat2euler(msckf.imu_state.q_IG)
# Loop through data
# for i in range(1, 100):
for i in range(1, len(data.oxts)):
print("frame %d" % i)
# Track features
img = cv2.imread(data.image_00_files[i])
# tracker.update(img, True)
# key = cv2.waitKey(1)
# if key == 113:
# exit(0)
tracker.update(img)
tracks = tracker.remove_lost_tracks()
# Accelerometer and gyroscope and dt measurements
a_m, w_m = data.get_imu_measurements(i)
dt = data.get_dt(i)
# MSCKF prediction and measurement update
msckf.prediction_update(a_m, w_m, dt)
msckf.measurement_update(tracks)
# cov_plot.update(msckf.P())
# Store history
pos = msckf.imu_state.p_G
vel = msckf.imu_state.v_G
att = quat2euler(msckf.imu_state.q_IG)
pos_est = np.hstack((pos_est, pos))
vel_est = np.hstack((vel_est, vel))
att_est = np.hstack((att_est, att))
# Plot
debug = True
# debug = False
if debug:
# Position
self.plot_position(data.get_local_position(), pos_est,
msckf.cam_states)
# Velocity
self.plot_velocity(data.get_timestamps(),
data.get_inertial_velocity(), vel_est)
# Attitude
self.plot_attitude(data.get_timestamps(), data.get_attitude(),
att_est)
# data.plot_accelerometer()
# data.plot_gyroscope()
plt.show()
def test_sandbox(self):
p_I_C = np.array([1.0, 2.0, 3.0])
p_G_I = np.array([1.0, 0.0, 0.0])
rpy_IG = np.array([deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)])
q_IG = euler2quat(rpy_IG)
C_IG = C(q_IG)
print(p_G_I + np.dot(C_IG, p_I_C))
class FeatureEstimatorTest(unittest.TestCase):
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_triangulate(self):
# Camera 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)
# -- Camera state 1
p_G_C1 = np.array([1.0, 1.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)
# Features
landmark = np.array([0.0, 0.0, 10.0])
kp1 = self.cam_model.project(landmark, C_C0G, p_G_C0)[0:2]
kp2 = self.cam_model.project(landmark, C_C1G, p_G_C1)[0:2]
# Calculate rotation and translation of first and last camera states
# -- Obtain rotation and translation from camera 0 to camera 1
C_C0C1 = dot(C_C0G, C_C1G.T)
t_C0_C1C0 = dot(C_C0G, (p_G_C1 - p_G_C0))
# -- Convert from pixel coordinates to image coordinates
pt1 = self.cam_model.pixel2image(kp1)
pt2 = self.cam_model.pixel2image(kp2)
# Triangulate
p_C0_C1C0, r = self.estimator.triangulate(pt1, pt2, C_C0C1, t_C0_C1C0)
# Assert
self.assertTrue(np.allclose(p_C0_C1C0.ravel(), landmark))
def test_estimate(self):
nb_features = 100
bounds = {
"x": {"min": 5.0, "max": 10.0},
"y": {"min": -1.0, "max": 1.0},
"z": {"min": -1.0, "max": 1.0}
}
features = rand3dfeatures(nb_features, bounds)
dt = 0.1
p_G = np.array([0.0, 0.0, 0.0])
v_G = np.array([0.1, 0.0, 0.0])
q_CG = np.array([0.5, -0.5, 0.5, -0.5])
# Setup camera states
track_cam_states = []
for i in range(10):
p_G = p_G + v_G * dt
track_cam_states.append(CameraState(i, q_CG, p_G))
# Feature Track
track_length = 10
start = 0
end = track_length
for i in range(10):
feature_idx = random.randint(0, features.shape[1] - 1)
feature = T_camera_global * features[:, feature_idx]
print("feature in global frame:",
(T_global_camera * feature).ravel())
R_C0G = dot(R_global_camera, C(track_cam_states[0].q_CG))
R_C1G = dot(R_global_camera, C(track_cam_states[1].q_CG))
p_C_C0 = T_camera_global * track_cam_states[0].p_G
p_C_C1 = T_camera_global * track_cam_states[1].p_G
kp1 = self.cam_model.project(feature, R_C0G, p_C_C0)
kp2 = self.cam_model.project(feature, R_C1G, p_C_C1)
kp1 = KeyPoint(kp1.ravel()[:2], 21)
kp2 = KeyPoint(kp2.ravel()[:2], 21)
track = FeatureTrack(start, end, kp1, kp2)
track.ground_truth = T_global_camera * feature
for i in range(2, track_length):
R_CG = dot(R_global_camera, C(track_cam_states[i].q_CG))
p_C_Ci = T_camera_global * track_cam_states[i].p_G
kp = self.cam_model.project(feature, R_CG, p_C_Ci)
kp = KeyPoint(kp.ravel()[:2], 21)
# kp[0] += np.random.normal(0, 0.1)
# kp[1] += np.random.normal(0, 0.1)
track.update(i, kp)
# Estimate feature
p_G_f = self.estimator.estimate(self.cam_model,
track,
track_cam_states)
print("estimation: ", p_G_f.ravel())
print()
# Debug
# debug = False
# debug = True
# if debug:
# C_CG = C(track_cam_states[-1].q_CG)
# p_G_C = track_cam_states[-1].p_G
# p_C_f = dot(C_CG, (p_G_f - p_G_C))
# Assert
feature_G = T_global_camera * feature
self.assertTrue(abs(p_G_f[0, 0] - feature_G[0]) < 0.1)
self.assertTrue(abs(p_G_f[1, 0] - feature_G[1]) < 0.1)
self.assertTrue(abs(p_G_f[2, 0] - feature_G[2]) < 0.1)
@unittest.skip("skip")
def test_estimate2(self):
# Load RAW KITTI dataset
data = RawSequence(RAW_DATASET, "2011_09_26", "0001")
# data = RawSequence(RAW_DATASET, "2011_09_26", "0046")
# data = RawSequence(RAW_DATASET, "2011_09_26", "0005")
K = data.calib_cam2cam["P_rect_00"].reshape((3, 4))[0:3, 0:3]
cam_model = PinholeCameraModel(1242, 375, K)
# Initialize feature tracker
img = cv2.imread(data.image_00_files[0])
tracker = FeatureTracker()
tracker.update(img)
# Setup plot
features = None
features_plot = None
pos_data = data.get_local_position(0)
debug = False
if debug:
fig = plt.figure()
plt.ion()
ax = fig.add_subplot(111)
pos_plot = ax.plot(pos_data[0], pos_data[1],
marker=".", color="blue")[0]
# ax.set_xlim([-60.0, 5.0])
# ax.set_ylim([-60.0, 5.0])
ax.set_xlim([0.0, 50.0])
ax.set_ylim([-100.0, 0.0])
fig.canvas.draw()
plt.show(block=False)
# Setup feature tracks
tracks = []
for i in range(1, 10):
# Track features
img = cv2.imread(data.image_00_files[i])
tracker.update(img, True)
if cv2.waitKey(1) == 113:
exit(0)
tracks = tracker.remove_lost_tracks()
# Loop feature tracks
p_G_f = None
for track in tracks:
if track.tracked_length() < 8:
continue
# Setup feature track camera states
track_cam_states = []
for j in range(track.tracked_length()):
frame_id = track.frame_start + j
imu_q_IG = euler2quat(data.get_attitude(frame_id))
imu_p_G = data.get_local_position(frame_id)
ext_q_CI = np.array([0.5, -0.5, 0.5, -0.5])
cam_q_IG = dot(quatlcomp(ext_q_CI), imu_q_IG)
cam_p_G = imu_p_G
cam_state = CameraState(frame_id, cam_q_IG, cam_p_G)
track_cam_states.append(cam_state)
# Estimate feature track
p_G_f = self.estimator.estimate(cam_model,
track,
track_cam_states)
if p_G_f is not None:
C_CG = C(track_cam_states[-1].q_CG)
p_G_C = track_cam_states[-1].p_G
p_C_f = dot(C_CG, (p_G_f - p_G_C))
print("p_G_f: ", p_G_f.ravel())
print("p_C_f: ", p_C_f.ravel())
print()
# Plot
pos = data.get_local_position(i)
pos_data = np.hstack((pos_data, pos))
if debug:
if features is None and p_G_f is not None:
features = p_G_f
features_plot = ax.plot(p_G_f[0], p_G_f[1],
marker="x", color="red", ls='')[0]
elif p_G_f is not None:
features = np.hstack((features, p_G_f))
features_plot.set_xdata(features[0, :])
features_plot.set_ydata(features[1, :])
pos_plot.set_xdata(pos_data[0, :])
pos_plot.set_ydata(pos_data[1, :])
ax.relim()
ax.autoscale_view(True, True, True)
fig.canvas.draw()
class GeometryTest(unittest.TestCase):
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 project_points(self, features, camera, R, t):
obs = []
# Make 3D feature homogenenous and project and store pixel measurement
for f in features.T:
x = camera.project(f, R, t)
obs.append(x.ravel()[0:2])
return np.array(obs).T
def test_triangulate_point(self):
# Triangulate a single point
x1 = self.obs0[:, 0]
x2 = self.obs1[:, 0]
P1 = self.cam.P(self.R_0, self.t_0)
P2 = self.cam.P(self.R_1, self.t_1)
X = triangulate_point(x1, x2, P1, P2)
X = X[0:3]
# Assert
self.assertTrue(np.linalg.norm(X - self.features[:, 0]) < 0.1)
def test_triangulate(self):
# Triangulate a set of features
x1 = self.obs0
x2 = self.obs1
P1 = self.cam.P(self.R_0, self.t_0)
P2 = self.cam.P(self.R_1, self.t_1)
result = triangulate(x1, x2, P1, P2)
# Assert
for i in range(result.shape[1]):
X = result[:3, i]
self.assertTrue(np.linalg.norm(X - self.features[:, i]) < 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)
def test_estimate2(self):
# Load RAW KITTI dataset
data = RawSequence(RAW_DATASET, "2011_09_26", "0001")
# data = RawSequence(RAW_DATASET, "2011_09_26", "0046")
# data = RawSequence(RAW_DATASET, "2011_09_26", "0005")
K = data.calib_cam2cam["P_rect_00"].reshape((3, 4))[0:3, 0:3]
cam_model = PinholeCameraModel(1242, 375, K)
# Initialize feature tracker
img = cv2.imread(data.image_00_files[0])
tracker = FeatureTracker()
tracker.update(img)
# Setup plot
features = None
features_plot = None
pos_data = data.get_local_position(0)
debug = False
if debug:
fig = plt.figure()
plt.ion()
ax = fig.add_subplot(111)
pos_plot = ax.plot(pos_data[0], pos_data[1],
marker=".", color="blue")[0]
# ax.set_xlim([-60.0, 5.0])
# ax.set_ylim([-60.0, 5.0])
ax.set_xlim([0.0, 50.0])
ax.set_ylim([-100.0, 0.0])
fig.canvas.draw()
plt.show(block=False)
# Setup feature tracks
tracks = []
for i in range(1, 10):
# Track features
img = cv2.imread(data.image_00_files[i])
tracker.update(img, True)
if cv2.waitKey(1) == 113:
exit(0)
tracks = tracker.remove_lost_tracks()
# Loop feature tracks
p_G_f = None
for track in tracks:
if track.tracked_length() < 8:
continue
# Setup feature track camera states
track_cam_states = []
for j in range(track.tracked_length()):
frame_id = track.frame_start + j
imu_q_IG = euler2quat(data.get_attitude(frame_id))
imu_p_G = data.get_local_position(frame_id)
ext_q_CI = np.array([0.5, -0.5, 0.5, -0.5])
cam_q_IG = dot(quatlcomp(ext_q_CI), imu_q_IG)
cam_p_G = imu_p_G
cam_state = CameraState(frame_id, cam_q_IG, cam_p_G)
track_cam_states.append(cam_state)
# Estimate feature track
p_G_f = self.estimator.estimate(cam_model,
track,
track_cam_states)
if p_G_f is not None:
C_CG = C(track_cam_states[-1].q_CG)
p_G_C = track_cam_states[-1].p_G
p_C_f = dot(C_CG, (p_G_f - p_G_C))
print("p_G_f: ", p_G_f.ravel())
print("p_C_f: ", p_C_f.ravel())
print()
# Plot
pos = data.get_local_position(i)
pos_data = np.hstack((pos_data, pos))
if debug:
if features is None and p_G_f is not None:
features = p_G_f
features_plot = ax.plot(p_G_f[0], p_G_f[1],
marker="x", color="red", ls='')[0]
elif p_G_f is not None:
features = np.hstack((features, p_G_f))
features_plot.set_xdata(features[0, :])
features_plot.set_ydata(features[1, :])
pos_plot.set_xdata(pos_data[0, :])
pos_plot.set_ydata(pos_data[1, :])
ax.relim()
ax.autoscale_view(True, True, True)
fig.canvas.draw()
class DatasetGenerator(object):
"""Dataset Generator
Attributes
----------
camera_model : PinholeCameraModel
Camera model
nb_features : int
Number of features
feature_bounds : dict
feature_bounds = {
"x": {"min": -10.0, "max": 10.0},
"y": {"min": -10.0, "max": 10.0},
"z": {"min": -10.0, "max": 10.0}
}
counter_frame_id : int
Counter Frame ID
counter_track_id : int
Counter Track ID
tracks_tracking : :obj`list` of :obj`int`
List of feature track id
tracks_lost : :obj`list` of :obj`int`
List of lost feature track id
tracks_buffer : :obj`dict` of :obj`FeatureTrack`
Tracks buffer
max_buffer_size : int
Max buffer size (Default: 5000)
img_ref : np.array
Reference image
fea_ref :
Reference feature
unmatched : :obj`list` of `Feature`
List of features
"""
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 = {}
# self.detect(self.pos, self.att)
def debug(self, string):
if self.debug_mode:
print(string)
def add_feature_track(self, feature_id, kp, pos, rpy):
"""Add feature track
Parameters
----------
feature_id : int
Feature id
kp : KeyPoint
KeyPoint
"""
frame_id = self.counter_frame_id
track_id = self.counter_track_id
ground_truth = self.get_feature_position(feature_id)
ground_truth = T_global_camera * ground_truth
track = FeatureTrack(track_id,
frame_id,
kp,
ground_truth=ground_truth,
pos=[pos],
rpy=[rpy])
self.features_tracking.append(feature_id)
self.features_buffer[feature_id] = track_id
self.tracks_tracking.append(track_id)
self.tracks_buffer[track_id] = track
self.counter_track_id += 1
self.debug("+ [track_id: %d, feature_id: %d]" % (track_id, feature_id))
def remove_feature_track(self, feature_id):
"""Remove feature track
Parameters
----------
feature_id : int
Feature id
"""
track_id = self.features_buffer.pop(feature_id)
track = self.tracks_buffer.pop(track_id)
self.features_tracking.remove(feature_id)
self.tracks_tracking.remove(track_id)
self.tracks_lost.append(track)
self.debug("- [track_id: %d, feature_id: %d]" % (track_id, feature_id))
def update_feature_track(self, feature_id, kp, pos, rpy):
"""Update feature track
Parameters
----------
feature_id : int
Feature id
kp : KeyPoint
KeyPoint
"""
track_id = self.features_buffer[feature_id]
track = self.tracks_buffer[track_id]
if track.tracked_length() > 20:
self.remove_feature_track(feature_id)
else:
track.update(self.counter_frame_id, kp, pos, rpy)
self.debug("Update [track_id: %d, feature_id: %d]" %
(track_id, feature_id))
def detect(self, pos, rpy):
"""Update tracker with current image
Parameters
----------
pos : np.array
Robot position (x, y, z)
rpy : np.array
Robot attitude (roll, pitch, yaw)
"""
# Convert both euler angles and translation from NWU to EDN
# Note: We assume here that we are using a robot model
# where x = [pos_x, pos_y, theta] in world frame
rpy = T_camera_global * rpy # Motion model only modelled yaw
t = T_camera_global * pos # Translation
# Obtain list of features observed at this time step
fea_cur = self.cam_model.observed_features(self.features, rpy, t)
if fea_cur is None:
return
# Remove lost feature tracks
feature_ids_cur = [feature_id for _, feature_id in list(fea_cur)]
for feature_id in list(self.features_tracking):
if feature_id not in feature_ids_cur:
self.remove_feature_track(feature_id)
# Add or update feature tracks
for feature in fea_cur:
kp, feature_id = feature
kp = KeyPoint(kp, 0)
if feature_id not in self.features_tracking:
self.add_feature_track(feature_id, kp, pos, rpy)
else:
self.update_feature_track(feature_id, kp, pos, rpy)
self.debug("tracks_tracking: {}".format(self.tracks_tracking))
self.debug("features_tracking: {}\n".format(self.features_tracking))
self.counter_frame_id += 1
def remove_lost_tracks(self):
"""Remove lost tracks"""
if len(self.tracks_lost) == 0:
return []
# Make a copy of current lost tracks
lost_tracks = []
for track in self.tracks_lost:
if track.tracked_length() > 15:
lost_tracks.append(track)
# Reset tracks lost array
self.tracks_lost = []
# Filter and return lost tracks
if len(lost_tracks) > 500:
return lost_tracks[:500]
else:
return lost_tracks
def get_feature_position(self, feature_id):
"""Returns feature position"""
return self.features[:, feature_id].reshape((3, 1))
def step(self):
"""Step
Returns
-------
(a_B, w_B) : (np.array 3x1, np.array 3x1)
Accelerometer and Gyroscope measurement in body frame (mimicks IMU
measurements)
"""
# Update motion model
self.pos = self.pos + self.vel * self.dt
self.vel = self.vel + self.acc * self.dt
self.a_B = self.a_B + np.random.normal(0.0, 0.001, 3).reshape((3, 1))
self.w_B = self.w_B + np.random.normal(0.0, 0.001, 3).reshape((3, 1))
self.acc = dot(R(self.att, 321), self.a_B)
self.att = self.att + dot(R(self.att, 321), self.w_B) * self.dt
# Check feature
self.detect(self.pos, self.att)
# Update
self.t += self.dt
# Keep track
self.time_true = np.hstack((self.time_true, self.t))
self.pos_true = np.hstack((self.pos_true, self.pos))
self.vel_true = np.hstack((self.vel_true, self.vel))
self.acc_true = np.hstack((self.acc_true, self.a_B))
self.att_true = np.hstack((self.att_true, self.att))
imu_accel = self.a_B + np.random.normal(0.0, 0.05)
imu_gyro = self.w_B + np.random.normal(0.0, 0.05)
# imu_accel = self.a_B
# imu_gyro = self.w_B
return (imu_accel, imu_gyro)
def simulate_test_data(self):
"""Simulate test data"""
for i in range(300):
self.step()
