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()