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_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_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_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))
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()
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_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_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_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_update(self):
# Setup dataset
data = RawSequence(RAW_DATASET, "2011_09_26", "0005")
v0 = data.get_inertial_velocity(0)
q0 = euler2quat(data.get_attitude(0))
# Setup IMU state
n_g = np.ones((3, 1)) * 0.01 # Gyro Noise
n_a = np.ones((3, 1)) * 0.02 # Accel Noise
n_wg = np.ones((3, 1)) * 0.03 # Gyro Random Walk Noise
n_wa = np.ones((3, 1)) * 0.04 # Accel Random Walk Noise
n_imu = np.block([[n_g], [n_wg], [n_a], [n_wa]])
q_IG = q0
b_g = np.zeros((3, 1))
v_G = v0
b_a = np.zeros((3, 1))
p_G = np.zeros((3, 1))
imu_state = IMUState(q_IG, b_g, v_G, b_a, p_G, n_imu)
# Setup state history storage and covariance plot
pos_est = imu_state.p_G
vel_est = imu_state.v_G
att_est = quat2euler(imu_state.q_IG)
# labels = ["theta_x", "theta_y", "theta_z",
# "bx_g", "by_g", "bz_g",
# "vx", "vy", "vz",
# "bx_a", "by_a", "bz_a",
# "px", "py", "pz"]
# P_plot = PlotMatrix(imu_state.P,
# labels=labels,
# show_ticks=True,
# # show=True)
# show=False)
# fig = plt.figure()
# ax = fig.add_subplot(111)
# plt.show(block=False)
# Loop through data
for i in range(1, len(data.oxts)):
a_m, w_m = data.get_imu_measurements(i)
dt = data.get_dt(i)
imu_state.update(a_m, w_m, dt)
# P_plot.update(imu_state.P)
# plot_error_ellipse(imu_state.p_G[0:2])
# mean = imu_state.p_G[0:2].ravel()
# cov = imu_state.P[12:14, 12:14]
# ax.clear()
# plot_error_ellipse(mean, cov, ax)
# fig.canvas.draw()
pos = imu_state.p_G
vel = imu_state.v_G
att = quat2euler(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:
self.plot_position(data.get_local_position(), pos_est)
self.plot_velocity(data.get_timestamps(),
data.get_inertial_velocity(), vel_est)
self.plot_attitude(data.get_timestamps(), data.get_attitude(),
att_est)
plt.show()
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()