def load_dem(self, dem_file):
# install using `conda install -c conda-forge rastrio`
wgs84 = CRS.from_epsg(4326)
with rs.open(dem_file) as ds:
top, left, bottom, right = self.bounds
top, bottom = int(ds.height * top), int(ds.height * bottom)
left, right = int(ds.width * left), int(ds.width * right)
xx, yy = np.meshgrid(np.arange(left, right),
np.arange(top, bottom))
alt = ds.read(1)[top:bottom, left:right]
lon, lat = ds.xy(yy, xx)
lon, lat, alt = transform(
ds.crs, wgs84,
*map(lambda x: np.array(x).flatten(), (lon, lat, alt)))
# from https://proj.org/operations/conversions/topocentric.html
# should work since proj version 8, gives errors here though
#
# lc = CRS.from_proj4(('cct -d 3 +proj=pipeline +step +proj=cart +ellps=WGS84 '
# + '+step +proj=topocentric +ellps=WGS84 +lat_0=%f +lon_0=%f +h_0=%f') % self.coord0)
# now going first to wgs84 and then using pygeodesy, which is very slow but luckily need to do only once
lc = LocalCartesian(*self.coord0)
dem = np.zeros((len(lon), 3), dtype=np.float32)
for i, (lat_, lon_, alt_) in enumerate(zip(lat, lon, alt)):
xyz = lc.forward(lat_, lon_, alt_)
dem[i, :] = xyz.xyz
# transform to correct coordinate frame (now x east, y north, z up => x east, y south, z down)
dem = tools.q_times_mx(self.w2c.quat.conj(), dem)
dem = dem.reshape((bottom - top, right - left, 3))
return dem
def ray_intersect_dist(self, obj_idxs, rel_pos_v, rel_rot_q):
# return distance to objects along -z-axis, supports laser algorithm, put here because efficient
if False:
# Should find the nearest intersection with object faces on the camera axis.
# However, tools.intersections return some error code, seems difficult to debug..
candidates = []
ray = np.array([0, 0, -1.0]).reshape((3, 1))
for i, obj_idx in enumerate(obj_idxs):
verts = tools.q_times_mx(
rel_rot_q[i],
self._raw_objs[obj_idx].vertices) + rel_pos_v[i]
x = tools.intersections(self._raw_objs[obj_idx].faces, verts,
ray)
candidates.extend(np.abs(x))
dist = np.min(candidates) if len(candidates) > 0 else None
else:
# alternative method: just render and pick center pixel
_, depth = self.render(obj_idxs,
rel_pos_v,
rel_rot_q, [1, 0, 0],
get_depth=True,
shadows=False,
textures=False)
dist = depth[depth.shape[0] // 2, depth.shape[1] // 2]
if dist >= self._frustum_far * 0.99:
dist = None
return dist
def plot_only(path, frame=None):
import matplotlib.pyplot as plt
from matplotlib import cm
from mpl_toolkits.mplot3d import Axes3D
from visnav.missions.nokia import NokiaSensor
w2b, b2c = NokiaSensor.w2b, NokiaSensor.b2c
by_geom, skip = True, 10
depth_path = os.path.join(path, 'depth')
geom_path = os.path.join(path, 'geometry')
for fname in tqdm.tqdm(os.listdir(geom_path if by_geom else depth_path)):
m = re.search(r'(.*?)\.(d|xyz)\.exr', fname)
if not m:
continue
depth_file = os.path.join(depth_path, m[1] + '.d.exr')
geom_file = os.path.join(geom_path, m[1] + '.xyz.exr')
other_exists = os.path.exists(depth_file if by_geom else geom_file)
if not frame or m[1] == frame:
# plot depth
if not by_geom or other_exists:
plt.figure(1)
depth = cv2.imread(os.path.join(depth_path, fname),
cv2.IMREAD_UNCHANGED)
plt.imshow(depth)
# plot geometry
if by_geom or other_exists:
xyz = cv2.imread(geom_file, cv2.IMREAD_UNCHANGED)
xyz = xyz.reshape((-1, 3))
x, y, z = tools.q_times_mx(w2b.quat * b2c.quat,
xyz[::skip, :]).T
f, ax = plt.subplots(1, 1)
ax.set_aspect('equal')
ax.set_xlabel("east", fontsize=12)
ax.set_ylabel("north", fontsize=12)
line = ax.scatter(x,
y,
s=5,
c=z,
marker='o',
vmin=-5.,
vmax=100.,
cmap=cm.get_cmap('jet'))
f.colorbar(line)
tools.hover_annotate(f, ax, line, ['%.1f' % v for v in z])
f.gca().set_title(m[1])
plt.show()
def _px_ray_axes(self, scaling, dq): # TODO: use dq
# construct an array of unit rays, one ray per pixel
iK = self.cam.inv_intrinsic_camera_mx()
xx, yy = np.meshgrid(np.linspace(self.cam.width, 0,
int(self.cam.width * scaling)),
np.linspace(0, self.cam.height,
int(self.cam.height * scaling)),
indexing='xy')
img_coords = np.vstack(
(xx.flatten() + 0.5, yy.flatten() + 0.5, np.ones(xx.size)))
ray_axes = iK.dot(img_coords).T * -1
ray_axes /= np.linalg.norm(ray_axes, axis=1).reshape((-1, 1))
return tools.q_times_mx(dq.conj(), ray_axes), xx.shape
def project(self, cam, pose):
# project 3d points to 2d, filter out non-visible
q_cf = tools.angleaxis_to_q(pose[:3])
pts3d_cf = tools.q_times_mx(q_cf, self.pts3d) + pose[3:]
I = pts3d_cf[:, 2] > 0 # in front of cam
# project without distortion first to remove far away points that would be warped into the image
uvph = cam.cam_mx.dot(pts3d_cf.T).T
pts2d = uvph[:, :2] / uvph[:, 2:]
margin = cam.width * 0.2
I = np.logical_and.reduce(
(I, pts2d[:, 0] >= -margin, pts2d[:, 0] < cam.width + margin,
pts2d[:, 1] >= -margin, pts2d[:, 1] < cam.height + margin))
if np.sum(I) > 0:
pts2d = np.atleast_2d(
cam.project(pts3d_cf[I, :].astype(np.float32)) +
0.5).astype(int)
J = np.logical_and.reduce(
(pts2d[:, 0] >= 0, pts2d[:, 0] < cam.width, pts2d[:, 1] >= 0,
pts2d[:, 1] < cam.height))
pts2d = pts2d[J, :]
px_vals = self.px_vals[I, :][J, :]
# calculate suitable blob radius
median_dist = np.median(np.linalg.norm(pts3d_cf[I, :], axis=1))
radius = round((self.ground_res / 2 / median_dist) /
(1 / cam.cam_mx[0, 0])) # or ceil?
diam = radius * 2 + 1
else:
diam, pts2d, px_vals = 1, np.zeros((0, 2), dtype=int), np.zeros(
(0, 3), dtype=int)
# draw image
image = np.zeros((cam.height, cam.width, 3), dtype=np.uint8)
image[pts2d[:, 1], pts2d[:, 0], :] = px_vals
if diam >= 3:
image = cv2.dilate(
image,
cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (diam, diam)))
# fill in gaps
image = image.reshape((-1, 3)).astype(np.float32)
image[np.all(image == 0, axis=1), :] = np.nan
image = nan_blur(image.reshape((cam.height, cam.width, 3)), (5, 5),
onlynans=True).astype(np.uint8)
return image
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()
# print('est ypr: %s' % ' '.join('%.1fdeg' % math.degrees(a) for a in tools.q_to_ypr(res_q)))
print('rea ypr: %s' % ' '.join('%.1fdeg' % math.degrees(a)
for a in tools.q_to_ypr(ast_q)))
print('est ypr: %s' % ' '.join('%.1fdeg' % math.degrees(a)
for a in tools.q_to_ypr(est_q)))
# print('err ypr: %s' % ' '.join('%.2fdeg' % math.degrees(a) for a in tools.q_to_ypr(err_q)))
print('err angle: %.2fdeg' % math.degrees(err_angle))
# print('rea v: %s' % ' '.join('%.1fm' % a for a in cam_v*1000))
# print('est v: %s' % ' '.join('%.1fm' % a for a in res_v*1000))
print('rea v: %s' % ' '.join('%.1fm' % a for a in ast_v * 1000))
print('est v: %s' % ' '.join('%.1fm' % a for a in est_v * 1000))
# print('err v: %s' % ' '.join('%.2fm' % a for a in err_v*1000))
print('err norm: %.2fm\n' % np.linalg.norm(err_v * 1000))
if False and len(odo.state.map3d) > 0:
pts3d = tools.q_times_mx(SystemModel.cv2gl_q.conj(),
odo.get_3d_map_pts())
tools.plot_vectors(pts3d)
errs = tools.point_cloud_vs_model_err(pts3d, obj)
print('\n3d map err mean=%.3f, sd=%.3f, n=%d' % (
np.mean(errs),
np.std(errs),
len(odo.state.map3d),
))
# print('\n')
else:
print('no solution\n')
#cv2.imshow('image', image)
#cv2.waitKey()
def replay_keyframes(cam: Camera,
keyframes: List[Frame] = None,
map3d: List[Keypoint] = None,
file: str = 'results.pickle'):
import cv2
if keyframes is None:
with open(file, 'rb') as fh:
keyframes, map3d, frame_names, meta_names, ground_truth, *ba_errs = pickle.load(
fh)
ba_errs = ba_errs[0] if len(ba_errs) else None
if isinstance(keyframes[0], Frame):
keyframes = [
dict(pose=kf.pose,
meas=kf.measure,
time=kf.time,
id=kf.id,
kps_uv=kf.kps_uv,
image=kf.image) for kf in keyframes
]
kp_size, kp_color = 5, (200, 0, 0)
kp_ids = set(kf.id for kf in map3d
if not kf.bad_qlt and kf.inlier_count > 2
and kf.inlier_count / kf.total_count > 0.2)
map3d = {kf.id: kf for kf in map3d}
img_scale = 0.5
for kf in keyframes:
if kf['pose'] is None or kf['pose'].post is None:
continue
obs_ids = list(kp_ids.intersection(kf['kps_uv'].keys()))
if len(obs_ids) == 0:
continue
image = kf['image'].copy() if 'image' in kf else np.zeros(
(int(cam.height * img_scale), int(cam.width * img_scale), 3),
dtype=np.uint8)
if len(image.shape) == 2 or image.shape[2] == 1:
image = cv2.cvtColor(image, cv2.COLOR_GRAY2BGR)
p_pts2d = (np.array([kf['kps_uv'][id]
for id in obs_ids]) + 0.5).astype(int).squeeze()
p_pts3d = np.array([map3d[id].pt3d for id in obs_ids])
pts3d_cf = tools.q_times_mx(kf['pose'].post.quat,
p_pts3d) + kf['pose'].post.loc
pts2d_proj = (cam.project(pts3d_cf.astype(np.float32)) * img_scale +
0.5).astype(int)
for (x, y), (xp, yp) in zip(p_pts2d, pts2d_proj):
image = cv2.circle(image, (x, y), kp_size, kp_color,
1) # negative thickness => filled circle
image = cv2.rectangle(image, (xp - 2, yp - 2), (xp + 2, yp + 2),
kp_color, 1)
image = cv2.line(image, (xp, yp), (x, y), kp_color, 1)
cv2.imshow('keypoint reprojection', image)
cv2.waitKey()
def flux_density(cam_q,
cam,
mask=None,
mag_cutoff=MAG_CUTOFF,
array=False,
undistorted=False,
order_by=None):
"""
plots stars based on Tycho-2 database, gives out photon count per unit area given exposure time in seconds,
cam_q is a quaternion in ICRS coord frame, x_fov and y_fov in degrees
"""
# calculate query conditions for star ra and dec
cam_dec, cam_ra, _ = tools.q_to_ypr(
cam_q) # camera boresight in ICRS coords
d = np.linalg.norm((cam.x_fov, cam.y_fov)) / 2
min_dec, max_dec = math.degrees(cam_dec) - d, math.degrees(cam_dec) + d
dec_cond = '(dec BETWEEN %s AND %s)' % (min_dec, max_dec)
# goes over the pole to the other side of the sphere, easy solution => ignore limit on ra
skip_ra_cond = min_dec < -90 or max_dec > 90
if skip_ra_cond:
ra_cond = '1'
else:
min_ra, max_ra = math.degrees(cam_ra) - d, math.degrees(cam_ra) + d
if min_ra < 0:
ra_cond = '(ra < %s OR ra > %s)' % (max_ra,
(min_ra + 360) % 360)
elif max_ra > 360:
ra_cond = '(ra > %s OR ra < %s)' % (min_ra, max_ra % 360)
else:
ra_cond = '(ra BETWEEN %s AND %s)' % (min_ra, max_ra)
conn = sqlite3.connect(Stars.STARDB)
cursor = conn.cursor()
# the magnitudes for tycho id xxxx-xxxxx-2 entries are bad as they are most likely taken from hip catalog that bundles all .*-(\d)
results = cursor.execute(
"""
SELECT x, y, z, mag_v""" +
(", mag_b, t_eff, fe_h, log_g, dec, ra, id" if array else "") + """
FROM deep_sky_objects
WHERE """ + ("tycho like '%-1' AND " if Stars.STARDB ==
Stars.STARDB_TYC else "") + "mag_v < " +
str(mag_cutoff) + " AND " + dec_cond + " AND " + ra_cond +
((" ORDER BY %s ASC" % order_by) if order_by is not None else ''))
stars = np.array(results.fetchall())
conn.close()
flux_density = ([], None) if array else np.zeros(
(cam.height, cam.width), dtype=np.float32)
if len(stars) == 0:
return flux_density
stars[:, 0:3] = tools.q_times_mx(
SystemModel.sc2gl_q.conj() * cam_q.conj(), stars[:, 0:3])
stars_ixy_ = cam.calc_img_R(stars[:, 0:3], undistorted=undistorted)
stars_ixy = np.round(stars_ixy_.astype(np.float)).astype(np.int)
I = np.logical_and.reduce(
(np.all(stars_ixy >= 0, axis=1), stars_ixy[:, 0] <= cam.width - 1,
stars_ixy[:, 1] <= cam.height - 1))
if array:
cols = ('ix', 'iy', 'x', 'y', 'z', 'mag_v', 'mag_b', 't_eff',
'fe_h', 'log_g', 'dec', 'ra', 'id')
return (np.hstack((stars_ixy_[I, :], stars[I, :])),
dict(zip(cols, range(len(cols)))))
stars_ixy = stars_ixy[I, :]
flux_density_per_star = Stars.magnitude_to_spectral_flux_density(
stars[I, 3])
for i, f in enumerate(flux_density_per_star):
flux_density[stars_ixy[i, 1], stars_ixy[i, 0]] += f
if mask is not None:
flux_density[np.logical_not(mask)] = 0
if True:
# assume every star is like our sun, convert to total flux density [W/m2]
solar_constant = 1360.8
# sun magnitude from http://mips.as.arizona.edu/~cnaw/sun.html
sun_flux_density = Stars.magnitude_to_spectral_flux_density(
Stars.SUN_MAG_V)
flux_density = flux_density * (solar_constant / sun_flux_density)
return flux_density
def apply_transf(transf, x):
""" apply transformation calculated earlier with calc_transf """
tr, q = transf
return tools.q_times_mx(q, x - tr)
def main(mission_sc=False, simple=False):
mpl.rcParams['font.size'] = FONT_SIZE
mpl.rcParams['lines.markersize'] = MARKER_SIZE
mpl.rcParams['lines.linewidth'] = LINE_WIDTH
try:
switches = {'--video'}
args = sys.argv[1:]
video = '--video' in args
if video:
args = [a for a in args if a not in switches]
filename = args[0]
span = tuple(map(int, args[1].split(':'))) if len(args) > 1 else None
is_4km = '4km' in filename
is_vo = '-vo-' in filename
is_nac = '-nac-' in filename or '-1n-' in filename or '-2n-' in filename
except:
print('USAGE: %s <logfile>' % sys.argv[0])
quit()
raw = []
with open(filename, newline='') as fh:
reader = csv.reader(fh, delimiter='\t', quotechar='"')
for i, row in enumerate(reader):
if i == 0 and mission_sc:
continue
raw.append(row)
data = np.array(raw).astype('double')
if span is not None:
data = data[span[0]:span[1], :]
if mission_sc:
plot_mission_sc(data, format=3 if 'results' in filename else 2)
quit()
use_d2 = '-d2-' in filename.lower() or '-2n-' in filename.lower(
) or '-2w-' in filename.lower()
id = filename.split('\\')[-1].split('-')[0]
# time (1)
time = data[:, 0] / 3600
# real D1 pose (7)
d1_loc = data[:, 1:4]
d1_q = data[:, 4:8]
# real D2 pose (7) 15
d2_loc = data[:, 8:11]
d2_q = data[:, 11:15]
trg_loc = d2_loc if use_d2 else d1_loc
trg_q = quaternion.as_quat_array(d2_q if use_d2 else d1_q)
# real s/c pose (7)
sc_loc = data[:, 15:18]
sc_q = quaternion.as_quat_array(data[:, 18:22])
# init s/c pose (7) 29
isc_loc = data[:, 22:25]
isc_q = data[:, 25:29]
# landmark algo loc (3)
# TODO: check that sc_q works, seems that static
spl_loc = trg_loc - tools.q_times_mx(sc_q, data[:, 29:32])
# landmark algo loc ok (1) 33
spl_loc_ok = data[:, 32:33]
spl_loc[np.logical_not(spl_loc_ok).flatten(), :] = np.nan
# landmark algo ori (4)
spl_q = data[:, 33:37]
# landmark algo ori ok (1) 38
spl_q_ok = data[:, 37:38]
spl_q[np.logical_not(spl_q_ok).flatten(), :] = np.nan
# laser algo loc (3)
lsr_loc = trg_loc - tools.q_times_mx(sc_q, data[:, 38:41])
# laser algo loc ok (1) 42
lsr_loc_ok = data[:, 41:42]
lsr_loc[np.logical_not(lsr_loc_ok).flatten(), :] = np.nan
# nav filter loc (3)
flt_loc = data[:, 42:45] if True else np.full_like(spl_loc, np.nan)
# measurement log likelihood (1)
meas_ll = data[:, 45:46]
# delta-v spent (1)
cum_delta_v = data[:, 46:47]
# vo loc (3)
vo_loc = trg_loc - tools.q_times_mx(sc_q, data[:, 47:50])
# vo ori (4)
vo_q = data[:, 50:54]
# vo meas sds (6)
vo_meas_sds = data[:, 54:60]
# vo bias sds (6)
vo_bias_sds = data[:, 60:66]
# vo scale drift sd (1)
vo_bias_sds = data[:, 66:67]
# vo ok
vo_ok = data[:, 67:68]
# phase angle
phase_angle = data[:, 68:69]
# vo scale (1)
vo_scale = data[:, 69:70]
# cnt location (3)
cnt_loc = trg_loc - tools.q_times_mx(
sc_q, data[:, 70:73]) if data.shape[1] >= 73 else np.full_like(
spl_loc, np.nan)
# sun-sc vect
sun_v = data[:, 73:76] if data.shape[1] >= 76 else None
# s/c-target distance
distance = np.linalg.norm(sc_loc - trg_loc, axis=1)
has_spl = not np.all(np.isnan(spl_loc))
has_lsr = not np.all(np.isnan(lsr_loc))
has_vo = not np.all(np.isnan(vo_loc)) # and False
has_cnt = not np.all(np.isnan(cnt_loc))
has_flt = False
if use_d2:
sun_v = sun_v if sun_v is not None else {
'id2': np.array([-0.3067, -0.9427, -0.1315]),
'id4': np.array([-0.5252, -0.8379, -0.1485]),
'id5': np.array([0, -1, 0]),
}[id]
is_d2_ecl = d2_eclipses(sun_v, d1_loc, d2_loc)
d2_ecl = get_intervals(time, is_d2_ecl)
is_d1_bg, is_d1_fg = d2_when_d1_in_view(sc_loc, sc_q, d1_loc, d2_loc)
d1_bg, d1_fg = get_intervals(time,
is_d1_bg), get_intervals(time, is_d1_fg)
if not video:
cnt_max_dist = {
True: { # is_nac
True: 1300, # - use_d2
False: 5800, # - not use_d2
},
False: { # not is_nac
True: 225, # - use_d2
False: 1050, # - not use_d2
},
}[is_nac][use_d2]
cnt_loc[phase_angle.flatten() > 100 / 180 * np.pi, :] = np.nan
cnt_loc[distance.flatten() < cnt_max_dist, :] = np.nan
spl_loc[phase_angle.flatten() > 135 / 180 * np.pi, :] = np.nan
incl_for_stats = (phase_angle < 100 / 180 *
np.pi).flatten() # phase angle less than 100 deg
if use_d2:
incl_for_stats = np.logical_and.reduce((
incl_for_stats,
np.logical_not(is_d1_fg),
np.logical_not(is_d2_ecl),
))
# calculate transformation to synodic frame, apply
tr_sf = calc_transf(d1_loc, d2_loc)
# for error calculation and plots
tr_stf = calc_transf(sc_loc, d2_loc if use_d2 else d1_loc)
c_lab = ('distance', 'along orbit', 'above orbit')
if has_vo:
vo_loc, vo_scale, vo_loc_bias, nkf_idxs, is_mm = vo_data_prep(
vo_loc, vo_scale, vo_bias_sds, sc_loc, trg_loc)
if False:
# correct drifting scale
vo_loc = (vo_loc - trg_loc) / vo_scale.reshape((-1, 1)) + trg_loc
d1_loc_sf = apply_transf(tr_sf, d1_loc)
d2_loc_sf = apply_transf(tr_sf, d2_loc)
trg_loc_sf = d2_loc_sf if use_d2 else d1_loc_sf
sc_loc_sf = apply_transf(tr_sf, sc_loc)
isc_loc_sf = apply_transf(tr_sf, isc_loc)
spl_loc_sf = apply_transf(tr_sf, spl_loc)
lsr_loc_sf = apply_transf(tr_sf, lsr_loc)
flt_loc_sf = apply_transf(tr_sf, flt_loc)
vo_loc_sf = apply_transf(tr_sf, vo_loc)
cnt_loc_sf = apply_transf(tr_sf, cnt_loc)
d1_loc_stf = apply_transf(tr_stf, d1_loc)
d2_loc_stf = apply_transf(tr_stf, d2_loc)
trg_loc_stf = d2_loc_stf if use_d2 else d1_loc_stf
sc_loc_stf = apply_transf(tr_stf, sc_loc)
isc_loc_stf = apply_transf(tr_stf, isc_loc)
spl_loc_stf = apply_transf(tr_stf, spl_loc)
lsr_loc_stf = apply_transf(tr_stf, lsr_loc)
flt_loc_stf = apply_transf(tr_stf, flt_loc)
vo_loc_stf = apply_transf(tr_stf, vo_loc)
cnt_loc_stf = apply_transf(tr_stf, cnt_loc)
if has_vo:
vo_loc_sf = apply_transf(tr_sf, vo_loc)
vo_loc_stf = apply_transf(tr_stf, vo_loc)
vo_loc_bias_stf = apply_transf(tr_stf, vo_loc_bias)
#vo_loc_sf, _, vo_loc_bias_sf, _, _ = vo_data_prep(vo_loc_sf, vo_scale, vo_bias_sds, sc_loc_sf, trg_loc_sf)
#vo_loc_stf, vo_scale, vo_loc_bias_stf, nkf_idxs, is_mm = vo_data_prep(vo_loc_stf, vo_scale, vo_bias_sds, sc_loc_stf, trg_loc_stf)
if has_flt:
flt_err_mean = tools.robust_mean(flt_loc_stf - sc_loc_stf, axis=0)
flt_err_std = tools.robust_std(flt_loc_stf - sc_loc_stf, axis=0)
if has_lsr:
lsr_err_mean = tools.robust_mean((lsr_loc_stf - sc_loc_stf), axis=0)
lsr_err_std = tools.robust_std((lsr_loc_stf - sc_loc_stf), axis=0)
if has_cnt:
cnt_err_mean = tools.robust_mean(
(cnt_loc_stf - sc_loc_stf)[incl_for_stats, :], axis=0)
cnt_err_std = tools.robust_std(
(cnt_loc_stf - sc_loc_stf)[incl_for_stats, :], axis=0)
if has_spl:
spl_err_mean = tools.robust_mean(
(spl_loc_stf - sc_loc_stf)[incl_for_stats, :], axis=0)
spl_err_std = tools.robust_std(
(spl_loc_stf - sc_loc_stf)[incl_for_stats, :], axis=0)
if has_vo:
vo_err_mean = tools.robust_mean(
(vo_loc_stf - sc_loc_stf)[incl_for_stats, :], axis=0)
vo_err_std = tools.robust_std(
(vo_loc_stf - sc_loc_stf)[incl_for_stats, :], axis=0)
# nkf_idxs need to include a nan value between vo resets, is_mm
vo_delta_scale_mean = tools.robust_mean(
np.diff(vo_scale[nkf_idxs])[np.logical_not(is_mm[1:])])
vo_delta_scale_std = tools.robust_std(np.diff(
vo_scale[nkf_idxs])[np.logical_not(is_mm[1:])],
mean=0)
vo_delta_bias_mean = tools.robust_mean(np.diff(
vo_loc_bias_stf[nkf_idxs], axis=0)[np.logical_not(is_mm[1:]), :],
axis=0)
vo_delta_bias_std = tools.robust_std(np.diff(
vo_loc_bias_stf[nkf_idxs], axis=0)[np.logical_not(is_mm[1:]), :],
mean=0,
axis=0)
vo_mm_delta_scale_mean = tools.robust_mean(
np.diff(vo_scale[nkf_idxs])[is_mm[1:]])
vo_mm_delta_scale_std = tools.robust_std(np.diff(
vo_scale[nkf_idxs])[is_mm[1:]],
mean=0)
vo_mm_delta_bias_mean = tools.robust_mean(np.diff(
vo_loc_bias_stf[nkf_idxs], axis=0)[is_mm[1:], :],
axis=0)
vo_mm_delta_bias_std = tools.robust_std(np.diff(
vo_loc_bias_stf[nkf_idxs], axis=0)[is_mm[1:], :],
mean=0,
axis=0)
cutoff_time = time[0] + {
'id1': time[-1],
'id2': 1.5 * 73.125,
'id3': time[-1],
'id4': 4 * 11.91,
'id5': 2 * 11.91,
}[id]
cutoff = np.argmax(time > cutoff_time)
# normal plots
if simple:
fig2, axs = plt.subplots(4 + (0 if has_vo else 0),
1,
sharex=True,
figsize=(8, 6))
axs[0].plot(time, phase_angle / np.pi * 180, 'C0', label='phase angle')
axs[0].set_ylabel('phase angle', color='C0')
axs[0].tick_params(axis='y', labelcolor='C0')
axs[0].set_ybound(0, 180)
ax0b = axs[0].twinx()
ax0b.plot(time, distance, 'C1', label='distance')
ax0b.set_ylabel('distance', color='C1')
ax0b.tick_params(axis='y', labelcolor='C1')
axs[-1].set_xlabel('time [h]')
for i, lab in enumerate(c_lab):
axs[i + 1].set_ylabel(lab + ' error [m]')
# for i, a in enumerate('real '+a for a in c_lab):
# axs[i+1].plot(time, sc_loc_stf[:, i] - sc_loc_stf[:, i], label=a)
# print('filter err μ=(%.2f, %.2f, %.2f), σ=(%.2f, %.2f, %.2f)' % (*flt_err_mean, *flt_err_std))
# for i, a in enumerate('filter '+a for a in c_lab):
# axs[i+1].plot(time, flt_loc_stf[:, i] - sc_loc_stf[:, i], label=a)
if id in ('id3', 'id5'):
idx = np.isclose((time * 60 * 60 - 5 + 1e-10) % 60, 0)
else:
idx = np.ones(len(time), dtype=np.bool)
if has_cnt:
print('cnt err μ=(%.2f, %.2f, %.2f), σ=(%.2f, %.2f, %.2f)' %
(*cnt_err_mean, *cnt_err_std))
for i, a in enumerate(c_lab):
axs[i + 1].plot(time[idx],
cnt_loc_stf[idx, i] - sc_loc_stf[idx, i],
'C0--',
label='CNT')
if has_spl:
print('spl err μ=(%.2f, %.2f, %.2f), σ=(%.2f, %.2f, %.2f)' %
(*spl_err_mean, *spl_err_std))
for i, a in enumerate(c_lab):
axs[i + 1].plot(time[idx],
spl_loc_stf[idx, i] - sc_loc_stf[idx, i],
'C1--',
label='SPL')
if has_lsr:
print('lsr err μ=(%.2f, %.2f, %.2f), σ=(%.2f, %.2f, %.2f)' %
(*lsr_err_mean, *lsr_err_std))
for i, a in enumerate(c_lab):
axs[i + 1].plot(time[idx],
lsr_loc_stf[idx, i] - sc_loc_stf[idx, i],
'C3:',
label='LSR')
if has_vo:
print('vo delta scale μ=%.2f, σ=%.2f' %
(vo_delta_scale_mean, vo_delta_scale_std))
print('vo delta bias μ=(%.2f, %.2f, %.2f), σ=(%.2f, %.2f, %.2f)' %
(*vo_delta_bias_mean, *vo_delta_bias_std))
print('vo mm delta scale μ=%.2f, σ=%.2f' %
(vo_mm_delta_scale_mean, vo_mm_delta_scale_std))
print(
'vo mm delta bias μ=(%.2f, %.2f, %.2f), σ=(%.2f, %.2f, %.2f)' %
(*vo_mm_delta_bias_mean, *vo_mm_delta_bias_std))
print('vo meas err μ=(%.2f, %.2f, %.2f), σ=(%.2f, %.2f, %.2f)' %
(*vo_err_mean, *vo_err_std))
if id == 'id5':
idx4 = np.isclose((time * 60 * 60 - 5 + 1e-10) % 60, 0)
else:
idx4 = np.ones(len(time), dtype=np.bool)
for i, a in enumerate(c_lab):
axs[i + 1].plot(time[idx4],
vo_loc_stf[idx4, i] - sc_loc_stf[idx4, i],
'C2-.',
label='VO')
# for i, a in enumerate('vo bias ' + a for a in c_lab):
# axs[i].plot(time[idx], vo_loc_bias_stf[idx, i], 'b-', label=a)
# axs[-1].plot(time[idx], vo_scale[idx], 'b-', label='vo scale')
bounded = True
bad_pa = get_intervals(time, phase_angle > 135 / 180 * np.pi)
if id == 'id1':
pass
#axs[i].set_ybound(-1000, 1000)
elif id == 'id2':
if bounded:
axs[1].set_ybound(-400, 400)
axs[2].set_ybound(-40, 40)
axs[3].set_ybound(-40, 40)
elif id == 'id3':
pass
#axs[i].set_ybound(-1000, 1000)
elif id == 'id4':
if bounded:
axs[1].set_ybound(-20, 20)
axs[2].set_ybound(-40, 40)
axs[3].set_ybound(-40, 40)
elif id == 'id5':
if bounded:
axs[1].set_ybound(-5, 5)
axs[2].set_ybound(-10, 10)
axs[3].set_ybound(-10, 10)
for i in range(1, 4):
axs[i].legend(loc='lower right')
for s, e in bad_pa:
axs[i].axvspan(s, e, facecolor='#f7aaaa', alpha=0.5) # pink
if use_d2:
for s, e in d2_ecl:
axs[i].axvspan(s, e, facecolor='#b0f9ef',
alpha=0.5) # turq
for s, e in d1_bg:
axs[i].axvspan(s, e, facecolor='#f8f9b0',
alpha=0.5) # green
for s, e in d1_fg:
axs[i].axvspan(s, e, facecolor='#f5b0f9',
alpha=0.5) # purple
if bounded:
ax0b.set_xbound(time[0], cutoff_time)
else:
fig2, axs = plt.subplots(4 if cum_delta_v[-1] > 0 else 3,
1,
sharex=True,
figsize=(8, 6))
# # location errors
# i = 0
# for j, a in enumerate('real '+a for a in c_lab):
# axs[i].plot(time, sc_loc_stf[:, j], label=a)
# axs[i].set_prop_cycle(None)
# for j, a in enumerate('filter '+a for a in c_lab):
# axs[i].plot(time, flt_loc_stf[:, j], ':', label=a)
#
# axs[i].set_title('filter output\nerr μ=(%.2f, %.2f, %.2f), σ=(%.2f, %.2f, %.2f)' % (*flt_err_mean, *flt_err_std))
# axs[i].legend(loc='lower right')
#
# # measurements
# i += 1
# for j, a in enumerate('real '+a for a in c_lab):
# axs[i].plot(time, sc_loc_stf[:, j], label=a)
# axs[i].set_prop_cycle(None)
#
# if has_spl:
# for j, a in enumerate('spl '+a for a in c_lab):
# axs[i].plot(time, spl_loc_stf[:, j], 'C1--', label=a)
# axs[i].set_prop_cycle(None)
#
# if has_lsr:
# for j, a in enumerate('laser '+a for a in c_lab):
# axs[i].plot(time, lsr_loc_stf[:, j], 'r:', label=a)
# axs[i].set_prop_cycle(None)
#
# if has_vo:
# for j, a in enumerate('vo '+a for a in c_lab):
# axs[i].plot(time, vo_loc_stf[:, j], 'C2.-', label=a)
# axs[i].set_prop_cycle(None)
#
# axs[i].set_title('measurements'
# + ('\nopt err μ=(%.2f, %.2f, %.2f), σ=(%.2f, %.2f, %.2f)' % (*spl_err_mean, *spl_err_std) if has_spl else '')
# + ('\nlsr err μ=(%.2f, %.2f, %.2f), σ=(%.2f, %.2f, %.2f)' % (*lsr_err_mean, *lsr_err_std) if has_lsr else '')
# + ('\nvo err μ=(%.2f, %.2f, %.2f), σ=(%.2f, %.2f, %.2f)' % (*vo_err_mean, *vo_err_std) if has_vo else '')
# )
# axs[i].legend(loc='lower right')
# measurement likelihood
i += 1
axs[i].plot(time, meas_ll)
axs[i].set_title('measurement likelihood')
# delta-v used
if cum_delta_v[-1] > 0:
i += 1
axs[i].plot(time, cum_delta_v)
axs[i].set_title('cumulative delta-v usage')
axs[i].set_xlim(np.min(time), np.max(time))
plt.tight_layout()
# plot didymain & didymoon
fig1, ax = plt.subplots(figsize=(7, 7))
if video:
framerate = 25
dw, dh = fig1.canvas.get_width_height()
writer = cv2.VideoWriter(filename[:-4] + '.avi',
cv2.VideoWriter_fourcc(*'DIVX'), framerate,
(dw * 2, dh * 2))
try:
skip = 2
for c in range(skip, len(d1_loc_sf), skip):
if video:
tools.show_progress(len(d1_loc_sf) // skip, c // skip)
else:
c = cutoff or -1
if is_vo:
# for c in range(0, len(d1_loc_sf), 30):
# plot_orbit_sf(ax, d1_loc_sf, sc_loc_sf, vo_loc_sf, cutoff=c, idx1=1, static=False)
plot_orbit_sf(ax,
d1_loc_sf,
sc_loc_sf,
vo_loc_sf,
cutoff=c,
static=not video)
elif is_4km:
plot_orbit_sf(
ax,
d1_loc,
d2_loc,
sc_loc,
flt_loc if has_flt else None,
spl_loc=spl_loc[idx, :] if id in ('id1', 'id2',
'id3') else None,
vo_loc=vo_loc[idx4, :]
if id in ('id1', 'id3') and has_vo else None,
synodic=False,
cutoff=c,
static=not video)
else:
plot_orbit_sf(
ax,
d1_loc_sf,
d2_loc_sf,
sc_loc_sf,
flt_loc_sf if has_flt else None,
spl_loc=spl_loc_sf[idx, :] if id in ('id4',
'id5') else None,
#vo_loc=vo_loc_sf[idx4, :] if id in ('id5',) else None,
synodic=True,
cutoff=c,
static=not video)
if video:
#plt.tight_layout()
# plt.pause(0.05)
# plt.waitforbuttonpress()
mi = [m for m in (5760, 7593) if m < c]
if len(mi) > 0:
ax.plot(spl_loc[mi, 0],
spl_loc[mi, 1],
'bv',
label='Maneuver',
fillstyle='none')
errtxt = 'error [m]: x=%5.1f, y=%5.1f, z=%5.1f' % tuple(
spl_loc[c, :] - sc_loc[c, :])
plt.text(2650,
9500,
errtxt,
family='monospace',
fontsize=12,
horizontalalignment='center')
ax.set_xbound(-5200, 10500)
ax.set_ybound(-7100, 8600)
fig1.canvas.draw()
img = np.frombuffer(fig1.canvas.tostring_argb(),
dtype=np.uint8)
img.shape = (dh * 3, dw * 3, 4) # why need *3 ???
# canvas.tostring_argb give pixmap in ARGB mode. Roll the ALPHA channel to have it in RGBA mode
img = np.roll(img, 3, axis=2)
img = cv2.cvtColor(img, cv2.COLOR_RGBA2BGR)
img = cv2.resize(img, (dw * 2, dh * 2))
if False:
cv2.imshow('test', img)
cv2.waitKey()
writer.write(img)
ax.clear()
else:
plt.tight_layout()
plt.show()
break
finally:
if video:
writer.release()