def _laser_meas(self, params): time = params[0] d1_v, d1_q, d2_v, d2_q, sc_v, sc_q = self._parse_poses(params, offset=1) d1, d2 = self.asteroids q = SystemModel.sc2gl_q.conj() * sc_q.conj() rel_rot_q = np.array( [q * d1_q * d1.ast2sc_q.conj(), q * d2_q * d2.ast2sc_q.conj()]) rel_pos_v = np.array( [tools.q_times_v(q, d1_v - sc_v), tools.q_times_v(q, d2_v - sc_v)]) self._maybe_load_objects() dist = self._renderer.ray_intersect_dist(self._obj_idxs[0:2], rel_pos_v, rel_rot_q) if dist is None: if np.random.uniform(0, 1) < self._laser_false_prob: noisy_dist = np.random.uniform(self._laser_min_range, self._laser_nominal_max_range) else: noisy_dist = None else: if np.random.uniform(0, 1) < self._laser_fail_prob: noisy_dist = None else: noisy_dist = dist * 1000 + np.random.normal( 0, self._laser_err_sigma) if noisy_dist < self._laser_min_range or noisy_dist > self._laser_max_range: noisy_dist = None return json.dumps(noisy_dist)
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 _update_target(self): """ Change camera orientation so that target is on the camera bore-sight defined by target_axis vector. Additional constraint is needed for unique final rotation, this can be provided by target_up vector. """ boresight = np.array(self.target_axis) loc = self.target.loc - self.loc axis = np.cross(boresight, loc) angle = tools.angle_between_v(boresight, loc) q = tools.angleaxis_to_q((angle,) + tuple(axis)) # if up target given, use it if self.target_up is not None: current_up = tools.q_times_v(q, np.array(self.target_axis_up)) target_up = np.array(self.target_up) # project target_up on camera bore-sight, then remove the projection from target_up to get # it projected on a plane perpendicular to the bore-sight target_up_proj = target_up - np.dot(target_up, loc) * loc / np.dot(loc, loc) if np.linalg.norm(target_up_proj) > 0: axis = np.cross(target_up_proj, current_up) angle = tools.angle_between_v(current_up, target_up_proj) q = tools.angleaxis_to_q((angle,) + tuple(axis)) * q self.q = q
def main(): cam_mx = np.array([[1580.356552, 0.000000, 994.026697], [0.000000, 1580.553177, 518.938726], [0.000000, 0.000000, 1.000000]]) sc_v0, sc_v1 = np.array([0, 0, -100]), np.array([0, -5, -100]) sc_q0, sc_q1 = tools.ypr_to_q(0, 0, 0), tools.ypr_to_q(0, 0, math.radians(-2)) w_q0, w_q1 = sc_q0.conj(), sc_q1.conj() w_v0, w_v1 = tools.q_times_v(w_q0, -sc_v0), tools.q_times_v(w_q1, -sc_v1) f0 = Frame(0, None, 1.0, PoseEstimate(None, Pose(w_v0, w_q0), 0)) f1 = Frame(1, None, 1.0, PoseEstimate(None, Pose(w_v1, w_q1), 0)) ref_pts4d = np.concatenate( (np.random.uniform(-30, 30, (100, 2)), np.zeros( (100, 1)), np.ones((100, 1))), axis=1) P0, P1 = cam_mx.dot(f0.to_mx()), cam_mx.dot(f1.to_mx()) uvs0, uvs1 = P0.dot(ref_pts4d.T).T, P1.dot(ref_pts4d.T).T uvs0, uvs1 = uvs0[:, :2] / uvs0[:, 2:], uvs1[:, :2] / uvs1[:, 2:] plt.figure(1) for uv0, uv1 in zip(uvs0, uvs1): plt.plot([uv0[0], uv1[0]], [uv0[1], uv1[1]]) plt.plot(uvs1[:, 0], uvs1[:, 1], '.') plt.gca().invert_yaxis() pts3d = [] for uv0, uv1 in zip(uvs0, uvs1): kp4d = cv2.triangulatePoints(P0, P1, uv0.reshape((-1, 1, 2)), uv1.reshape((-1, 1, 2))) pt3d = (kp4d.T[:, :3] / kp4d.T[:, 3:])[0] pts3d.append(pt3d) pts3d = np.array(pts3d) fig = plt.figure(2) ax = fig.add_subplot(111, projection='3d') ax.plot(pts3d[:, 0], pts3d[:, 1], pts3d[:, 2], '.') ax.set_xlabel('x') ax.set_ylabel('y') ax.set_zlabel('z') plt.show() print('done')
def flux_density_voxels(self, lf_ast_v, lf_ast_q, mask, solar_flux, down_scaling=1, quad_lim=15): # TODO: implement this someday in OpenGL, e.g. like this: http://www.alexandre-pestana.com/volumetric-lights assert down_scaling >= 1, 'only values of >=1 make sense for down_scaling' dq = lf_ast_q * self.voxels.lf_ast_q.conj() dv = tools.q_times_v(dq.conj(), lf_ast_v - self.voxels.lf_ast_v) ny, nx, nz = self.voxels.voxel_data.shape dx, dy, dz = [self.voxels.cell_size] * 3 gx = np.linspace(-(nx - 1) * dx / 2, (nx - 1) * dx / 2, nx) gy = np.linspace(-(ny - 1) * dy / 2, (ny - 1) * dy / 2, ny) gz = np.linspace(-(nz - 1) * dz / 2, (nz - 1) * dz / 2, nz) interp3d = RegularGridInterpolator((gx, gy, gz), self.voxels.voxel_data, bounds_error=False, fill_value=0.0) #interp3d = NearestNDInterpolator((gx, gy, gz), self.voxels.voxel_data) ray_axes, sc_shape = self._px_ray_axes( 1 / down_scaling, dq) # tools.ypr_to_q(0, np.pi, 0) * dq) margin = 0.0 dist = np.linalg.norm(dv) fg_near = dist - nz * dz / 2 fg_far = dist - margin / 2 bg_near = dist + margin / 2 bg_far = dist + nz * dz / 2 def quad_fn(interp3d, ray_axes, near, far): points = None #np.linspace(near, far, nx/2) res = integrate.quad_vec(lambda r: interp3d(ray_axes * r - dv), near, far, points=points, limit=quad_lim) return res[0] # integrate density along the rays (quad_vec requires at least scipy 1.4.x) res = quad_fn(interp3d, ray_axes, bg_near, bg_far) bg_res = cv2.resize( res.reshape(sc_shape).astype(np.float32), mask.shape) bg_res[mask] = 0 # integrate density along the rays (quad_vec requires at least scipy 1.4.x) res = quad_fn(interp3d, ray_axes, fg_near, fg_far) fg_res = cv2.resize( res.reshape(sc_shape).astype(np.float32), mask.shape) result = ((0 if bg_res is None else bg_res) + (0 if fg_res is None else fg_res)) \ * Particles.CONE_INTENSITY_COEF * solar_flux * self.voxels.intensity return result
def render(self, name_suffix): self.prepare() for i, o in self._objs.values(): o.prepare(self) for c in self._cams.values(): c.prepare(self) sun_sc_v = np.mean(np.array([o.loc - self._sun_loc for _, o in self._objs.values()]).reshape((-1, 3)), axis=0) sun_distance = np.linalg.norm(sun_sc_v) obj_idxs = [i for i, o in self._objs.values()] for cam_name, c in self._cams.items(): rel_pos_v = {} rel_rot_q = {} for i, o in self._objs.values(): rel_pos_v[i] = tools.q_times_v(c.q.conj(), o.loc - c.loc) rel_rot_q[i] = c.q.conj() * o.q # make sure correct order, correct scale rel_pos_v = [rel_pos_v[i]/self.object_scale for i in obj_idxs] rel_rot_q = [rel_rot_q[i] for i in obj_idxs] light_v = tools.q_times_v(c.q.conj(), tools.normalize_v(sun_sc_v)) self._renderer.set_frustum(c.model.x_fov, c.model.y_fov, c.frustum_near, c.frustum_far) flux = TestLoop.render_navcam_image_static(None, self._renderer, obj_idxs, rel_pos_v, rel_rot_q, light_v, c.q, sun_distance, cam=c.model, auto_gain=False, use_shadows=True, use_textures=True, fluxes_only=True, stars=self.stars, lens_effects=self.lens_effects, reflmod_params=self.hapke_params, star_db=RenderScene.STAR_DB) image = flux if self.flux_only else c.model.sense(flux, exposure=c.exposure, gain=c.gain) if self.normalize: image /= np.max(image) if self.debug: sc = 1536/image.shape[0] img = cv2.resize(image, None, fx=sc, fy=sc) / (np.max(image) if self.flux_only else 1) cv2.imshow('result', img) cv2.waitKey() # save image self._save_img(image, cam_name, name_suffix)
def _set_sc_from_ast_rot_and_trans(self, rvec, tvec, discretization_err_q, rotate_sc=False): sm = self.system_model # rotate to gl frame from opencv camera frame gl2cv_q = sm.frm_conv_q(sm.OPENGL_FRAME, sm.OPENCV_FRAME) new_sc_pos = tools.q_times_v(gl2cv_q, tvec) # camera rotation in opencv frame cv_cam_delta_q = tools.angleaxis_to_q(rvec) # solvePnPRansac has some bug that apparently randomly gives 180deg wrong answer if new_sc_pos[2] > 0: tpos = -new_sc_pos tdelta_q = cv_cam_delta_q * tools.lat_lon_roll_to_q(0, math.pi, 0) # logger.info('Bug with solvePnPRansac, correcting:\np\t%s => %s\np\t%s => %s'%( # new_sc_pos, tpos, tools.q_to_lat_lon_roll(cv_cam_delta_q), tools.q_to_lat_lon_roll(tdelta_q))) new_sc_pos = tpos cv_cam_delta_q = tdelta_q sm.spacecraft_pos = new_sc_pos err_q = discretization_err_q or np.quaternion(1, 0, 0, 0) if rotate_sc: sc2cv_q = sm.frm_conv_q(sm.SPACECRAFT_FRAME, sm.OPENCV_FRAME) sc_delta_q = err_q * sc2cv_q * cv_cam_delta_q.conj( ) * sc2cv_q.conj() sm.rotate_spacecraft( sc_delta_q ) # TODO: check that rotation ok, changed from sc_q = sc_q * dq to sc_q = dq * sc_q else: sc2cv_q = sm.frm_conv_q(sm.SPACECRAFT_FRAME, sm.OPENCV_FRAME) sc_q = sm.spacecraft_q frame_q = sc_q * err_q * sc2cv_q ast_delta_q = frame_q * cv_cam_delta_q * frame_q.conj() err_corr_q = sc_q * err_q.conj() * sc_q.conj() ast_q0 = sm.asteroid_q sm.rotate_asteroid(err_corr_q * ast_delta_q) if self.est_real_ast_orient: # so that can track rotation of 67P ast_q = sm.asteroid_q err_deg = math.degrees(tools.angle_between_q(ast_q0, ast_q)) self.extra_values = list(quaternion.as_float_array(ast_q)) + [ sm.time.value, err_deg ]
def north_to_up(sc_q): # assume -y axis is towards north, camera borehole is along +z axis, and cam up is towards -y axis north_v, cam_axis, cam_up = np.array([0, -1, 0]), np.array([0, 0, 1]), np.array([0, -1, 0]) # north vector in image frame sc_north = tools.q_times_v(sc_q, north_v) # project to image plane img_north = tools.vector_rejection(sc_north, cam_axis) # calculate angle between projected north vector and image up angle = tools.angle_between_v(cam_up, img_north, direction=cam_axis) return angle
def closest_scene(self, sc_ast_q=None, light_v=None): """ in opengl frame """ if sc_ast_q is None: sc_ast_q, _ = self.system_model.gl_sc_asteroid_rel_q() if light_v is None: light_v, _ = self.system_model.gl_light_rel_dir() d_sc_ast_q, i1 = tools.discretize_q(sc_ast_q, points=self._fdb_sc_ast_perms) err_q = sc_ast_q * d_sc_ast_q.conj() c_light_v = tools.q_times_v(err_q.conj(), light_v) d_light_v, i2 = tools.discretize_v(c_light_v, points=self._fdb_light_perms) err_angle = tools.angle_between_v(light_v, d_light_v) return i1, i2, d_sc_ast_q, d_light_v, err_q, err_angle
def _render_params(self, discretize_tol=False, center_model=False): assert not discretize_tol, 'discretize_tol deprecated at _render_params function' m = self.system_model # NOTE: with wide angle camera, would need to take into account # im_xoff, im_yoff, im_width and im_height xc_off = (self.im_xoff + self.im_width / 2 - self._cam.width / 2) xc_angle = xc_off / self._cam.width * math.radians(self._cam.x_fov) yc_off = (self.im_yoff + self.im_height / 2 - self._cam.height / 2) yc_angle = yc_off / self._cam.height * math.radians(self._cam.y_fov) # first rotate around x-axis, then y-axis, # note that diff angle in image y direction corresponds to rotation # around x-axis and vise versa q_crop = (np.quaternion(math.cos(-yc_angle / 2), math.sin( -yc_angle / 2), 0, 0) * np.quaternion(math.cos(-xc_angle / 2), 0, math.sin(-xc_angle / 2), 0)) x = m.x_off.value y = m.y_off.value z = m.z_off.value # rotate offsets using q_crop x, y, z = tools.q_times_v(q_crop.conj(), np.array([x, y, z])) # maybe put object in center of view if center_model: x, y = 0, 0 # get object rotation and turn it a bit based on cropping effect q, err_q = m.gl_sc_asteroid_rel_q(discretize_tol) sc2gl_q = m.frm_conv_q(m.SPACECRAFT_FRAME, m.OPENGL_FRAME) self.latest_discretization_err_q = sc2gl_q * err_q * sc2gl_q.conj( ) if discretize_tol else False qfin = (q * q_crop.conj()) # light direction light, err_angle = m.gl_light_rel_dir(err_q, discretize_tol) self.latest_discretization_light_err_angle = err_angle if discretize_tol else False return (x, y, z), qfin, light
def renderParams(self): m = self.systemModel # NOTE: with wide angle camera, would need to take into account # im_xoff, im_yoff, im_width and im_height xc_off = (self.im_xoff + self.im_width / 2 - m.cam.width / 2) xc_angle = xc_off / m.cam.width * math.radians(m.cam.x_fov) yc_off = (self.im_yoff + self.im_height / 2 - m.cam.height / 2) yc_angle = yc_off / m.cam.height * math.radians(m.cam.y_fov) # first rotate around x-axis, then y-axis, # note that diff angle in image y direction corresponds to rotation # around x-axis and vise versa q_crop = (np.quaternion(math.cos(-yc_angle / 2), math.sin( -yc_angle / 2), 0, 0) * np.quaternion(math.cos(-xc_angle / 2), 0, math.sin(-xc_angle / 2), 0)) x = m.x_off.value y = m.y_off.value z = m.z_off.value # rotate offsets using q_crop x, y, z = tools.q_times_v(q_crop.conj(), np.array([x, y, z])) # maybe put object in center of view if self._center_model: x, y = 0, 0 # get object rotation and turn it a bit based on cropping effect q, err_q = m.gl_sc_asteroid_rel_q(self._discretize_tol) if self._discretize_tol: self.latest_discretization_err_q = err_q qfin = (q * q_crop.conj()) rv = tools.q_to_angleaxis(qfin) # light direction light, _ = m.gl_light_rel_dir(err_q) res = (light, (x, y, z), (math.degrees(rv[0]), ) + tuple(rv[1:])) return res
def costfun(x, debug=0, verbose=True): set_params(x) err = 0 for phase_angle in np.radians(np.linspace(0, 150, img_n)): light = tools.q_times_v(tools.ypr_to_q(phase_angle, 0, 0), np.array([0, 0, -1])) synth1 = re.render(obj_idx, pos, np.quaternion(1, 0, 0, 0), tools.normalize_v(light), get_depth=False, reflection=m_hapke) synth2 = re.render(obj_idx, pos, np.quaternion(1, 0, 0, 0), tools.normalize_v(light), get_depth=False, reflection=m_ll) err_img = (synth1.astype('float') - synth2.astype('float'))**2 err += np.mean(err_img) if debug: if debug % 2: cv2.imshow( 'hapke vs ll', np.concatenate((synth1.astype('uint8'), 255 * np.ones( (synth2.shape[0], 1), dtype='uint8'), synth2), axis=1)) if debug > 1: err_img = err_img**0.2 cv2.imshow('err', err_img / np.max(err_img)) cv2.waitKey() err /= img_n if verbose: print('%s => %f' % (', '.join(['%.4e' % i for i in np.array(x) * scales]), err)) return err
control = RenderController(outpath) control.create_scene('test_sc') control.set_scene_config({ 'debug': True, 'flux_only': False, 'normalize': False, 'stars': True, 'lens_effects': True, # includes the sun 'hapke_params': RenderObject.HAPKE_PARAMS, }) control.create_camera('test_cam', scenes='test_sc') control.configure_camera('test_cam', lens=35.0, sensor=5e-3*1024) control.set_exposure(0.001 if target == 'sun' else 0.3 if target == 'sssb' else 50.0) control.set_resolution((1024, 1024)) control.set_output_format('png', '8') control.set_sun_location(tools.q_times_v(tools.ypr_to_q(math.radians(-90+120), 0, 0), [1.496e11, 0, 0])) control.set_camera_location('test_cam') objfile1, url1 = os.path.join(datapath, 'ryugu+tex-d1-16k.obj'), 'https://drive.google.com/uc?authuser=0&id=1Lu48I4nnDKYtvArUN7TnfQ4b_MhSImKM&export=download' objfile2, url2 = os.path.join(datapath, 'ryugu+tex-d1-16k.mtl'), 'https://drive.google.com/uc?authuser=0&id=1qf0YMbx5nIceGETqhNqePyVNZAmqNyia&export=download' objfile3, url3 = os.path.join(datapath, 'ryugu.png'), 'https://drive.google.com/uc?authuser=0&id=19bT_Qd1MBfxM1wmnCgx6PT58ujnDFmsK&export=download' RenderController.download_file(url1, objfile1, maybe=True) RenderController.download_file(url2, objfile2, maybe=True) RenderController.download_file(url3, objfile3, maybe=True) obj = control.load_object(os.path.join(datapath, 'ryugu+tex-d1-16k.obj'), 'ryugu-16k') obj.location = (0, 0, 0) if target == 'sun': control.target_camera(control.SOL, "test_cam") elif target == 'sssb': control.target_camera(obj, "test_cam") else: control.set_camera_location("test_cam", None, (0, 1, 0, 0))
def export(sm, dst_path, src_path=None, src_imgs=None, trg_shape=(224, 224), crop=False, debug=False, img_prefix="", title=""): trg_w, trg_h = trg_shape assert (src_path is not None) + (src_imgs is not None) == 1, 'give either src_path or src_imgs, not both' if debug: renderer = RenderEngine(sm.cam.width, sm.cam.height, antialias_samples=0) obj_idx = renderer.load_object(sm.asteroid.target_model_file, smooth=sm.asteroid.render_smooth_faces) algo = AlgorithmBase(sm, renderer, obj_idx) metadatafile = os.path.join(dst_path, 'dataset_all.txt') if not os.path.exists(metadatafile): with open(metadatafile, 'w') as f: f.write('\n'.join(['%s, camera centric coordinate frame used' % title, 'Image ID, ImageFile, Target Pose [X Y Z W P Q R], Sun Vector [X Y Z]', '', ''])) files = list(os.listdir(src_path)) if src_imgs is None else src_imgs files = sorted(files) id = 0 for i, fn in enumerate(files): if src_imgs is not None or re.search(r'(?<!far_)\d{4}\.png$', fn): c = 2 if src_imgs is None else 1 tools.show_progress(len(files)//c, i//c) id += 1 # read system state, write out as relative to s/c fname = os.path.basename(fn) if src_imgs is None: fn = os.path.join(src_path, fn) lbl_fn = re.sub(r'_%s(\d{4})' % img_prefix, r'_\1', fn[:-4]) + '.lbl' sm.load_state(lbl_fn) sm.swap_values_with_real_vals() if not crop: shutil.copy2(fn, os.path.join(dst_path, fname)) if os.path.exists(fn[:-4] + '.d.exr'): shutil.copy2(fn[:-4] + '.d.exr', os.path.join(dst_path, fname[:-4] + '.d.exr')) if os.path.exists(fn[:-4] + '.xyz.exr'): shutil.copy2(fn[:-4] + '.xyz.exr', os.path.join(dst_path, fname[:-4] + '.xyz.exr')) if os.path.exists(fn[:-4] + '.s.exr'): shutil.copy2(fn[:-4] + '.s.exr', os.path.join(dst_path, fname[:-4] + '.s.exr')) _write_metadata(metadatafile, id, fname, sm.get_system_scf()) continue from visnav.algo.absnet import AbsoluteNavigationNN # read image, detect box, resize, adjust relative pose img = cv2.imread(fn, cv2.IMREAD_GRAYSCALE) assert img is not None, 'image file %s not found' % fn # detect target, get bounds x, y, w, h = ImageProc.single_object_bounds(img, threshold=AbsoluteNavigationNN.DEF_LUMINOSITY_THRESHOLD, crop_marg=AbsoluteNavigationNN.DEF_CROP_MARGIN, min_px=AbsoluteNavigationNN.DEF_MIN_PIXELS, debug=debug) if x is None: continue # write image metadata system_scf = sm.get_cropped_system_scf(x, y, w, h) _write_metadata(metadatafile, id, fname, system_scf) others, (depth, coords, px_size), k = [], [False] * 3, 1 if os.path.exists(fn[:-4] + '.d.exr'): depth = True others.append(cv2.imread(fn[:-4] + '.d.exr', cv2.IMREAD_UNCHANGED)) if os.path.exists(fn[:-4] + '.xyz.exr'): coords = True others.append(cv2.imread(fn[:-4] + '.xyz.exr', cv2.IMREAD_UNCHANGED)) if os.path.exists(fn[:-4] + '.s.exr'): px_size = True others.append(cv2.imread(fn[:-4] + '.s.exr', cv2.IMREAD_UNCHANGED)) # crop & resize image, write it cropped = ImageProc.crop_and_zoom_image(img, x, y, w, h, None, (trg_w, trg_h), others=others) cv2.imwrite(os.path.join(dst_path, fname), cropped[0], [cv2.IMWRITE_PNG_COMPRESSION, 9]) if depth: cv2.imwrite(os.path.join(dst_path, fname[:-4] + '.d.exr'), cropped[k], (cv2.IMWRITE_EXR_TYPE, cv2.IMWRITE_EXR_TYPE_FLOAT)) k += 1 if coords: cv2.imwrite(os.path.join(dst_path, fname[:-4] + '.xyz.exr'), cropped[k], (cv2.IMWRITE_EXR_TYPE, cv2.IMWRITE_EXR_TYPE_FLOAT)) k += 1 if px_size: cv2.imwrite(os.path.join(dst_path, fname[:-4] + '.s.exr'), cropped[k], (cv2.IMWRITE_EXR_TYPE, cv2.IMWRITE_EXR_TYPE_FLOAT)) if debug: sc, dq = sm.cropped_system_tf(x, y, w, h) sm.spacecraft_pos = tools.q_times_v(SystemModel.sc2gl_q.conj(), sc_ast_lf_r) sm.rotate_spacecraft(dq) #sm.set_cropped_system_scf(x, y, w, h, sc_ast_lf_r, sc_ast_lf_q) if False: sm.load_state(lbl_fn) sm.swap_values_with_real_vals() imgd = cv2.resize(img, (trg_h, trg_w)) imge = algo.render(center=False, depth=False, shadows=True) h, w = imge.shape imge = cv2.resize(imge[:, (w - h)//2:(w - h)//2+h], cropped[0].shape) cv2.imshow('equal?', np.hstack(( cropped[0], np.ones((cropped[0].shape[0], 1), dtype=cropped[0].dtype) * 255, imge, ))) cv2.waitKey() if i > 60: quit()
if __name__ == '__main__': from visnav.batch1 import get_system_model # sm = get_system_model('rose024', False) # params = load_image_meta(r'C:\projects\visnav\data\rosetta-mtp024\ROS_CAM1_20151223T145135.LBL', sm) sm = get_system_model('rose018', False) params = load_image_meta(r'C:\projects\visnav\data\rosetta-mtp018\ROS_CAM1_20150710T074301.LBL', sm) sun_ast_igrf_r = sm.asteroid.real_position sun_sc_igrf_r = params['sun_sc_ec_p'] ast_sc_igrf_r = -params['sc_ast_ec_p'] ast_igrf_q = sm.asteroid.rotation_q(sm.time.real_value) z_axis = np.array([0, 0, 1]) x_axis = np.array([1, 0, 0]) ast_axis_u = tools.q_times_v(ast_igrf_q, z_axis) ast_zlon_u = tools.q_times_v(ast_igrf_q, x_axis) ast_axis_dec, ast_axis_ra, _ = tools.cartesian2spherical(*ast_axis_u) ast_zlon_ra = sm.asteroid.rotation_theta(sm.time.real_value) arr2str = lambda arr: '[%s]' % ', '.join(['%f' % v for v in arr]) print('sun_ast_igrf_r [m]: %s' % arr2str(sun_ast_igrf_r * 1e3)) print('sun_sc_igrf_r [m]: %s' % arr2str(sun_sc_igrf_r * 1e3)) print('ast_sc_igrf_r [m]: %s' % arr2str(ast_sc_igrf_r * 1e3)) print('ast_axis_ra [deg]: %f' % math.degrees(ast_axis_ra)) print('ast_axis_dec [deg]: %f' % math.degrees(ast_axis_dec)) print('ast_zlon_ra [deg]: %f' % math.degrees(ast_zlon_ra)) aa = quaternion.as_rotation_vector(sm.spacecraft_q) angle = np.linalg.norm(aa)
def get_ba_params(path, ff, lf, results, kapt, sensor_id): frames = [(id, fname[sensor_id]) for id, fname in kapt.records_camera.items() if id >= ff and (lf is None or id < lf)] fname2id = {fname: id for id, fname in frames} poses = np.array([[ *tools.q_to_angleaxis(kapt.trajectories[id][sensor_id].r, True), *kapt.trajectories[id][sensor_id].t ] for id, fname in frames]).astype(float) pts3d = kapt.points3d[:, :3] if SET_3D_POINT_ALT: pts3d[:, 1] = -TAKEOFF_LAWN_ALT feat = kapt.keypoints[FEATURE_NAME] uv_map = {} for id_f, fname in frames: uvs = image_keypoints_from_file( os.path.join(path, 'kapture', 'reconstruction', 'keypoints', FEATURE_NAME, fname + '.kpt'), feat.dtype, feat.dsize) uv_map[id_f] = uvs f_uv = {} for id3, r in kapt.observations.items(): for fname, id2 in r[FEATURE_NAME]: if fname in fname2id: id_f = fname2id[fname] f_uv.setdefault(id_f, {})[id3] = uv_map[id_f][id2, :] # f_uv = {id_f: {id3: uv_map[id_f][id2, :] # for id3 in range(len(pts3d)) # for id2 in range(len(uv_map[id_f])) # if (fname, id2) in kapt.observations.get(id3, {}).get(VO_FEATURE_NAME, {})} # for id_f, fname in frames} obs_kp = list(set.union(*[set(m.keys()) for m in f_uv.values()])) cam_idxs, pt3d_idxs, pts2d = list( map( np.array, zip(*[(i, id3, uv.flatten()) for i, (id_f, kps_uv) in enumerate(f_uv.items()) for id3, uv in kps_uv.items()]))) meas_idxs = np.array([ i for i, r in enumerate(results) if r['meas'] is not None and i < len(poses) ], dtype=int) meas_q = {i: results[i]['pose'].prior.quat.conj() for i in meas_idxs} meas_r = np.array([ tools.q_times_v(meas_q[i], -results[i]['pose'].prior.loc) for i in meas_idxs ], dtype=np.float32) meas_aa = np.array( [tools.q_to_angleaxis(meas_q[i], compact=True) for i in meas_idxs], dtype=np.float32) return poses, pts3d, pts2d, cam_idxs, pt3d_idxs, meas_r, meas_aa, meas_idxs, obs_kp
) #keypoint_algo=VisualOdometry.KEYPOINT_FAST) # ast_q = quaternion.one ast_q = tools.rand_q(math.radians(15)) current_sc = 1 / np.linalg.norm(ast_v) sc_threshold = 3 for t in range(100): ast_v[2] += 0.001 #ast_q = q**t #ast_q = tools.rand_q(math.radians(0.1)) * ast_q #n_ast_q = ast_q * tools.rand_q(math.radians(.3)) n_ast_q = ast_q cam_q = SystemModel.cv2gl_q * n_ast_q.conj( ) * SystemModel.cv2gl_q.conj() cam_v = tools.q_times_v(cam_q * SystemModel.cv2gl_q, -ast_v) prior = Pose(cam_v, cam_q, np.ones((3, )) * 0.1, np.ones((3, )) * 0.01) if False and 1 / np.linalg.norm(ast_v) > current_sc * sc_threshold: # TODO: implement crop_model and augment_model obj = tools.crop_model(obj, cam_v, cam_q, sm.cam.x_fov, sm.cam.y_fov) obj = tools.augment_model(obj, multiplier=sc_threshold) obj_idx = re.load_object(obj) current_sc = 1 / np.linalg.norm(ast_v) image = re.render(obj_idx, ast_v, ast_q, np.array([1, 0, -1]) / math.sqrt(2), gamma=1.8,
def plot_results(keyframes=None, map3d=None, frame_names=None, meta_names=None, ground_truth=None, ba_errs=None, file='result.pickle', nadir_looking=False): import matplotlib.pyplot as plt from matplotlib import cm from mpl_toolkits.mplot3d import Axes3D # TODO: remove override nadir_looking = False 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 w2b, b2c = NokiaSensor.w2b, NokiaSensor.b2c nl_dq = tools.eul_to_q( (-np.pi / 2, ), 'y') if nadir_looking else quaternion.one est_loc = np.ones((len(keyframes), 3)) * np.nan est_ori = np.ones((len(keyframes), 3)) * np.nan meas_loc = np.ones((len(keyframes), 3)) * np.nan meas_ori = np.ones((len(keyframes), 3)) * np.nan for i, kf in enumerate(keyframes): if kf and kf['pose'] and kf['pose'].post: if kf['pose'].method == VisualOdometry.POSE_2D3D: est_loc[i, :] = (( -kf['pose'].post).to_global(b2c).to_global(w2b)).loc meas_loc[i, :] = (( -kf['pose'].prior).to_global(b2c).to_global(w2b)).loc est_ori[i, :] = tools.q_to_ypr( nl_dq.conj() * ((-kf['pose'].post).to_global(b2c)).quat) meas_ori[i, :] = tools.q_to_ypr( nl_dq.conj() * ((-kf['pose'].prior).to_global(b2c)).quat) est_ori = est_ori / np.pi * 180 meas_ori = meas_ori / np.pi * 180 if nadir_looking: # TODO: better way, now somehow works heuristically est_ori = est_ori[:, (2, 0, 1)] meas_ori = meas_ori[:, (2, 0, 1)] fst = np.where(np.logical_not(np.isnan(est_loc[:, 0])))[0][0] idx = np.where([kf['pose'] is not None for kf in keyframes])[0] idx2 = np.where([ kf['pose'] is not None and kf['meas'] is not None for kf in keyframes ])[0] t0 = keyframes[idx[0]]['time'].timestamp() t = np.array([keyframes[i]['time'].timestamp() - t0 for i in idx]) t2 = np.array([ keyframes[i]['time'].timestamp() - t0 + keyframes[i]['meas'].time_off + keyframes[i]['meas'].time_adj for i in idx2 ]) dt = np.array([keyframes[i]['meas'].time_adj for i in idx2]) rng = np.nanmax(est_loc[idx, :2], axis=0) - np.nanmin(est_loc[idx, :2], axis=0) rng2 = 0 if len(idx2) == 0 else np.nanmax( meas_loc[idx2, :2], axis=0) - np.nanmin(meas_loc[idx2, :2], axis=0) mrg = 0.05 * max(np.max(rng), np.max(rng2)) min_x = min(np.nanmin(est_loc[idx, 0]), 9e9 if len(idx2) == 0 else np.nanmin(meas_loc[idx2, 0])) - mrg max_x = max(np.nanmax(est_loc[idx, 0]), -9e9 if len(idx2) == 0 else np.nanmax(meas_loc[idx2, 0])) + mrg min_y = min(np.nanmin(est_loc[idx, 1]), 9e9 if len(idx2) == 0 else np.nanmin(meas_loc[idx2, 1])) - mrg max_y = max(np.nanmax(est_loc[idx, 1]), -9e9 if len(idx2) == 0 else np.nanmax(meas_loc[idx2, 1])) + mrg min_z = min(np.nanmin(est_loc[idx, 2]), 9e9 if len(idx2) == 0 else np.nanmin(meas_loc[idx2, 2])) - mrg max_z = max(np.nanmax(est_loc[idx, 2]), -9e9 if len(idx2) == 0 else np.nanmax(meas_loc[idx2, 2])) + mrg xx, yy = np.meshgrid(np.linspace(min_x, max_x, 10), np.linspace(min_y, max_y, 10)) zz = np.ones(xx.shape) * min(min_z + mrg / 2, 0) min_z = min(0, min_z) fig = plt.figure(1) ax = fig.add_subplot(111, projection='3d') ax.plot_surface(xx, yy, zz, zorder=2) line = ax.plot(est_loc[idx, 0], est_loc[idx, 1], est_loc[idx, 2], 'C0', zorder=3) ax.scatter(est_loc[fst, 0], est_loc[fst, 1], est_loc[fst, 2], 'C2', zorder=4) line2 = ax.plot(meas_loc[idx2, 0], meas_loc[idx2, 1], meas_loc[idx2, 2], 'C1', zorder=5) ax.set_xlim(min_x, max_x) ax.set_ylim(min_y, max_y) ax.set_zlim(min_z, max_z) ax.set_xlabel('x') ax.set_ylabel('y') ax.set_zlabel('z') tools.hover_annotate(fig, ax, line[0], frame_names) tools.hover_annotate(fig, ax, line2[0], [meta_names[i] for i in idx2]) if 1: fig, axs = plt.subplots(1, 1) axs = [axs] axs[0].set_aspect('equal') rng_x = max_x - min_x rng_y = max_y - min_y if True or rng_x > rng_y: axs[0].plot(est_loc[fst, 0], est_loc[fst, 1], 'oC0', mfc='none') line = axs[0].plot(est_loc[idx, 0], est_loc[idx, 1], 'C0') # , '+-') line2 = axs[0].plot(meas_loc[idx2, 0], meas_loc[idx2, 1], 'C1') # , '+-') axs[0].set_xlim(min_x, max_x) axs[0].set_ylim(min_y, max_y) axs[0].set_xlabel('x') axs[0].set_ylabel('y') else: axs[0].plot(est_loc[fst, 1], est_loc[fst, 0], 'oC0', mfc='none') line = axs[0].plot(est_loc[idx, 1], est_loc[idx, 0], 'C0') # , '+-') line2 = axs[0].plot(meas_loc[idx2, 1], meas_loc[idx2, 0], 'C1') # , '+-') axs[0].set_xlim(min_y, max_y) axs[0].set_ylim(min_x, max_x) axs[0].set_xlabel('y') axs[0].set_ylabel('x') tools.hover_annotate(fig, axs[0], line[0], frame_names) tools.hover_annotate(fig, axs[0], line2[0], [meta_names[i] for i in idx2]) fig, axs = plt.subplots(2, 1, sharex=True) axs[0].plot(t[fst], est_loc[fst, 2], 'oC0', mfc='none') line = axs[0].plot(t, est_loc[idx, 2], 'C0') # , '+-') line2 = axs[0].plot(t2, meas_loc[idx2, 2], 'C1') # , '+-') tools.hover_annotate(fig, axs[0], line[0], frame_names) tools.hover_annotate(fig, axs[0], line2[0], [meta_names[i] for i in idx2]) axs[0].set_ylabel('z') axs[0].set_xlabel('t') if ba_errs is None: line = axs[1].plot(t2, dt, 'C0') #, '+-') tools.hover_annotate(fig, axs[1], line[0], [meta_names[i] for i in idx2]) axs[1].set_ylabel('dt') else: id2idx = {kf['id']: i for i, kf in enumerate(keyframes)} t3 = [ keyframes[id2idx[int(id)]]['time'].timestamp() - t0 for id in ba_errs[:, 0] ] axs[1].plot(t3, ba_errs[:, 1], 'C0', label='repr [px]') axs[1].plot(t3, ba_errs[:, 2], 'C1', label='loc [m]') axs[1].plot(t3, ba_errs[:, 3], 'C2', label='ori [deg]') axs[1].set_title('BA errors') axs[1].legend() axs[1].set_xlabel('t') fig, axs = plt.subplots(3, 1, sharex=True) for i, title in enumerate(('yaw', 'pitch', 'roll')): axs[i].plot(t[fst], est_ori[fst, i], 'oC0', mfc='none') line = axs[i].plot(t, est_ori[idx, i], 'C0') # , '+-') line2 = axs[i].plot(t2, meas_ori[idx2, i], 'C1') # , '+-') tools.hover_annotate(fig, axs[i], line[0], frame_names) tools.hover_annotate(fig, axs[i], line2[0], [meta_names[j] for j in idx2]) axs[i].set_ylabel(title) axs[i].set_xlabel('t') plt.tight_layout() if map3d is not None and len(map3d) > 0: x, y, z = np.array( [tools.q_times_v(w2b.quat * b2c.quat, pt.pt3d) for pt in map3d]).T if 1: fig, 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=20, c=z, marker='o', vmin=-5., vmax=100., cmap=cm.get_cmap('jet')) #, vmax=20.) fig.colorbar(line) tools.hover_annotate(fig, ax, line, ['%.1f' % v for v in z]) else: fig = plt.figure() ax = fig.add_subplot(111, projection='3d') ax.plot(x, y, z, '.') ax.set_xlabel("east", fontsize=12) ax.set_ylabel("north", fontsize=12) ax.set_zlabel("alt", fontsize=12) plt.show() print('ok: %.1f%%, delta loc std: %.3e' % ( 100 * (1 - np.mean(np.isnan(est_loc[:, 0]))), np.nanstd(np.linalg.norm(np.diff(est_loc[:, :3], axis=0), axis=1)), ))
def _laser_algo(self, params): time = params[0] dist_meas = params[1] if not dist_meas or dist_meas < self._laser_min_range or dist_meas > self._laser_max_range: raise PositioningException( 'invalid laser distance measurement: %s' % dist_meas) d1_v, d1_q, d2_v, d2_q, sc_v, sc_q = self._parse_poses(params, offset=2) q = SystemModel.sc2gl_q.conj() * sc_q.conj() d1, d2 = self.asteroids ast = d2 if self._target_d2 else d1 ast_v = d2_v if self._target_d2 else d1_v ast_q = d2_q if self._target_d2 else d1_q rel_rot_q = q * ast_q * ast.ast2sc_q.conj() rel_pos_v = tools.q_times_v(q, ast_v - sc_v) * 1000 max_r = ast.max_radius max_diam = 2 * max_r / 1000 # set orthographic projection self._onboard_renderer.set_orth_frustum(max_diam, max_diam, -max_diam / 2, max_diam / 2) # render orthographic depth image _, zz = self._onboard_renderer.render(self._target_obj_idx, [0, 0, 0], rel_rot_q, [1, 0, 0], get_depth=True, shadows=False, textures=False) # restore regular perspective projection self._onboard_renderer.set_frustum(self._sm.cam.x_fov, self._sm.cam.y_fov, self._sm.min_altitude * .1, self._sm.max_distance) zz[zz > max_diam / 2 * 0.999] = float('nan') zz = zz * 1000 - rel_pos_v[2] xx, yy = np.meshgrid( np.linspace(-max_r, max_r, self._sm.view_width) - rel_pos_v[0], np.linspace(-max_r, max_r, self._sm.view_height) - rel_pos_v[1]) x_expected = np.clip( (rel_pos_v[0] + max_r) / max_r / 2 * self._sm.view_width + 0.5, 0, self._sm.view_width - 1.001) y_expected = np.clip( (rel_pos_v[1] + max_r) / max_r / 2 * self._sm.view_height + 0.5, 0, self._sm.view_height - 1.001) dist_expected = tools.interp2(zz, x_expected, y_expected, discard_bg=True) # mse cost function balances between adjusted location and measurement error adj_dist_sqr = (zz - dist_meas)**2 + xx**2 + yy**2 cost = self._laser_adj_loc_weight * adj_dist_sqr \ + (self._laser_meas_err_weight - self._laser_adj_loc_weight) * (zz - dist_meas)**2 j, i = np.unravel_index(np.nanargmin(cost), cost.shape) if np.isnan(zz[j, i]): raise PositioningException( 'laser algo results in off asteroid pointing') if math.sqrt(adj_dist_sqr[j, i]) >= self._laser_max_adj_dist: raise PositioningException( 'laser algo solution too far (%.0fm, limit=%.0fm), spurious measurement assumed' % (math.sqrt(adj_dist_sqr[j, i]), self._laser_max_adj_dist)) dx, dy, dz = xx[0, i], yy[j, 0], zz[j, i] - dist_meas if self._result_frame == ApiServer.FRAME_GLOBAL: # return global ast-sc vector est_sc_ast_v = ast_v * 1000 - tools.q_times_v( q.conj(), rel_pos_v + np.array([dx, dy, dz])) else: # return local sc-ast vector est_sc_ast_v = tools.q_times_v(SystemModel.sc2gl_q, rel_pos_v + np.array([dx, dy, dz])) dist_expected = float( dist_expected) if not np.isnan(dist_expected) else -1.0 return json.dumps([list(est_sc_ast_v), dist_expected])
control.set_scene_config({ 'debug': True, 'flux_only': False, 'normalize': False, 'stars': True, 'lens_effects': True, # includes the sun 'brdf_params': RenderObject.HAPKE_PARAMS, }) control.create_camera('test_cam', scenes='test_sc') control.configure_camera('test_cam', lens=35.0, sensor=5e-3 * 1024) control.set_exposure(0.001 if target == 'sun' else 0.3 if target == 'sssb' else 50.0) control.set_resolution((1024, 1024)) control.set_output_format('png', '8') control.set_sun_location( tools.q_times_v(tools.ypr_to_q(math.radians(-90 + 120), 0, 0), [1.496e11, 0, 0])) control.set_camera_location('test_cam') objfile1, url1 = os.path.join( datapath, 'ryugu+tex-d1-16k.obj' ), 'https://drive.google.com/uc?authuser=0&id=1Lu48I4nnDKYtvArUN7TnfQ4b_MhSImKM&export=download' objfile2, url2 = os.path.join( datapath, 'ryugu+tex-d1-16k.mtl' ), 'https://drive.google.com/uc?authuser=0&id=1qf0YMbx5nIceGETqhNqePyVNZAmqNyia&export=download' objfile3, url3 = os.path.join( datapath, 'ryugu.png' ), 'https://drive.google.com/uc?authuser=0&id=19bT_Qd1MBfxM1wmnCgx6PT58ujnDFmsK&export=download' RenderController.download_file(url1, objfile1, maybe=True) RenderController.download_file(url2, objfile2, maybe=True) RenderController.download_file(url3, objfile3, maybe=True) obj = control.load_object(os.path.join(datapath, 'ryugu+tex-d1-16k.obj'), 'ryugu-16k')
def render_67p(show=False): sm = RosettaSystemModel(hi_res_shape_model=False, res_mult=1.0) re = RenderEngine(sm.cam.width, sm.cam.height, antialias_samples=0) re.set_frustum(sm.cam.x_fov, sm.cam.y_fov, 0.05, sm.max_distance) obj_idx = re.load_object(sm.asteroid.real_shape_model) RenderEngine.REFLMOD_PARAMS[RenderEngine.REFLMOD_HAPKE] = [ 553.38, # J 0 27, # th_p 27 0.034, # w 0.034 -0.08, # b -0.078577 0, # c 0 2.25, # B_SH0 2.25 math.radians(0.061), # hs math.radians(0.061) 0, # B_CB0 0 0.005, # hc 0.005 1, # K 1 ] g_sc_q = tools.angleaxis_to_q([1.892926, 0.781228, -0.540109, -0.312995]) # x, y, z = 69.54, 64.11, 162.976134 x, y, z = 146.540, 167.110, 154.976 # asteroid g_ast_q = get_ast_q(x, y, z) g_sol_ast_v = np.array([163613595198.0, 101637176823.0, 36457220690.0 ]) * 1e-3 g_sol_sc_v = np.array([163613518304.0, 101637309778.0, 36457190373.0 ]) * 1e-3 g_sc_ast_v = g_sol_ast_v - g_sol_sc_v l_ast_sc_v = tools.q_times_v(SystemModel.sc2gl_q.conj() * g_sc_q.conj(), g_sc_ast_v) l_ast_q = SystemModel.sc2gl_q.conj() * g_sc_q.conj( ) * g_ast_q * SystemModel.sc2gl_q l_light_v = tools.q_times_v(SystemModel.sc2gl_q.conj() * g_sc_q.conj(), g_sol_ast_v / np.linalg.norm(g_sol_ast_v)) l_vx_ast_q = SystemModel.sc2gl_q.conj() * quaternion.one.conj( ) * g_ast_q * SystemModel.sc2gl_q print(str(l_vx_ast_q)) particles = load_particles( r'C:\projects\sispo\data\models\Jets--ROS_CAM1_20150710T074301.exr', lf_ast_q=l_vx_ast_q, cell_size=0.066667) a, b, c = [0] * 3 w = 10 while True: print('%.3f, %.3f, %.3f' % (x + a, y + b, z + c)) if particles is None and 0: img = re.render(obj_idx, l_ast_sc_v, l_ast_q, l_light_v, flux_density=1.0, gamma=1.0, get_depth=False, shadows=True, textures=False, reflection=RenderEngine.REFLMOD_HAPKE) else: img = TestLoop.render_navcam_image_static( None, re, [obj_idx], rel_pos_v=l_ast_sc_v, rel_rot_q=l_ast_q, light_v=l_light_v, sc_q=g_sc_q, sun_distance=np.linalg.norm(g_sol_ast_v) * 1e3, exposure=None, gain=None, gamma=1.8, auto_gain=True, reflmod_params=RenderEngine.REFLMOD_PARAMS[ RenderEngine.REFLMOD_HAPKE], use_shadows=True, use_textures=False, cam=sm.cam, fluxes_only=True, stars=True, lens_effects=False, particles=particles, return_depth=False) k = output(img, show, maxval=0.50, gamma=1.8) #k = output(img, show, maxval=0.70, gamma=1.0) if k is None or k == 27: break if k in (ord('a'), ord('d')): b += (1 if k == ord('a') else -1) * w if k in (ord('w'), ord('s')): a += (1 if k == ord('s') else -1) * w if k in (ord('q'), ord('e')): c += (1 if k == ord('q') else -1) * w if 0: l_light_v = tools.q_times_v( SystemModel.sc2gl_q.conj() * tools.ypr_to_q( math.radians(a), math.radians(b), math.radians(c)) * g_sc_q.conj(), g_sol_ast_v / np.linalg.norm(g_sol_ast_v)) elif 1: g_ast_q = get_ast_q(x + a, y + b, z + c) l_ast_q = SystemModel.sc2gl_q.conj() * g_sc_q.conj( ) * g_ast_q * SystemModel.sc2gl_q
def _odometry_track(self, params): VO_EST_CAM_POSE = False session = params[0] fname = params[1] img = cv2.imread(fname, cv2.IMREAD_GRAYSCALE) orig_time = params[2] time = datetime.fromtimestamp(orig_time, pytz.utc) sun_ast_v = tools.normalize_v(np.array(params[3][:3])) d1_v, d1_q, d2_v, d2_q, sc_v, sc_q = self._parse_poses(params, offset=4) ast_q = d2_q if self._target_d2 else d1_q ast_v = d2_v if self._target_d2 else d1_v cv2sc_q = SystemModel.cv2gl_q * SystemModel.sc2gl_q.conj() sc_ast_cvf_q = cv2sc_q * sc_q.conj() * ast_q * cv2sc_q.conj() sc_ast_cvf_v = tools.q_times_v(cv2sc_q * sc_q.conj(), ast_v - sc_v) * 1000 if VO_EST_CAM_POSE: # still from global to old ast-sc frame sc_ast_cvf_q = cv2sc_q * ast_q.conj() * sc_q * cv2sc_q.conj() sc_ast_cvf_v = tools.q_times_v(cv2sc_q * ast_q.conj(), sc_v - ast_v) * 1000 # sc_ast_cvf_v = tools.q_times_v(sc_ast_cvf_q * cv2sc_q, sc_v - ast_v) # TODO: modify so that uncertainties are given (or not) prior = Pose(sc_ast_cvf_v, sc_ast_cvf_q, np.ones((3, )) * 0.1, np.ones((3, )) * 0.01) # tracemalloc.start() # current0, peak0 = tracemalloc.get_traced_memory() # print("before: %.0fMB" % (current0/1024/1024)) if session not in self._odometry: self._odometry[session] = VisualOdometry( self._sm.cam, self._sm.view_width * 2, verbose=1, use_scale_correction=False, est_cam_pose=VO_EST_CAM_POSE) post, bias_sds, scale_sd = self._odometry[session].process( img, time, prior, sc_q) # current, peak = tracemalloc.get_traced_memory() # print("transient: %.0fMB" % ((peak - current)/1024/1024)) # tracemalloc.stop() if post is not None: est_sc_ast_scf_q = cv2sc_q.conj() * post.quat * cv2sc_q est_sc_ast_scf_v = tools.q_times_v(cv2sc_q.conj(), post.loc) if VO_EST_CAM_POSE: # still from old ast-sc frame to local sc-ast frame est_sc_ast_scf_q = cv2sc_q.conj() * post.quat.conj() * cv2sc_q # NOTE: we use prior orientation instead of posterior one as the error there grows over time est_sc_ast_scf_v = -tools.q_times_v( cv2sc_q.conj() * prior.quat.conj(), post.loc) if self._result_frame == ApiServer.FRAME_GLOBAL: est_sc_ast_scf_q = sc_q * est_sc_ast_scf_q est_sc_ast_scf_v = tools.q_times_v(sc_q, est_sc_ast_scf_v) # TODO: (1) return in sc local frame # - distance err sd # - lateral err sd # - orientation err sd (pitch & yaw) # - orientation err sd (roll) # - distance bias drift sd # - lateral bias drift sd # - orientation bias drift sd (pitch & yaw) # - orientation bias drift sd (roll) # - scale drift sd dist = np.linalg.norm(sc_ast_cvf_v) bias_sds = bias_sds * dist est_sc_ast_lf_v_s2 = post.loc_s2 * dist est_sc_ast_lf_so3_s2 = post.so3_s2 result = [ list(est_sc_ast_scf_v), list(quaternion.as_float_array(est_sc_ast_scf_q)), list(est_sc_ast_lf_v_s2) + list(est_sc_ast_lf_so3_s2) + list(bias_sds) + [scale_sd], orig_time, ] else: raise PositioningException('No tracking result') # send back in json format return json.dumps(result)
def render(self, name_suffix): self.prepare() for i, o in self._objs.values(): o.prepare(self) for c in self._cams.values(): c.prepare(self) sun_sc_v = np.mean(np.array( [o.loc - self._sun_loc for _, o in self._objs.values()]).reshape( (-1, 3)), axis=0) sun_distance = np.linalg.norm(sun_sc_v) obj_idxs = [i for i, o in self._objs.values()] for cam_name, c in self._cams.items(): rel_pos_v = {} rel_rot_q = {} for i, o in self._objs.values(): rel_pos_v[i] = tools.q_times_v(c.q.conj(), o.loc - c.loc) rel_rot_q[i] = c.q.conj() * o.q # make sure correct order, correct scale rel_pos_v = [rel_pos_v[i] / self.object_scale for i in obj_idxs] rel_rot_q = [rel_rot_q[i] for i in obj_idxs] light_v = tools.q_times_v(c.q.conj(), tools.normalize_v(sun_sc_v)) brdf_params = RenderObject.HAPKE_PARAMS if self.brdf_params is None else self.brdf_params self._renderer.set_frustum(c.model.x_fov, c.model.y_fov, c.frustum_near, c.frustum_far) flux = TestLoop.render_navcam_image_static( None, self._renderer, obj_idxs, rel_pos_v, rel_rot_q, light_v, self._gl2sc(c.q), sun_distance, cam=c.model, auto_gain=False, use_shadows=True, use_textures=True, fluxes_only=True, stars=self.stars, lens_effects=self.lens_effects, particles=self._particles, reflmod_params=brdf_params, star_db=self._stardb) if self.flux_only: image = flux elif self.sispo_cam: image = self.sispo_cam.sense(flux) else: image = c.model.sense(flux, exposure=c.exposure, gain=c.gain) if self.normalize or self.sispo_cam: tmp = np.max(image) image /= tmp if tmp > 0 else 1 # in case whole image is just zeros # TODO: possibly call code related to self.with_infobox or self.with_clipping at compositor.py if self.debug: sc = 1536 / image.shape[0] img = cv2.resize(image, None, fx=sc, fy=sc) / ( np.max(image) if self.flux_only else 1) cv2.imshow('result', img) cv2.waitKey() # save image self._save_img(image, cam_name, name_suffix)
if compare: title = 'real vs synthetic' img = np.concatenate( [real, 255 * np.ones( (real.shape[0], 1), dtype='uint8')] + synth, axis=1) else: title = 'synthetic' img = synth[0] sc = min(1536 / img.shape[1], 1024 / img.shape[0]) cv2.imshow(title, cv2.resize(img, None, fx=sc, fy=sc)) cv2.waitKey() else: # try different light directions a fixed angle (d) away from default d = 10 for a in np.linspace(0, 360 * 2, 36 * 2, endpoint=False): lat, lon, r = tools.cartesian2spherical(*sm.asteroid.real_position) qa = tools.ypr_to_q(lat, lon, 0) qd = tools.ypr_to_q(0, 0, np.radians(a)) * tools.ypr_to_q( np.radians(d), 0, 0) q = qa * qd * qa.conj() sm.asteroid.real_position = tools.q_times_v( q, sm.asteroid.real_position) img = cv2.resize( ab.render(shadows=True, reflection=RenderEngine.REFLMOD_HAPKE), (1024, 1024)) cv2.imshow('s', img) cv2.waitKey() quit()
def flux_density_cones(self, lf_ast_v, lf_ast_q, mask, solar_flux, down_scaling=1, quad_lim=25): """ - The jet generated is a truncated cone that has a density proportional to (truncation_distance/distance_from_untruncated_origin)**2 - Truncation so that base width is 0.1 of mask diameter - base_loc gives the coordinates of the base in camera frame (opengl type, -z camera axis, +y is up) - 95% of luminosity lost when distance from base is `length` - angular_radius [rad] of the cone, 95% of luminosity lost if this much off axis, uses normal distribution - intensity of the cone at truncation point (>0) - if phase_angle < pi/2, cone not drawn on top of masked parts of image as it starts behind object - direction: 0 - to the right, pi/2 - up, pi - left, -pi/2 - down """ assert down_scaling >= 1, 'only values of >=1 make sense for down_scaling' scaling = 1 / down_scaling base_locs = [c.base_loc for c in self.cones] phase_angles = [c.phase_angle for c in self.cones] directions = [c.direction for c in self.cones] trunc_lens = [c.trunc_len for c in self.cones] angular_radii = [c.angular_radius for c in self.cones] intensities = [c.intensity for c in self.cones] axes = [] for i in range(len(self.cones)): # q = np.quaternion(math.cos(-direction / 2), 0, 0, math.sin(-direction / 2)) \ # * np.quaternion(math.cos(phase_angle / 2), 0, math.sin(phase_angle / 2), 0) q1 = np.quaternion(math.cos(-phase_angles[i] / 2), 0, math.sin(-phase_angles[i] / 2), 0) q2 = np.quaternion(math.cos(directions[i] / 2), 0, 0, math.sin(directions[i] / 2)) q = q2 * q1 axis = tools.q_times_v(q, np.array([0, 0, -1])) axes.append(axis) base_locs[i] -= axis * trunc_lens[i] # density function of the jet def density(loc_arr, base_loc, axis, d0, angular_radius, intensity): loc_arr = loc_arr - base_loc r, d = tools.dist_across_and_along_vect(loc_arr, axis) # r, d = tools.point_vector_dist(loc_arr, axis, dist_along_v=True) # get distance along axis coef = np.zeros((len(loc_arr), 1)) coef[d > d0] = (d0 / d[d > d0])**2 # get radial distance from axis, use normal dist pdf but scaled so that max val is 1 r_sd = d[coef > 0] * np.tan(angular_radius) coef[coef > 0] *= np.exp((-0.5 / r_sd**2) * (r[coef > 0]**2)) return coef * intensity dq = lf_ast_q * self.cones.lf_ast_q.conj() dv = tools.q_times_v(dq.conj(), lf_ast_v - self.cones.lf_ast_v) ray_axes, sc_shape = self._px_ray_axes(scaling, dq) def i_fun(r, arg_arr): result = None for args in arg_arr: res = density(ray_axes * r - dv, *args) if result is None: result = res else: result += res return result bg_args_arr, bg_near, bg_far = [], np.inf, -np.inf fg_args_arr, fg_near, fg_far = [], np.inf, -np.inf for args in zip(base_locs, axes, trunc_lens, angular_radii, intensities): base_loc, axis, trunc_len, angular_radius, intensity = args dist = np.linalg.norm(base_loc) if axis[2] < 0: # z-component of axis is negative => jet goes away from cam => starts behind object bg_args_arr.append(args) bg_near = min(bg_near, dist - trunc_len) bg_far = max(bg_far, 2 * dist) # crude heuristic, is it enough? else: # jet goes towards cam fg_args_arr.append(args) fg_near = min(fg_near, 0) fg_far = max(fg_far, dist + trunc_len) bg_res, fg_res = None, None if bg_args_arr: # integrate density along the rays (quad_vec requires at least scipy 1.4.x) res = integrate.quad_vec(lambda r: i_fun(r, bg_args_arr), bg_near, bg_far, limit=quad_lim) # bg_sc = np.max(res[0])/maxval bg_res = cv2.resize(res[0].reshape(sc_shape).astype(np.float32), mask.shape) # bg_res = cv2.resize((res[0]/bg_sc).reshape(xx.shape).astype(img.dtype), img.shape).astype(np.float32)*bg_sc bg_res[mask] = 0 if fg_args_arr: # integrate density along the rays (quad_vec requires at least scipy 1.4.x) res = integrate.quad_vec(lambda r: i_fun(r, fg_args_arr), fg_near, fg_far, limit=quad_lim) # fg_sc = np.max(res[0])/maxval fg_res = cv2.resize(res[0].reshape(sc_shape).astype(np.float32), mask.shape) # fg_res = cv2.resize((res[0]/fg_sc).reshape(xx.shape).astype(img.dtype), img.shape).astype(np.float32)*fg_sc result = ((0 if bg_res is None else bg_res) + (0 if fg_res is None else fg_res)) \ * Particles.CONE_INTENSITY_COEF * solar_flux # max_r = np.max(result) # if max_r > 0: # maxval = ImageProc._img_max_valid(img) # result = (result / max_r) * maxval * np.max(intensities) return result
re = RenderEngine(sm.cam.width, sm.cam.height, antialias_samples=0) re.set_frustum(sm.cam.x_fov, sm.cam.y_fov, 0.05, 2) obj = sm.asteroid.real_shape_model obj_idx = re.load_object(obj) light = np.array([1, 0, -0.5]) light /= np.linalg.norm(light) cam_ast_v0 = np.array([0, 0, -sm.min_med_distance * 0.7]) cam_ast_q0 = quaternion.one dq = tools.angleaxis_to_q((math.radians(1), 0, 1, 0)) inputs = [] for i in range(60): cam_ast_v = cam_ast_v0 cam_ast_q = dq**i * cam_ast_q0 image = re.render(obj_idx, cam_ast_v, cam_ast_q, light, gamma=1.8, get_depth=False) cam_ast_cv_v = tools.q_times_v(SystemModel.cv2gl_q, cam_ast_v) cam_ast_cv_q = SystemModel.cv2gl_q * cam_ast_q * SystemModel.cv2gl_q.conj() inputs.append([image, cam_ast_cv_v, cam_ast_cv_q]) if 0: for image, _, _ in inputs: cv2.imshow('t', cv2.resize(image, None, fx=0.5, fy=0.5)) cv2.waitKey() else: t = TestOdometry() t.setUp(verbose=True) t.test_rotating_object(inputs=inputs) else: unittest.main()
def render_itokawa(show=False): cam = Camera(4096, 4096, aperture=1.5, focal_length=120.8, sensor_size=[12.288] * 2, px_saturation_e=30e3) shape_model_file = [ r'C:\projects\sispo\data\models\itokawa_16k.obj', r'C:\projects\sispo\data\models\itokawa_f3145728.obj', ][0] RenderEngine.REFLMOD_PARAMS[RenderEngine.REFLMOD_HAPKE] = [ 553.38, # J 0 26, # th_p 26 0.31, # w 0.31 -0.35, # b -0.35 0, # c 0 0.86, # B_SH0 0.86 0.00021, # hs 0.00021 0, # B_CB0 0 0.005, # hc 0.005 1, # K 1 ] re = RenderEngine(cam.width, cam.height, antialias_samples=0) #, enable_extra_data=True) re.set_frustum(cam.x_fov, cam.y_fov, 0.05, 12) obj_idx = re.load_object(shape_model_file) if 1: g_sc_q = tools.angleaxis_to_q( [1.847799, -0.929873, 0.266931, -0.253146]) #x, y, z = -13.182141, -64.694813, 116.263134 x, y, z = 93.818, -8.695, 1.263 g_ast_q = get_ast_q(x, y, z) g_sol_ast_v = np.array([146226194732.0, -68812326932.0, -477863381.0 ]) * 1e-3 g_sol_sc_v = np.array([146226188132.0, -68812322442.0, -477863711.0 ]) * 1e-3 g_sc_ast_v = g_sol_ast_v - g_sol_sc_v l_ast_sc_v = tools.q_times_v( SystemModel.sc2gl_q.conj() * g_sc_q.conj(), g_sc_ast_v) l_ast_q = SystemModel.sc2gl_q.conj() * g_sc_q.conj( ) * g_ast_q * SystemModel.sc2gl_q l_light_v = tools.q_times_v(SystemModel.sc2gl_q.conj() * g_sc_q.conj(), g_sol_ast_v / np.linalg.norm(g_sol_ast_v)) else: l_ast_sc_v = np.array([0, 0, -7.990 * 1]) l_ast_q = tools.angleaxis_to_q((math.radians(20), 0, 1, 0)) l_light_v = np.array([1, 0, -1]) / math.sqrt(2) sc_mode = 0 a, b, c = [0] * 3 ds, dq = 0.135, tools.ypr_to_q(math.radians(1.154), 0, math.radians(-5.643)) # ds, dq = 0.535, quaternion.one # itokawa centered while True: img = re.render(obj_idx, tools.q_times_v(dq, l_ast_sc_v * ds), l_ast_q, l_light_v, flux_density=1.0, gamma=1.0, get_depth=False, shadows=True, textures=True, reflection=RenderEngine.REFLMOD_HAPKE) # data = re.render_extra_data(obj_idx, tools.q_times_v(dq, l_ast_sc_v*ds), l_ast_q, l_light_v) # import matplotlib.pyplot as plt # plt.imshow(data[:, :, 0]) k = output(img, show, maxval=0.90) if k is None or k == 27: break tmp = 1 if k in (ord('a'), ord('s'), ord('q')) else -1 if k in (ord('a'), ord('d')): if sc_mode: dq = tools.ypr_to_q(math.radians(tmp * 0.033), 0, 0) * dq else: b += tmp if k in (ord('w'), ord('s')): if sc_mode: dq = tools.ypr_to_q(0, 0, math.radians(tmp * 0.033)) * dq else: a += tmp if k in (ord('q'), ord('e')): if sc_mode: ds *= 0.9**tmp else: c += tmp if k == ord('i'): y, p, r = tools.q_to_ypr(dq) print('+c: %.3f, %.3f, %.3f' % (x + a, y + b, z + c)) print('ds, h, v: %.3f, %.3f, %.3f' % (ds, math.degrees(y), math.degrees(r))) g_ast_q = get_ast_q(x + a, y + b, z + c) l_ast_q = SystemModel.sc2gl_q.conj() * g_sc_q.conj( ) * g_ast_q * SystemModel.sc2gl_q
def _get_pose(self, params, algo_id=1): # load target navcam image fname = params[0] img = cv2.imread(fname, cv2.IMREAD_GRAYSCALE) # asteroid position relative to the sun self._sm.asteroid.real_position = np.array(params[1][:3]) d1_v, d1_q, d2_v, d2_q, sc_v, init_sc_q = self._parse_poses(params, offset=2) # set asteroid orientation d1, d2 = self.asteroids ast = d2 if self._target_d2 else d1 init_ast_q = d2_q if self._target_d2 else d1_q self._sm.asteroid_q = init_ast_q * ast.ast2sc_q.conj() # set spacecraft orientation self._sm.spacecraft_q = init_sc_q # initial rel q init_rel_q = init_sc_q.conj() * init_ast_q # sc-asteroid relative location ast_v = d2_v if self._target_d2 else d1_v # relative to barycenter init_sc_pos = tools.q_times_v( SystemModel.sc2gl_q.conj() * init_sc_q.conj(), ast_v - sc_v) self._sm.spacecraft_pos = init_sc_pos # run keypoint algo err = None rot_ok = True try: if algo_id == 1: self._keypoint.solve_pnp( img, fname[:-4], KeypointAlgo.AKAZE, verbose=1 if self._result_rendering else 0) elif algo_id == 2: self._centroid.adjust_iteratively(img, fname[:-4]) rot_ok = False else: assert False, 'invalid algo_id=%s' % algo_id except PositioningException as e: err = e rel_gf_q = np.quaternion(*([np.nan] * 4)) if err is None: sc_q = self._sm.spacecraft_q # resulting sc-ast relative orientation rel_lf_q = self._sm.asteroid_q * ast.ast2sc_q if rot_ok else np.quaternion( *([np.nan] * 4)) rel_gf_q = sc_q.conj() * rel_lf_q # sc-ast vector in meters rel_lf_v = tools.q_times_v( SystemModel.sc2gl_q, np.array(self._sm.spacecraft_pos) * 1000) rel_gf_v = tools.q_times_v(sc_q, rel_lf_v) # collect to one result list if self._result_frame == ApiServer.FRAME_GLOBAL: result = [ list(rel_gf_v), list(quaternion.as_float_array(rel_gf_q)) ] else: result = [ list(rel_lf_v), list(quaternion.as_float_array(rel_lf_q)) ] diff_angle = math.degrees( tools.angle_between_q(init_rel_q, rel_gf_q)) if diff_angle > ApiServer.MAX_ORI_DIFF_ANGLE: err = PositioningException( 'Result orientation too different than initial one, diff %.1f°, max: %.1f°' % (diff_angle, ApiServer.MAX_ORI_DIFF_ANGLE)) # render a result image if self._result_rendering: self._render_result([fname] + [ list(np.array(self._sm.spacecraft_pos) * 1000) + list(quaternion.as_float_array(rel_gf_q)) ] + [ list(np.array(init_sc_pos) * 1000) + list(quaternion.as_float_array(init_rel_q)) ]) if err is not None: raise err # send back in json format return json.dumps(result)
def load_image_meta(src, sm): # params given in equatorial J2000 coordinates, details: # https://pds.nasa.gov/ds-view/pds/viewProfile.jsp # ?dsid=RO-C-NAVCAM-2-ESC3-MTP021-V1.0 with open(src, 'r') as f: config_data = f.read() config_data = '[meta]\n' + config_data config_data = re.sub(r'^/\*', '#', config_data, flags=re.M) config_data = re.sub(r'^\^', '', config_data, flags=re.M) config_data = re.sub(r'^(\w+):(\w+)', r'\1__\2', config_data, flags=re.M) config_data = re.sub(r'^END\s*$', '', config_data, flags=re.M) config_data = re.sub(r'^NOTE\s*=\s*"[^"]*"', '', config_data, flags=re.M) config_data = re.sub(r'^OBJECT\s*=\s*.*?END_OBJECT\s*=\s*\w+', '', config_data, flags=re.M | re.S) config_data = re.sub(r' <(DEGREE|SECOND|KILOMETER)>', '', config_data) config = ConfigParser(converters={'tuple': literal_eval}) config.read_string(config_data) image_time = config.get('meta', 'START_TIME') # spacecraft orientation, equatorial J2000 sc_rot_ra = config.getfloat('meta', 'RIGHT_ASCENSION') sc_rot_dec = config.getfloat('meta', 'DECLINATION') sc_rot_cnca = config.getfloat('meta', 'CELESTIAL_NORTH_CLOCK_ANGLE') sc_igrf_q = tools.ypr_to_q( rads(sc_rot_dec), rads(sc_rot_ra), -rads(sc_rot_cnca)) # same with rosetta lbls also # from asteroid to spacecraft, asteroid body fixed coordinates # TODO: figure out why FOR SOME REASON distance is given ~30x too close ast_sc_dist = config.getfloat('meta', 'TARGET_CENTER_DISTANCE') * 30 ast_sc_lat = config.getfloat('meta', 'SUB_SPACECRAFT_LATITUDE') ast_sc_lon = config.getfloat('meta', 'SUB_SPACECRAFT_LONGITUDE') ast_sc_bff_r = tools.spherical2cartesian(rads(ast_sc_lat), rads(ast_sc_lon), ast_sc_dist) ast_axis_img_clk_ang = config.getfloat('meta', 'BODY_POLE_CLOCK_ANGLE') ast_axis_img_plane_ang = config.getfloat( 'meta', 'HAY__BODY_POLE_ASPECT_ANGLE') # what is the use? # from sun to spacecraft, equatorial J2000 ast_sun_dist = config.getfloat('meta', 'TARGET_HELIOCENTRIC_DISTANCE') ast_sun_lat = config.getfloat('meta', 'SUB_SOLAR_LATITUDE') ast_sun_lon = config.getfloat('meta', 'SUB_SOLAR_LONGITUDE') sun_ast_bff_r = -tools.spherical2cartesian(rads(ast_sun_lat), rads(ast_sun_lon), ast_sun_dist) sun_sc_bff_r = sun_ast_bff_r + ast_sc_bff_r ast_axis_sun_ang = config.getfloat('meta', 'HAY__BODY_POLE_SUN_ANGLE') a = config.getfloat('meta', 'SUB_SOLAR_AZIMUTH') # what is this!? # TODO: continue here ast_axis_scf_q = tools.ypr_to_q(-rads(ast_sc_lat), -rads(ast_sc_lon), 0) # TODO: figure out: how to get roll as some ast_axis_img_clk_ang come from ast_sc_lat? ast_rot_scf_q = tools.ypr_to_q(0, 0, -rads(ast_axis_img_clk_ang)) ast_scf_q = ast_axis_scf_q #* ast_rot_scf_q dec = 90 - ast_sc_lat ra = -ast_sc_lon if dec > 90: dec = 90 + ast_sc_lat ra = tools.wrap_degs(ra + 180) print('ra: %f, dec: %f, zlra: %f' % (ra, dec, ast_axis_img_clk_ang)) ast_igrf_q = ast_scf_q * sc_igrf_q sun_ast_igrf_r = tools.q_times_v(ast_igrf_q, sun_ast_bff_r) ast_sc_igrf_r = tools.q_times_v(ast_igrf_q, ast_sc_bff_r) sun_sc_igrf_r = tools.q_times_v(ast_igrf_q, sun_sc_bff_r) z_axis = np.array([0, 0, 1]) x_axis = np.array([1, 0, 0]) ast_axis_u = tools.q_times_v(ast_igrf_q, z_axis) ast_zlon_u = tools.q_times_v(ast_igrf_q, x_axis) ast_axis_dec, ast_axis_ra, _ = tools.cartesian2spherical(*ast_axis_u) ast_zlon_proj = tools.vector_rejection(ast_zlon_u, z_axis) ast_zlon_ra = tools.angle_between_v(ast_zlon_proj, x_axis) ast_zlon_ra *= 1 if np.cross(x_axis, ast_zlon_proj).dot(z_axis) > 0 else -1 # frame where ast zero lat and lon point towards the sun? # ast_axis_ra = -ast_sun_lon # ast_axis_dec = 90 - ast_sun_lat # ast_axis_zero_lon_ra = 0 arr2str = lambda arr: '[%s]' % ', '.join(['%f' % v for v in arr]) print('sun_ast_bff_r: %s' % arr2str(sun_ast_bff_r * 1e3)) print('sun_sc_bff_r: %s' % arr2str(sun_sc_bff_r * 1e3)) print('ast_sc_bff_r: %s' % arr2str(ast_sc_bff_r * 1e3)) # TODO: even the light is wrong, should be right based on the sun_ast and sun_sc vectors!! print('sun_ast_igrf_r: %s' % arr2str(sun_ast_igrf_r * 1e3)) print('sun_sc_igrf_r: %s' % arr2str(sun_sc_igrf_r * 1e3)) print('ast_sc_igrf_r: %s' % arr2str(ast_sc_igrf_r * 1e3)) print('ast_axis_ra: %f' % degs(ast_axis_ra)) print('ast_axis_dec: %f' % degs(ast_axis_dec)) print('ast_zlon_ra: %f' % degs(ast_zlon_ra)) aa = quaternion.as_rotation_vector(sc_igrf_q) angle = np.linalg.norm(aa) sc_angleaxis = [angle] + list(aa / angle) print('sc_angleaxis [rad]: %s' % arr2str(sc_angleaxis))
def _render(self, params): time = params[0] sun_distance = np.linalg.norm(np.array(params[1][:3])) # in meters sun_ast_v = tools.normalize_v(np.array(params[1][:3])) d1_v, d1_q, d2_v, d2_q, sc_v, sc_q = self._parse_poses(params, offset=2) d1, d2 = self.asteroids q = SystemModel.sc2gl_q.conj() * sc_q.conj() rel_rot_q = np.array([ q * d1_q * d1.ast2sc_q.conj(), q * d2_q * d2.ast2sc_q.conj(), np.quaternion(1, 0, 1, 0).normalized() ]) # last one is the for the spacecraft rel_pos_v = np.array([ tools.q_times_v(q, d1_v - sc_v), tools.q_times_v(q, d2_v - sc_v), [0, 0, 0] ]) light_v = tools.q_times_v(q, sun_ast_v) self._maybe_load_objects() # lazy load objects exp_range = (0.001, 3.5) for i in range(20): if self._autolevel and self._current_level: level = self._current_level else: level = 3 * 2.5 * 1.3e-3 if self._use_nac else 1.8 * 2.5 * 1.3e-3 exp, gain = self._sm.cam.level_to_exp_gain(level, exp_range) img = TestLoop.render_navcam_image_static(self._sm, self._renderer, self._obj_idxs, rel_pos_v, rel_rot_q, light_v, sc_q, sun_distance, exposure=exp, gain=gain, auto_gain=False, gamma=1.0, use_shadows=True, use_textures=True) if self._autolevel: v = np.percentile(img, 100 - 0.0003) level_trg = level * 170 / v print( 'autolevel (max_v=%.1f, e=%.3f, g=%.1f) current: %.3f, target: %.3f' % (v, exp, gain, level, level_trg)) self._current_level = level_trg if not self._current_level else \ (self._current_level*self._level_lambda + level_trg*(1-self._level_lambda)) if v < 85 or (v == 255 and level > exp_range[0]): level = level_trg if v < 85 else level * 70 / v self._current_level = level continue break if False: img = ImageProc.default_preprocess(img) date = datetime.fromtimestamp(time, pytz.utc) # datetime.now() fname = os.path.join(self._logpath, date.isoformat()[:-6].replace(':', '')) + '.png' cv2.imwrite(fname, img, [cv2.IMWRITE_PNG_COMPRESSION, 0]) return fname