def initialize_frame(self, time, image, measure):
nf = super(VisualGPSNav, self).initialize_frame(time, image, measure)
if nf.measure:
old_kf = [kf for kf in self.state.keyframes if kf.measure]
nf.measure.time_adj = old_kf[-1].measure.time_adj if len(old_kf) > 0 else 0
geodetic_origin = self.geodetic_origin
lat, lon, alt = nf.measure.data[0:3]
roll, pitch, yaw = nf.measure.data[3:6]
b2c_roll, b2c_pitch, b2c_yaw = nf.measure.data[6:9]
# world frame: +z up, +x is east, +y is north
# body frame: +z down, +x is fw towards north, +y is right wing (east)
# camera frame: +z into the image plane, -y is up, +x is right
# wf2bf_q = tools.eul_to_q((np.pi, -np.pi / 2), 'xz')
# bf2cf = Pose(None, tools.eul_to_q((np.pi / 2, np.pi / 2), 'yz'))
if 1:
w2b_bf_q = tools.ypr_to_q(yaw, pitch, roll)
b2c_bf_q = tools.ypr_to_q(b2c_yaw, b2c_pitch, b2c_roll)
yaw, pitch, roll = tools.q_to_ypr(w2b_bf_q * b2c_bf_q * (self.ori_off_q or quaternion.one))
cf_w2c = tools.lla_ypr_to_loc_quat(geodetic_origin, [lat, lon, alt], [yaw, pitch, roll], b2c=self.bf2cf)
if len(old_kf) > 0:
cf_w2c.vel = (cf_w2c.loc - (-old_kf[-1].pose.prior).loc) / (nf.time - old_kf[-1].time).total_seconds()
else:
cf_w2c.vel = np.array([0, 0, 0])
nf.pose.prior = -cf_w2c
else:
wf_body_r = tools.to_cartesian(lat, lon, alt, *geodetic_origin)
bf_world_body_q = tools.ypr_to_q(yaw, pitch, roll)
if 0:
cf_world_q = bf_cam_q.conj() * bf_world_body_q.conj()
cf_body_r = tools.q_times_v(bf_cam_q.conj(), -bf_cam_r)
cf_body_world_r = tools.q_times_v(cf_world_q * self.wf2bf_q.conj(), -wf_body_r)
nf.pose.prior = Pose(cf_body_r + cf_body_world_r, cf_world_q * bf_cam_q)
else:
bf_world_q = bf_world_body_q.conj() * self.wf2bf_q.conj()
cf_world_q = bf_cam_q.conj() * bf_world_q
cf_body_r = tools.q_times_v(bf_cam_q.conj(), -bf_cam_r)
cf_body_world_r = tools.q_times_v(cf_world_q, -wf_body_r)
# bf_cam_q.conj() * bf_world_body_q.conj() * self.wf2bf_q.conj()
nf.pose.prior = Pose(cf_body_r + cf_body_world_r, bf_cam_q.conj() * bf_world_body_q.conj() * bf_cam_q)
# bf_cam_q.conj() * bf_world_body_q.conj() * bf_cam_q
# (NokiaSensor.w2b_q * NokiaSensor.b2c_q) * prior.quat.conj()
# self.wf2bf_q * bf_cam_q * (bf_cam_q.conj() * bf_world_body_q.conj() * bf_cam_q).conj()
# => self.wf2bf_q * bf_cam_q //*// bf_cam_q.conj() * bf_world_body_q * bf_cam_q //*//
# self.wf2bf_q * bf_cam_q //*// bf_cam_q.conj() * bf_world_body_q * bf_cam_q //*// bf_cam_q.conj() * bf_world_body_q.conj() * self.wf2bf_q.conj()
return nf
def init_odo(self):
sc = 0.5
params = {
'use_ba': True,
'new_keyframe_ba': False,
'threaded_ba':
False, # TODO: debug adjustment to new keyframes & 3d points (!!) after ba completed
'online_cam_calib': False,
'verify_feature_tracks': True,
'max_keypoints': 320, # 315
'min_keypoint_dist': round(50 * sc),
'min_tracking_quality': 0.0005, # def 0.0001, was 0.0003
'repr_refine_kp_uv': False,
'repr_refine_coef': 0.2,
'refine_kp_uv': False,
'max_kp_refine_dist': 5,
'repr_err_fov_ratio': 0.0005, # was 0.002
'max_repr_err_fov_ratio': 0.003, # was 0.003
'est_2d2d_prob': 0.9999,
'pose_2d2d_quality_lim': 0.04,
'use_3d2d_ransac': False,
'opt_init_ransac': False,
# 'est_3d2d_iter_count': 10000,
'new_kf_min_displ_fov_ratio': 0.016,
'ini_kf_triangulation_trigger': 40,
'max_keyframes': 50000,
'max_ba_keyframes': 60,
'ba_interval': 10,
'max_ba_fun_eval': 25 * 2,
'loc_err_sd': 2,
'ori_err_sd': math.radians(5.0),
}
if 0:
params.update({
'orb_feature_tracking': True,
'orb_search_dist': 0.03, # in fov ratio
'max_keypoints': 2000,
'min_keypoint_dist': round(20 * sc),
'repr_err_fov_ratio': 0.006, # was 0.004
'max_repr_err_fov_ratio': 0.008, # was 0.006
})
logging.basicConfig(level=logging.INFO)
odo = VisualGPSNav(self.cam,
round(self.cam.width * sc),
geodetic_origin=self.coord0,
wf_body_q=self.w2b_q,
bf_cam_pose=Pose(np.array([0, 0, 0]), self.b2c_q),
verbose=1,
pause=False,
**params)
return odo
def get_pose(self, coord0, mes0, mes1, weight, full_rot):
ypr0 = np.flip(mes0[3:6]) if full_rot else [mes0[5], 0, 0]
ypr1 = np.flip(mes1[3:6]) if full_rot else [mes1[5], 0, 0]
cf_w2c0 = tools.lla_ypr_to_loc_quat(coord0,
mes0[:3],
ypr0,
b2c=self.b2c)
cf_w2c1 = tools.lla_ypr_to_loc_quat(coord0,
mes1[:3],
ypr1,
b2c=self.b2c)
delta = cf_w2c1 - cf_w2c0
aa = tools.q_to_angleaxis(delta.quat)
aa[0] = tools.wrap_rads(aa[0]) * weight
cf_w2c = Pose(cf_w2c0.loc * (1 - weight) + cf_w2c1.loc * weight,
tools.angleaxis_to_q(aa) * cf_w2c0.quat)
cf_c2w = -cf_w2c
return cf_c2w
def main():
parser = argparse.ArgumentParser(
description='Estimate depthmaps using COLMAP')
parser.add_argument('-i',
'--kapture',
required=True,
help='input path to kapture data root directory')
parser.add_argument('-s',
'--sensor',
default='cam',
help='input kapture image sensor name')
parser.add_argument('-nl',
'--keypoint',
default='gftt',
help='input kapture keypoint type name')
parser.add_argument('-p', '--path', required=True, help='output base path')
parser.add_argument('-t',
'--txt',
default='txt',
help='output text folder name')
parser.add_argument('-d',
'--dense',
default='dense',
help='output dense folder name')
parser.add_argument('-e', '--export', help='export depth maps here')
parser.add_argument(
'--export-bits',
type=int,
default=32,
choices=(16, 24, 32),
help=
'Export depth and geometry using float16/24/32 exr-files (default 32). Safe range of '
'values can be calculated using (2**ceil(log(trg_prec * 2**x)/log(2)) - 1), where '
'trg_prec is the target precision and x is the length of the significand '
'(10, 15, 23 for 16, 24, 32 bit floats, respectively). '
'E.g., if ground resolution at 100m for fl=2315px is 0.043m (100/2315), the safe range '
'for different choices would be [-63, 63], [-2047, 2047], and [-524287, 524287].'
)
parser.add_argument('-c', '--cmd', help='path to colmap command')
parser.add_argument(
'--composite-cmd',
help=
'composite cmd to invoke colmap command, e.g. "singularity exec colmap.sif colmap"'
)
parser.add_argument(
'--gpu',
default='0',
help='gpu indices to use, e.g. 0 (default) or 0,0,0,0 or 0,1,2')
parser.add_argument('--mem',
default=32,
type=int,
help='max mem usage in GB ')
parser.add_argument('--min-depth',
type=float,
default=10,
help='min depth for depth map estimation')
parser.add_argument('--max-depth',
type=float,
default=200,
help='max depth for depth map estimation')
parser.add_argument(
'--win-rad',
type=int,
default=5,
help='window radius for colmap depth map estimation (default=5)')
parser.add_argument(
'--win-step',
type=int,
default=1,
help='window step size for colmap depth map estimation (default=1)')
parser.add_argument('--filter-min-ncc',
type=float,
default=0.1,
help='--PatchMatchStereo.filter_min_ncc arg (=0.1)')
parser.add_argument(
'--filter-min-triangulation-angle',
type=float,
default=3.0,
help='--PatchMatchStereo.filter_min_triangulation_angle arg (=3.0)')
parser.add_argument(
'--filter-min-num-consistent',
type=int,
default=2,
help='--PatchMatchStereo.filter_min_num_consistent arg (=2)')
parser.add_argument(
'--filter-geom-consistency-max-cost',
type=float,
default=1.0,
help='--PatchMatchStereo.filter_geom_consistency_max_cost arg (=1.0)')
parser.add_argument('--plot-only',
action='store_true',
help='plot only export results')
parser.add_argument('--skip-import',
action='store_true',
help='skip importing kapture to colmap format')
parser.add_argument('--skip-depth-est',
action='store_true',
help='skip depth map estimation')
parser.add_argument('--skip-export',
action='store_true',
help='skip exporting depth maps to exr format')
parser.add_argument('--skip-depth-filter',
action='store_true',
help='skip extra filter scheme for depth map')
parser.add_argument(
'--plot',
help='during export, only process and plot the frame given here')
args = parser.parse_args()
txt_rec = os.path.join(args.path, args.txt)
db_path = os.path.join(args.path, 'colmap.db')
img_path = get_record_fullpath(args.kapture)
dense_path = os.path.join(args.path, args.dense)
os.makedirs(os.path.join(dense_path, 'images', args.sensor), exist_ok=True)
logging.basicConfig(level=logging.INFO)
if not args.export:
args.export = os.path.join(args.kapture, 'reconstruction')
if args.plot_only:
plot_only(args.export, args.plot)
exit()
if args.composite_cmd:
cmd = args.composite_cmd.split(' ')
else:
assert args.cmd, 'either --cmd or --composite-cmd argument needs to be given'
cmd = [args.cmd]
if not args.skip_import:
export_colmap(args.kapture,
db_path,
txt_rec,
keypoints_type=args.keypoint,
force_overwrite_existing=True)
image_undistorter_args = [
"image_undistorter",
"--image_path",
img_path,
"--input_path",
txt_rec,
"--output_path",
dense_path,
"--blank_pixels",
"1",
]
exec_cmd(cmd + image_undistorter_args)
for f in ('consistency_graphs', 'depth_maps', 'normal_maps'):
os.makedirs(os.path.join(dense_path, 'stereo', f, args.sensor),
exist_ok=True)
if not args.skip_depth_est:
patch_match_stereo_args = [
"patch_match_stereo",
"--workspace_path",
dense_path,
"--PatchMatchStereo.depth_min",
str(args.min_depth),
"--PatchMatchStereo.depth_max",
str(args.max_depth),
"--PatchMatchStereo.window_radius",
str(args.win_rad),
"--PatchMatchStereo.window_step",
str(args.win_step),
"--PatchMatchStereo.gpu_index",
args.gpu,
"--PatchMatchStereo.cache_size",
str(args.mem),
"--PatchMatchStereo.filter_min_ncc",
str(args.filter_min_ncc),
"--PatchMatchStereo.filter_min_triangulation_angle",
str(args.filter_min_triangulation_angle),
"--PatchMatchStereo.filter_min_num_consistent",
str(args.filter_min_num_consistent),
"--PatchMatchStereo.filter_geom_consistency_max_cost",
str(args.filter_geom_consistency_max_cost),
]
exec_cmd(cmd + patch_match_stereo_args)
if not args.skip_export:
depth_path = os.path.join(dense_path, 'stereo', 'depth_maps',
args.sensor)
os.makedirs(os.path.join(args.export, 'depth'), exist_ok=True)
os.makedirs(os.path.join(args.export, 'geometry'), exist_ok=True)
kapt = kapture_from_dir(args.kapture)
sensor_id, width, height, fl_x, fl_y, pp_x, pp_y, *dist_coefs = get_cam_params(
kapt, args.sensor)
file2id = {fn[sensor_id]: id for id, fn in kapt.records_camera.items()}
if args.export_bits == 16:
exr_params_d = exr_params_xyz = (cv2.IMWRITE_EXR_TYPE,
cv2.IMWRITE_EXR_TYPE_HALF)
else:
exr_params_d = exr_params_xyz = (cv2.IMWRITE_EXR_TYPE,
cv2.IMWRITE_EXR_TYPE_FLOAT)
if args.export_bits == 24:
if hasattr(cv2, 'IMWRITE_EXR_COMPRESSION'):
# supported in OpenCV 4, see descriptions at
# https://rainboxlab.org/downloads/documents/EXR_Data_Compression.pdf
# or https://www.openexr.com/documentation/TechnicalIntroduction.pdf
exr_params_d += (cv2.IMWRITE_EXR_COMPRESSION,
cv2.IMWRITE_EXR_COMPRESSION_PXR24
) # zip 24bit floats
exr_params_xyz += (cv2.IMWRITE_EXR_COMPRESSION,
cv2.IMWRITE_EXR_COMPRESSION_PXR24)
else:
logging.warning(
'This version of OpenCV does not support PXR24 compression, defaulting to float32'
)
elif hasattr(cv2, 'IMWRITE_EXR_COMPRESSION'):
exr_params_d += (cv2.IMWRITE_EXR_COMPRESSION,
cv2.IMWRITE_EXR_COMPRESSION_ZIP
) # zip 32bit floats
exr_params_xyz += (cv2.IMWRITE_EXR_COMPRESSION,
cv2.IMWRITE_EXR_COMPRESSION_ZIP)
logging.info('Exporting geometric depth maps in EXR format...')
for fname in tqdm.tqdm(os.listdir(depth_path), mininterval=3):
m = re.search(r'(.*?)(\.jpg|\.png|\.jpeg)?\.geometric\.bin', fname)
if m and (not args.plot or m[1] == args.plot):
depth0 = read_colmap_array(os.path.join(depth_path, fname))
depth0[depth0 <= args.min_depth] = np.nan
depth0[depth0 >= args.max_depth] = np.nan
depth = depth0 if args.skip_depth_filter else filter_depth(
depth0, args)
if width != depth.shape[1] or height != depth.shape[0]:
logging.warning(
'Depth map for image %s is different size than the camera resolution %s vs %s'
% (m[1] + m[2], depth.shape, (height, width)))
outfile = os.path.join(args.export, 'depth', m[1] + '.d.exr')
cv2.imwrite(outfile, depth, exr_params_d)
frame_id = file2id.get(
args.sensor + '/' + m[1] + m[2],
file2id.get(args.sensor + '\\' + m[1] + m[2], None))
cf_cam_world_v, cf_cam_world_q = kapt.trajectories[frame_id][
sensor_id].t, kapt.trajectories[frame_id][sensor_id].r
cf_world_cam = -Pose(cf_cam_world_v, cf_cam_world_q)
px_u = cam_px_u(depth.shape[1], depth.shape[0], fl_x, fl_y,
pp_x, pp_y)
# the depth is actually the z-component, not the distance from the camera to the surface
dist = depth.flatten() / px_u[:, 2]
px_u = tools.q_times_mx(cf_world_cam.quat,
px_u * dist[:, None])
xyz = px_u.reshape(depth.shape +
(3, )) + cf_world_cam.loc.reshape((1, 1, 3))
outfile = os.path.join(args.export, 'geometry',
m[1] + '.xyz.exr')
cv2.imwrite(outfile, xyz.astype(np.float32), exr_params_xyz)
if args.plot:
_, depth2 = filter_depth(depth0, args, return_interm=True)
xyz = xyz.reshape((-1, 3))
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
plt.figure(1)
plt.imshow(depth0)
plt.figure(2)
plt.imshow(depth2)
plt.figure(3)
plt.imshow(depth)
f = plt.figure(4)
a = f.add_subplot(111, projection='3d')
a.set_xlabel('x')
a.set_ylabel('y')
a.set_zlabel('z')
a.plot(xyz[::5, 0], xyz[::5, 1], xyz[::5, 2], '.')
plt.show()
def main():
parser = argparse.ArgumentParser(
description=
'Estimate focal length and simple radial distortion based on '
'flat ground at the starting location')
parser.add_argument(
'--path',
nargs='+',
required=True,
help='path to folder with result.pickle and kapture-folder')
parser.add_argument('--takeoff',
nargs='+',
type=int,
required=True,
help='take-off trajectory in question?')
parser.add_argument('--first-frame',
'-f',
nargs='+',
type=int,
required=True,
help='first frame')
parser.add_argument('--last-frame',
'-l',
nargs='+',
type=int,
required=True,
help='last frame')
parser.add_argument('--img-sc',
default=0.5,
type=float,
help='image scale')
parser.add_argument('--nadir-looking',
action='store_true',
help='is cam looking down? used for plots only')
parser.add_argument('--ini-fl',
type=float,
help='initial value for focal length')
parser.add_argument('--ini-cx',
type=float,
help='initial value for x principal point')
parser.add_argument('--ini-cy',
type=float,
help='initial value for y principal point')
parser.add_argument('--ini-dist',
nargs='+',
default=[],
type=float,
help='initial values for radial distortion coefs')
parser.add_argument('--fix-fl',
action='store_true',
help='do not optimize focal length')
parser.add_argument('--fix-dist',
action='store_true',
help='do not optimize distortion coefs')
parser.add_argument(
'--pre-ba',
action='store_true',
help='run one ba pass to update params before optimization')
parser.add_argument('--pre-ba-dist-opt',
action='store_true',
help='optimize r1 and r2 params during pre ba')
parser.add_argument('--plot-init',
action='store_true',
help='plot initial result, after pre ba')
parser.add_argument('--opt-loc',
action='store_true',
help='use gps measure error')
parser.add_argument(
'--opt-disp',
action='store_true',
help='use dispersion around estimated surface as a measure')
parser.add_argument('--opt-pitch',
action='store_true',
help='expect pitch to not change')
parser.add_argument(
'--opt-ground',
type=float,
help='expect the ground to be at this altitude [m] in the final frame')
args = parser.parse_args()
cf_args = []
for i, path in enumerate(args.path):
with open(os.path.join(path, 'result.pickle'), 'rb') as fh:
results, map3d, frame_names, meta_names, gt, ba_errs = pickle.load(
fh)
kapt = kapture_from_dir(os.path.join(path, 'kapture'))
sensor_id, width, height, fl_x, fl_y, pp_x, pp_y, *dist_coefs = get_cam_params(
kapt, SENSOR_NAME)
if args.ini_fl:
fl_x = fl_y = args.ini_fl * args.img_sc
if args.ini_cx:
pp_x = args.ini_cx * args.img_sc
if args.ini_cy:
pp_y = args.ini_cy * args.img_sc
dist_n = np.where(np.array(dist_coefs) != 0)[0][-1] + 1
dist_coefs = dist_coefs[:dist_n]
x0 = list(
(args.ini_fl * args.img_sc if args.ini_fl else (fl_x + fl_y) / 2,
*(args.ini_dist if len(args.ini_dist) else dist_coefs)))
print('loading poses and keypoints...')
poses, pts3d, pts2d, cam_idxs, pt3d_idxs, meas_r, meas_aa, meas_idxs, obs_kp = \
get_ba_params(path, args.first_frame[i], args.last_frame[i], results, kapt, sensor_id)
if args.pre_ba:
if not args.pre_ba_dist_opt:
print('running initial global bundle adjustment...')
poses, pts3d, _, _, _ = run_ba(width,
height,
pp_x,
pp_y,
x0[0],
x0[1:],
poses,
pts3d,
pts2d,
cam_idxs,
pt3d_idxs,
meas_r,
meas_aa,
meas_idxs,
optimize_dist=True)
if args.pre_ba_dist_opt:
print(
'running initial global bundle adjustment with cam params...'
)
poses, pts3d, dist, cam_intr, _ = run_ba(
width,
height,
pp_x,
pp_y,
x0[0],
x0[1:],
poses,
pts3d,
pts2d,
cam_idxs,
pt3d_idxs,
meas_r,
meas_aa,
meas_idxs,
optimize_dist=PRE_BA_DIST_ENABLED,
n_cam_intr=N_CAM_INTR)
if PRE_BA_DIST_ENABLED:
x0[1:3] = dist[:2]
if cam_intr is not None:
if len(cam_intr) != 2:
x0[0] = cam_intr[0]
if len(cam_intr) > 1:
pp_x, pp_y = cam_intr[-2:]
lbl = ['fl', 'cx, cy', 'fl, cx, cy'][len(cam_intr) - 1]
print('new k1, k2: %s, new %s: %s' %
(dist, lbl, list(np.array(cam_intr) / args.img_sc)))
else:
print('new k1, k2: %s' % (dist, ))
results = results[:len(poses)]
for j in range(len(results)):
results[j][0].post = Pose(poses[j, 3:],
tools.angleaxis_to_q(poses[j, :3]))
map3d = [Keypoint(pt3d=pts3d[j, :]) for j in range(len(pts3d))]
with open(os.path.join(path, 'global-ba-result.pickle'),
'wb') as fh:
pickle.dump(
(results, map3d, frame_names, meta_names, gt, ba_errs), fh)
if args.plot_init:
plot_results(results,
map3d,
frame_names,
meta_names,
nadir_looking=args.nadir_looking)
error_types = {ERROR_TYPE_REPROJECTION}
if args.opt_loc:
error_types.add(ERROR_TYPE_LOCATION)
if args.takeoff[i]:
error_types.update({ERROR_TYPE_ALTITUDE, ERROR_TYPE_CURVATURE})
if args.opt_disp:
error_types.add(ERROR_TYPE_DISPERSION)
elif args.opt_pitch:
error_types.add(ERROR_TYPE_PITCH)
ground_alt = TAKEOFF_LAWN_ALT
if args.opt_ground is not None:
error_types.add(ERROR_TYPE_ALTITUDE)
ground_alt = args.opt_ground
cf_args.append(
(width, height, pp_x, pp_y, poses, pts3d, pts2d, cam_idxs,
pt3d_idxs, meas_r, meas_aa, meas_idxs, obs_kp, ground_alt,
error_types, x0[0] if args.fix_fl else None,
x0[1:] if args.fix_dist else None))
print('optimizing focal length and radial distortion coefs...')
lo_bounds = (0.7 * x0[0], )
hi_bounds = (1.4 * x0[0], )
diff_step = (0.0003, )
if len(x0) - 1 == 1:
lo_bounds += (-0.12, )
hi_bounds += (0.12, )
diff_step += (0.001, )
elif len(x0) - 1 == 2:
lo_bounds += (-0.2, -0.6)
hi_bounds += (0.2, 0.6)
diff_step += (0.001, 0.01)
elif len(x0) - 1 == 4:
lo_bounds += (-0.5, -0.5, -0.01, -0.01)
hi_bounds += (0.5, 0.5, 0.01, 0.01)
diff_step += (0.001, 0.01, 0.01, 0.01)
else:
assert False, 'not supported'
if 0:
# optimize focal length, simple radial distortion using gaussian process hyperparameter search
# TODO
pass
elif not args.fix_fl and not args.fix_dist:
res = least_squares(costfun,
tuple(x0),
verbose=2,
ftol=1e-5,
xtol=1e-5,
gtol=1e-8,
max_nfev=1000,
bounds=(lo_bounds, hi_bounds),
diff_step=diff_step,
args=(cf_args, ))
elif args.fix_fl:
res = least_squares(costfun,
tuple(x0[1:]),
verbose=2,
ftol=1e-5,
xtol=1e-5,
gtol=1e-8,
max_nfev=1000,
bounds=(lo_bounds[1:], hi_bounds[1:]),
diff_step=diff_step[1:],
args=(cf_args, ))
elif args.fix_dist:
res = least_squares(costfun,
tuple(x0[0:1]),
verbose=2,
ftol=1e-5,
xtol=1e-5,
gtol=1e-8,
max_nfev=1000,
bounds=(lo_bounds[0], hi_bounds[0]),
diff_step=diff_step[0],
args=(cf_args, ))
costfun(res.x, cf_args, plot=True)
def traj_costfun(x,
width,
height,
pp_x,
pp_y,
poses,
pts3d,
pts2d,
cam_idxs,
pt3d_idxs,
meas_r,
meas_aa,
meas_idxs,
obs_kp,
ground_alt,
error_types,
def_fl=None,
def_dist=None,
plot=False):
if def_fl is None and def_dist is None:
fl, dist_coefs = abs(x[0]), x[1:]
elif def_dist is None:
fl, dist_coefs = def_fl, x
else:
fl, dist_coefs = abs(x[0]), def_dist
# run ba
new_poses, new_pts3d, _, _, ba_errs = run_ba(width, height, pp_x, pp_y, fl,
dist_coefs, poses, pts3d,
pts2d, cam_idxs, pt3d_idxs,
meas_r, meas_aa, meas_idxs)
errs = []
labels = []
if ERROR_TYPE_REPROJECTION in error_types:
err_repr = ERROR_COEFS[ERROR_TYPE_REPROJECTION] * np.nanmean(
ba_errs[:, 0])
errs.append(err_repr)
labels.append('repr')
if ERROR_TYPE_LOCATION in error_types:
err_repr = ERROR_COEFS[ERROR_TYPE_LOCATION] * np.nanmean(
np.linalg.norm(ba_errs[:, 1:4], axis=1))
errs.append(err_repr)
labels.append('loc')
if {ERROR_TYPE_ALTITUDE, ERROR_TYPE_CURVATURE,
ERROR_TYPE_DISPERSION}.intersection(error_types):
new_pts3d = new_pts3d[obs_kp, :]
# fit a plane, based on https://math.stackexchange.com/questions/99299/best-fitting-plane-given-a-set-of-points
centroid = np.median(new_pts3d.T, axis=1, keepdims=True)
svd = np.linalg.svd(new_pts3d.T - centroid)
normal = svd[0][:, -1:].T
# fit a parabolic surface to keypoints
x0 = 0, *centroid.flatten(), *normal.flatten()
res = least_squares(paraboloid_costfun,
x0,
verbose=0,
ftol=1e-5,
xtol=1e-5,
gtol=1e-8,
max_nfev=1000,
loss='huber',
f_scale=1.0,
args=(new_pts3d, ))
# extract a plane that corresponds to the fitted parabolic surface (should be more accurate than the median based)
c, x0, y0, z0, xn, yn, zn = res.x
if ERROR_TYPE_CURVATURE in error_types:
errs.append(ERROR_COEFS[ERROR_TYPE_CURVATURE] * c)
labels.append('curv')
if ERROR_TYPE_DISPERSION in error_types:
displ = np.abs(res.fun)
lim = np.quantile(
displ,
0.8) # how well the keypoints lie on the estimated surface
err_disp = np.mean(displ[displ < lim])
errs.append(ERROR_COEFS[ERROR_TYPE_DISPERSION] * err_disp)
labels.append('disp')
if ERROR_TYPE_ALTITUDE in error_types:
centroid = np.array([x0, y0, z0]).reshape((1, 3))
normal = np.array([xn, yn, zn]).reshape((1, 3))
normal /= np.linalg.norm(normal)
pose_cf = new_poses[meas_idxs[-1], :]
loc_wf = (
-Pose(pose_cf[3:], tools.angleaxis_to_q(pose_cf[:3]))).loc
is_takeoff = ERROR_TYPE_CURVATURE in error_types
end_meas_alt = -meas_r[-1 if is_takeoff else 0,
2] - ground_alt # neg z-axis is altitude
if 1:
end_distance = np.abs(
normal.dot(loc_wf[:, None] - centroid.flatten()[:, None]))
err_alt = (end_meas_alt - end_distance)[0, 0]
else:
err_alt = np.quantile(-new_pts3d[:, 2], 0.2) - ground_alt
errs.append(ERROR_COEFS[ERROR_TYPE_ALTITUDE] * err_alt /
end_meas_alt)
labels.append('alt')
if ERROR_TYPE_PITCH in error_types:
ypr = np.array(
[tools.q_to_ypr(tools.angleaxis_to_q(p[:3]))
for p in new_poses]) / np.pi * 180
err_pitch = ypr[-1, 1] - ypr[0, 1]
errs.append(ERROR_COEFS[ERROR_TYPE_PITCH] * err_pitch)
labels.append('pitch')
print('=== fl: %s, dist: %s, cost: %s ===' %
(fl / 0.5, dist_coefs, dict(zip(labels, errs))))
if plot:
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.plot(new_pts3d[:, 0], new_pts3d[:, 1], new_pts3d[:, 2], 'C0.')
plt.show()
return np.array(errs)
class ToyNokiaSensor(Mission):
# world frame: +z up, +x is east, +y is north
# body frame: +z down, +x is fw towards north, +y is right wing (east)
# camera frame: +z into the image plane, -y is up (north), +x is right (east)
w2b = Pose(None, tools.eul_to_q((np.pi, -np.pi / 2), 'xz'))
b2c = Pose(None, tools.eul_to_q((np.pi / 2, ), 'z'))
def __init__(self, *args, **kwargs):
super(ToyNokiaSensor, self).__init__(*args, **kwargs)
self.real_cam = copy.deepcopy(self.init_cam())
if 0:
self.real_cam.dist_coefs = [
-0.11250615, 0.14296794, 0, 0, -0.11678778
]
elif 1:
self.real_cam.dist_coefs = [
-0.11250615, 0.14296794, -0.00175085, 0.00057391, -0.11678778
]
else:
self.real_cam.dist_coefs = [0, 0, 0, 0, 0]
def init_data(self):
from TelemetryParsing import readTelemetryCsv
t_time, *t_data = readTelemetryCsv(self.data_path, None, None)
coord0 = t_data[0][0], t_data[1][0], t_data[2][0]
time0 = t_time[0].astype(np.float64) / 1e6
t_time = (t_time - t_time[0]).astype(np.float64) / 1e6
t_data = np.array(list(zip(*t_data)), dtype=np.float32)
if 1:
# filter so that images would be less jumpy for better feature tracking (problem with yaw [-pi, pi] though)
t_data[:, :3] = cv2.filter2D(t_data[:, :3], cv2.CV_32F,
np.ones((3, 1)) / 3)
meas_interval = 10
frame_rate = 5
def data_gen():
first = True
f_id = 0
prev_t_t = None
prev_meas = None
poses = []
for t_id, (t_t, meas_data) in enumerate(zip(t_time, t_data)):
f_t = prev_t_t
if t_id == 0 or self.first_frame is not None and t_t < self.first_frame / frame_rate + self.video_toff:
f_id += meas_interval
prev_t_t = t_t
prev_meas = meas_data
continue
elif self.last_frame is not None and t_t > self.last_frame / frame_rate + self.video_toff:
break
# read a measurement
full_rot = False
dt = t_t - prev_t_t
lat, lon, alt, roll, pitch, yaw, gimbal_roll, gimbal_pitch, gimbal_yaw = prev_meas
if not full_rot:
roll, pitch = 0, 0
meas = Measure(data=np.array([lat, lon, alt, roll, pitch,
yaw]),
time_off=prev_t_t - f_t)
for i in range(meas_interval):
pose = self.get_pose(coord0,
prev_meas,
meas_data, (f_t - prev_t_t) / dt,
full_rot=full_rot)
poses.append(pose)
img = self.generate_image(pose)
f_ts = datetime.fromtimestamp(f_t + time0).strftime(
'%H:%M:%S.%f')
t_ts = datetime.fromtimestamp(prev_t_t + time0).strftime(
'%H:%M:%S.%f')
yield img, f_t, 'frame-%d (%s)' % (f_id, f_ts), meas, (
'meas-%d (%s)' % (t_id, t_ts)) if meas else None, None
meas = None
first = False
f_t += dt / meas_interval
f_id += 1
prev_t_t = t_t
prev_meas = meas_data
# import matplotlib.pyplot as plt
# plt.plot(np.array([p.loc for p in poses]))
# plt.show()
return data_gen(), time0, coord0
def get_pose(self, coord0, mes0, mes1, weight, full_rot):
ypr0 = np.flip(mes0[3:6]) if full_rot else [mes0[5], 0, 0]
ypr1 = np.flip(mes1[3:6]) if full_rot else [mes1[5], 0, 0]
cf_w2c0 = tools.lla_ypr_to_loc_quat(coord0,
mes0[:3],
ypr0,
b2c=self.b2c)
cf_w2c1 = tools.lla_ypr_to_loc_quat(coord0,
mes1[:3],
ypr1,
b2c=self.b2c)
delta = cf_w2c1 - cf_w2c0
aa = tools.q_to_angleaxis(delta.quat)
aa[0] = tools.wrap_rads(aa[0]) * weight
cf_w2c = Pose(cf_w2c0.loc * (1 - weight) + cf_w2c1.loc * weight,
tools.angleaxis_to_q(aa) * cf_w2c0.quat)
cf_c2w = -cf_w2c
return cf_c2w
_pts4d = None
def generate_image(self, pose, grid=5):
if self._pts4d is None:
x, y, z = pose.loc
if 0:
xx, yy = np.meshgrid(
np.linspace(x - z * 3, x + z * 3,
math.ceil(z * 3 / grid) * 2 + 1),
np.linspace(y - z * 3, y + z * 3,
math.ceil(z * 3 / grid) * 2 + 1))
else:
xx = np.random.uniform(x - z * 3, x + z * 3,
2 * (math.ceil(z * 3 / grid) * 2 + 1, ))
yy = np.random.uniform(x - z * 3, x + z * 3,
2 * (math.ceil(z * 3 / grid) * 2 + 1, ))
if 0:
zz = np.zeros_like(xx)
else:
zz = np.random.uniform(0, -40, xx.shape)
pts3d = np.stack((xx.flatten(), yy.flatten(), zz.flatten()),
axis=1).reshape((-1, 3))
self._pts4d = np.hstack((pts3d, np.ones((len(pts3d), 1))))
T = np.hstack(
(quaternion.as_rotation_matrix(pose.quat), pose.loc.reshape(
(-1, 1))))
proj_pts2d = self.real_cam.cam_mx.dot(T).dot(self._pts4d.T).T
uvp = proj_pts2d[:, :2] / proj_pts2d[:, 2:]
I = np.logical_and.reduce((
uvp[:, 0] >= 0,
uvp[:, 1] >= 0,
uvp[:, 0] < self.real_cam.width,
uvp[:, 1] < self.real_cam.height,
))
uvp = self.real_cam.distort(uvp[I, :])
uvp = (uvp + 0.5).astype(int)
img = np.ones(
(self.real_cam.height, self.real_cam.width), dtype=np.uint8) * 64
for i in range(5):
for j in range(5):
img[np.clip(uvp[:, 1] + i - 2, 0, self.real_cam.height - 1),
np.clip(uvp[:, 0] + j - 2, 0, self.real_cam.width -
1)] = 224
return img
def init_cam(self):
w, h, p = 1920, 1080, 0.00375
common_kwargs_worst = {
'sensor_size': (w * p, h * p),
'quantum_eff': 0.30,
'px_saturation_e': 2200, # snr_max = 20*log10(sqrt(sat_e)) dB
'lambda_min': 350e-9,
'lambda_eff': 580e-9,
'lambda_max': 800e-9,
'dark_noise_mu': 40,
'dark_noise_sd': 6.32,
'readout_noise_sd': 15,
# dark_noise_sd should be sqrt(dark_noise_mu)
'emp_coef': 1, # dynamic range = 20*log10(sat_e/readout_noise))
'exclusion_angle_x': 55,
'exclusion_angle_y': 90,
}
common_kwargs_best = dict(common_kwargs_worst)
common_kwargs_best.update({
'quantum_eff': 0.4,
'px_saturation_e': 3500,
'dark_noise_mu': 25,
'dark_noise_sd': 5,
'readout_noise_sd': 5,
})
common_kwargs = common_kwargs_best
cam = Camera(
w, # width in pixels
h, # height in pixels
62.554, # x fov in degrees (could be 6 & 5.695, 5.15 & 4.89, 7.7 & 7.309)
37.726, # y fov in degrees
f_stop=5, # TODO: put better value here
point_spread_fn=0.50, # ratio of brightness in center pixel
scattering_coef=
2e-10, # affects strength of haze/veil when sun shines on the lens
dist_coefs=None if 0 else
[-0.11250615, 0.14296794, -0.00175085, 0.00057391, -0.11678778],
cam_mx=np.array([[1.58174667e+03, 0.00000000e+00, 9.97176182e+02],
[0.00000000e+00, 1.58154569e+03, 5.15553843e+02],
[0.00000000e+00, 0.00000000e+00,
1.00000000e+00]]),
**common_kwargs)
return cam
def init_odo(self):
sc = 0.5
params = {
'use_ba': True,
'new_keyframe_ba': False,
'threaded_ba':
False, # TODO: debug adjustment to new keyframes & 3d points (!!) after ba completed
'online_cam_calib': False,
'verify_feature_tracks': True,
'max_keypoints': 320, # 315
'min_keypoint_dist': round(50 * sc),
'min_tracking_quality': 0.0005, # def 0.0001, was 0.0003
'repr_refine_kp_uv': False,
'repr_refine_coef': 0.2,
'refine_kp_uv': False,
'max_kp_refine_dist': 5,
'repr_err_fov_ratio': 0.0005, # was 0.002
'max_repr_err_fov_ratio': 0.003, # was 0.003
'est_2d2d_prob': 0.9999,
'pose_2d2d_quality_lim': 0.04,
'use_3d2d_ransac': False,
'opt_init_ransac': False,
# 'est_3d2d_iter_count': 10000,
'new_kf_min_displ_fov_ratio': 0.016,
'ini_kf_triangulation_trigger': 40,
'max_keyframes': 50000,
'max_ba_keyframes': 60,
'ba_interval': 10,
'max_ba_fun_eval': 25 * 2,
'loc_err_sd': 2,
'ori_err_sd': math.radians(5.0),
}
if 0:
params.update({
'orb_feature_tracking': True,
'orb_search_dist': 0.03, # in fov ratio
'max_keypoints': 2000,
'min_keypoint_dist': round(20 * sc),
'repr_err_fov_ratio': 0.006, # was 0.004
'max_repr_err_fov_ratio': 0.008, # was 0.006
})
logging.basicConfig(level=logging.INFO)
odo = VisualGPSNav(self.cam,
round(self.cam.width * sc),
geodetic_origin=self.coord0,
wf_body_q=self.w2b_q,
bf_cam_pose=Pose(np.array([0, 0, 0]), self.b2c_q),
verbose=1,
pause=False,
**params)
return odo
def main():
# parse arguments
parser = argparse.ArgumentParser(
description='Run visual odometry on a set of images')
parser.add_argument('--data', '-d', metavar='DATA', help='path to data')
parser.add_argument('--meta',
'-t',
metavar='META',
help='path to meta data')
parser.add_argument('--video-toff',
'--dt',
type=float,
metavar='dT',
help='video time offset compared to metadata')
parser.add_argument('--alt-scale',
'--as',
type=float,
default=1.0,
metavar='sc',
help='scale telemetry altitude with this value')
parser.add_argument('--res',
'-r',
metavar='OUT',
help='path to the result pickle')
parser.add_argument('--debug-out',
'-o',
metavar='OUT',
help='path to the debug output folder')
parser.add_argument('--kapture',
metavar='OUT',
help='path to kapture-format export folder')
parser.add_argument(
'--verbosity',
'-v',
type=int,
default=2,
help=
'verbosity level (0-4, 0-1: text only, 2:+debug imgs, 3: +keypoints, 4: +poses)'
)
parser.add_argument(
'--high-quality',
action='store_true',
help='high quality settings with more keypoints detected')
parser.add_argument('--mission',
'-mr',
choices=('hwproto', 'nokia', 'toynokia'),
help='select mission')
parser.add_argument('--undist-img',
action='store_true',
help='undistort image instead of keypoints')
parser.add_argument('--use-gimbal',
action='store_true',
help='gimbal data is ok, use it')
parser.add_argument(
'--drifting-gimbal',
action='store_true',
help=
'gimbal orientation measure drifts, update orientation offset with each odometry result'
)
parser.add_argument('--nadir-looking',
action='store_true',
help='downwards looking cam')
parser.add_argument(
'--ori-off-ypr',
type=float,
nargs=3,
help=
'orientation offset in ypr (deg) to be applied in body frame to orientation measurements'
)
parser.add_argument('--cam-dist',
type=float,
nargs='*',
help='cam distortion coeffs')
parser.add_argument('--cam-fl-x', type=float, help='cam focal length x')
parser.add_argument('--cam-fl-y', type=float, help='cam focal length y')
parser.add_argument('--cam-pp-x', type=float, help='cam principal point x')
parser.add_argument('--cam-pp-y', type=float, help='cam principal point y')
parser.add_argument('--first-frame',
'-f',
type=int,
default=0,
help='first frame (default: 0; -1: hardcoded value)')
parser.add_argument('--last-frame',
'-l',
type=int,
help='last frame (default: None; -1: hardcoded end)')
parser.add_argument('--skip',
'-s',
type=int,
default=1,
help='use only every xth frame (default: 1)')
parser.add_argument('--plot-only',
action='store_true',
help='only plot the result')
args = parser.parse_args()
if args.verbosity > 1:
import matplotlib.pyplot as plt
from matplotlib import cm
from mpl_toolkits.mplot3d import Axes3D
# init odometry and data
if args.mission == 'hwproto':
mission = HardwarePrototype(args.data, last_frame=(155, 321, None)[0])
elif args.mission == 'nokia':
ff = (100, 250, 520, 750, 1250, 1500,
1750)[0] if args.first_frame == -1 else args.first_frame
lf = (240, 400, 640, 1000, 1500, 1750,
2000)[1] if args.last_frame == -1 else args.last_frame
cam_mx = None
if args.cam_fl_x:
fl_y = args.cam_fl_y or args.cam_fl_x
pp_x = args.cam_pp_x or NokiaSensor.CAM_WIDTH / 2
pp_y = args.cam_pp_y or NokiaSensor.CAM_HEIGHT / 2
cam_mx = [[args.cam_fl_x, 0., pp_x], [0., fl_y, pp_y],
[0., 0., 1.]]
mission = NokiaSensor(
args.data,
data_path=args.meta,
video_toff=args.video_toff,
use_gimbal=args.use_gimbal,
undist_img=args.undist_img,
cam_mx=cam_mx,
cam_dist=args.cam_dist,
verbosity=args.verbosity,
high_quality=args.high_quality,
alt_scale=args.alt_scale,
ori_off_q=tools.ypr_to_q(*map(math.radians, args.ori_off_ypr))
if args.ori_off_ypr else None,
first_frame=ff,
last_frame=lf)
elif args.mission == 'toynokia':
mission = ToyNokiaSensor(args.data,
data_path=args.meta,
video_toff=args.video_toff,
first_frame=(100, 350, 850, 1650)[1],
last_frame=(415, 500, 1250, 1850, 2000)[2])
else:
assert False, 'bad mission given: %s' % args.mission
if args.debug_out:
mission.odo._track_save_path = args.debug_out
if args.nadir_looking:
mission.odo._nadir_looking = True
res_file = args.res or ('%s-result.pickle' % args.mission)
if args.plot_only:
plot_results(file=res_file, nadir_looking=args.nadir_looking)
replay_keyframes(mission.odo.cam, file=res_file)
return
# run odometry
prior = Pose(np.array([0, 0, 0]), quaternion.one)
frame_names0 = []
meta_names0 = []
results0 = []
ground_truth0 = []
ori_offs = []
ba_errs = []
kfid2img = {}
started = datetime.now()
ax = None
def ba_err_logger(frame_id, per_frame_ba_errs):
per_frame_ba_errs = np.stack(
(per_frame_ba_errs[:, 0],
np.linalg.norm(per_frame_ba_errs[:, 1:4], axis=1),
np.linalg.norm(per_frame_ba_errs[:, 4:7], axis=1) / np.pi * 180),
axis=1)
ba_errs.append([frame_id, *np.nanmean(per_frame_ba_errs, axis=0)])
mission.odo.ba_err_logger = ba_err_logger
vid_id = re.search(r'(^|\\|/)([\w-]+)\.mp4$', mission.video_path)
for i, (img, t, name, meta, meta_name, gt) in enumerate(mission.data):
if i % args.skip != 0:
continue
logger.info('')
logger.info(name)
frame_names0.append(name)
meta_names0.append(meta_name)
ground_truth0.append(gt)
if vid_id[2] == 'HD_CAM-1__514659341_03_12_2020_16_12_44' and t > 1050:
# hardcoding just to get dataset 10 to work
mission.odo.ori_off_q = tools.ypr_to_q(math.radians(0), 0, 0)
try:
nf, *_ = mission.odo.process(img,
datetime.fromtimestamp(mission.time0 +
t),
measure=meta)
if nf is not None and nf.id is not None:
kfid2img[nf.id] = i
if args.drifting_gimbal and nf.pose.post and meta:
est_w2c_bf_q = (-nf.pose.post).to_global(mission.b2c).quat
meas_w2c_bf_q = (-nf.pose.prior).to_global(
mission.b2c).quat * mission.odo.ori_off_q.conj()
new_ori_off_q = meas_w2c_bf_q.conj() * est_w2c_bf_q
filtered_ori_off_q = tools.mean_q(
[mission.odo.ori_off_q, new_ori_off_q], [0.8, 0.2])
if 0:
# TODO: debug, something wrong here as roll and pitch explode
mission.odo.ori_off_q = filtered_ori_off_q
else:
y, p, r = tools.q_to_ypr(filtered_ori_off_q)
mission.odo.ori_off_q = tools.ypr_to_q(y, 0, 0)
ori_offs.append(mission.odo.ori_off_q)
logger.info('new gimbal offset: %s' % (list(
map(lambda x: round(math.degrees(x), 2),
tools.q_to_ypr(mission.odo.ori_off_q))), ))
pts3d = np.array([
pt.pt3d for pt in mission.odo.state.map3d.values()
if pt.active
])
if len(pts3d) > 0 and nf.pose.post:
ground_alt = np.quantile(
-pts3d[:, 1], 0.2) # neg-y is altitude in cam frame
drone_alt = -(-nf.pose.post).loc[1]
expected_dist = drone_alt - mission.coord0[2]
modeled_dist = drone_alt - ground_alt
logger.info(
'ground at %.1f mr (%.1f mr), drone alt %.1f mr (%.1f mr)'
% (ground_alt, mission.coord0[2], modeled_dist,
expected_dist))
if args.verbosity > 3:
keyframes = mission.odo.all_keyframes()
post = np.zeros((len(keyframes), 7))
k, prior = 0, np.zeros((len(keyframes), 7))
for j, kf in enumerate(
[kf for kf in keyframes if kf.pose.post]):
post[j, :] = (-kf.pose.post).to_global(
NokiaSensor.b2c).to_global(
NokiaSensor.w2b).to_array()
if kf.measure is not None:
prior[k, :] = (-kf.pose.prior).to_global(
NokiaSensor.b2c).to_global(
NokiaSensor.w2b).to_array()
k += 1
if ax is not None:
ax.clear()
ax = tools.plot_poses(post,
axis=(0, 1, 0),
up=(0, 0, 1),
ax=ax,
wait=False)
tools.plot_poses(prior[:k, :],
axis=(0, 1, 0),
up=(0, 0, 1),
ax=ax,
wait=False,
colors=map(lambda c: (c, 0, 0, 0.5),
np.linspace(.3, 1.0, k)))
plt.pause(0.05)
except TypeError as e:
if 0:
nf, *_ = mission.odo.process(
img, datetime.fromtimestamp(mission.time0 + t), prior,
quaternion.one)
if nf and nf.pose.post:
prior = nf.pose.post
else:
raise e
results0.append(None if nf is None or nf.pose is None
or nf.pose.post is None else (
nf.pose, getattr(nf, 'measure', None), nf.time,
nf.id))
try:
mission.odo.flush_state() # flush all keyframes and keypoints
map3d = mission.odo.removed_keypoints
# results = [(kf.pose, kf.measure, kf.time, kf.id) for kf in mission.odo.removed_keyframes if kf.pose.post]
keyframes = [
dict(pose=kf.pose,
meas=kf.measure,
time=kf.time,
id=kf.id,
kps_uv=kf.kps_uv) for kf in mission.odo.removed_keyframes
if kf.pose.post
]
frame_names = [frame_names0[kfid2img[kf['id']]] for kf in keyframes]
meta_names = [meta_names0[kfid2img[kf['id']]] for kf in keyframes]
ground_truth = [ground_truth0[kfid2img[kf['id']]] for kf in keyframes]
ba_errs = np.array(ba_errs)
if 1:
pts3d = np.array([pt.pt3d for pt in map3d])
ground_alt = np.quantile(-pts3d[:, 1],
0.5) # neg-y is altitude in cam frame
drone_alt = -(-keyframes[-1]['pose'].post).loc[1] # word
expected_dist = drone_alt - mission.coord0[2]
modeled_dist = drone_alt - ground_alt
logger.info(
'ground at %.1f m (%.1f m), drone alt %.1f m (%.1f m)' %
(ground_alt, mission.coord0[2], modeled_dist, expected_dist))
if args.kapture:
kapture = KaptureIO(args.kapture,
reset=True,
jpg_qlt=95,
scale=0.5)
kapture.set_camera(1, 'cam', mission.cam)
kapture.add_frames(mission.odo.removed_keyframes, map3d)
kapture.write_to_dir()
except AttributeError as e:
if 0:
map3d = None
results = [(kf.pose, None, kf.time, kf.id)
for kf in mission.odo.all_keyframes()]
else:
raise e
mission.odo.quit()
if 0:
# show results as given by the online version of the algorithm
results, frame_names, meta_names, ground_truth = results0, frame_names0, meta_names0, ground_truth0
logger.info('time spent: %.0fs' %
(datetime.now() - started).total_seconds())
repr_errs = np.concatenate([
list(kf.repr_err.values()) for kf in mission.odo.removed_keyframes
if len(kf.repr_err)
])
err_q95 = np.quantile(np.linalg.norm(repr_errs, axis=1),
0.95) if len(repr_errs) else np.nan
logger.info('95%% percentile repr err: %.3fpx' % (err_q95, ))
interp = interp_loc(mission.odo.removed_keyframes, mission.time0)
loc_est = np.array([
np.ones((3, )) * np.nan if kf.pose.post is None else
(-kf.pose.post).loc for kf in mission.odo.removed_keyframes
])
loc_gps = np.array([
interp(kf.time.timestamp() - mission.time0).flatten()
for kf in mission.odo.removed_keyframes
]).squeeze()
mean_loc_err = np.nanmean(np.linalg.norm(loc_est - loc_gps, axis=1))
logger.info('mean loc err: %.3fm' % (mean_loc_err, ))
logger.info('mean ba errs (repr, loc, ori): %.3fpx %.3fm %.3fdeg' %
(*np.nanmean(ba_errs[:, 1:], axis=0), ))
logger.info('latest ba errs (repr, loc, ori): %.3fpx %.3fm %.3fdeg' %
(*ba_errs[-1, 1:], ))
if 0:
plt.figure(10)
plt.plot(loc_est[:, 0], loc_est[:, 1])
plt.plot(loc_gps[:, 0], loc_gps[:, 1])
plt.show()
if args.verbosity > 3:
plt.show() # stop to show last trajectory plot
res_folder = os.path.dirname(res_file)
if res_folder:
os.makedirs(res_folder, exist_ok=True)
with open(res_file, 'wb') as fh:
pickle.dump(
(keyframes, map3d, frame_names, meta_names, ground_truth, ba_errs),
fh)
if args.verbosity > 1:
plot_results(keyframes,
map3d,
frame_names,
meta_names,
ground_truth,
ba_errs,
res_file,
nadir_looking=args.nadir_looking)