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 calc_err(self, rvec, tvec, i1, j1, warn=False): q_res = tools.angleaxis_to_q(rvec) lat1, roll1 = self._fdb_sc_ast_perms[i1] lat2, roll2 = self._fdb_sc_ast_perms[j1] q_src = tools.ypr_to_q(lat1, 0, roll1) q_trg = tools.ypr_to_q(lat2, 0, roll2) q_rel = q_trg * q_src.conj() # q_res.x = -q_res.x # np.quaternion(0.707106781186547, 0, -0.707106781186547, 0) m = self.system_model q_frame = m.frm_conv_q(m.OPENGL_FRAME, m.OPENCV_FRAME) q_res = q_frame * q_res.conj() * q_frame.conj() err1 = math.degrees( tools.wrap_rads(tools.angle_between_q(q_res, q_rel))) err2 = np.linalg.norm(tvec - np.array((0, 0, -self.render_z)).reshape((3, 1))) ok = not (abs(err1) > 10 or abs(err2) > 0.10 * abs(self.render_z)) if not ok and warn: print( 'at (%s, %s), err1: %.1fdeg, err2: %.1fkm\n\tq_real: %s\n\tq_est: %s' % (i1, j1, err1, err2, q_rel, q_res)) return ok, err1, err2
def main(outfile='render-test.jpg'): logger = logging.getLogger(__name__) logger.info('Setting up renderer and loading the 3d model...') sm = RosettaSystemModel(hi_res_shape_model=False, skip_obj_load=True) re = RenderEngine(sm.cam.width, sm.cam.height, antialias_samples=0) re.set_frustum(sm.cam.x_fov, sm.cam.y_fov, 0.1 * sm.min_distance, 1.1 * sm.max_distance) obj_path = os.path.join(os.path.dirname(__file__), 'data', '67p-16k.obj') obj_idx = re.load_object(obj_path) pos = np.array([0, 0, -sm.min_med_distance * 1]) q = tools.angleaxis_to_q((math.radians(-20), 0, 1, 0)) light_v = np.array([1, 0, -1]) / math.sqrt(2) logger.info('Start rendering...') with tools.Stopwatch() as time: img = re.render(obj_idx, pos, q, light_v, gamma=1.8, get_depth=False, shadows=True, textures=True, reflection=RenderEngine.REFLMOD_HAPKE) logger.info('Rendered one image in %f secs' % time.elapsed) ok = cv2.imwrite(outfile, img) if ok: logger.info('Result saved in file %s' % outfile) else: logger.warning('Failed to save image in file %s' % outfile)
def set_orientation(self, q=None, angleaxis=None, dec_ra_pa=None): if q is not None: self.q = q elif angleaxis is not None: self.q = tools.angleaxis_to_q(angleaxis) else: assert dec_ra_pa is not None, 'no orientation given' dec, ra, pa = map(math.radians, dec_ra_pa) self.q = tools.ypr_to_q(dec, ra, pa)
def _render_shadowmap(self, obj_idxs, rel_pos_v, rel_rot_q, light_v): # shadows following http://www.opengl-tutorial.org/intermediate-tutorials/tutorial-16-shadow-mapping/ v = np.identity(4) angle = math.acos(np.clip(np.array([0, 0, -1]).dot(light_v), -1, 1)) axis = np.cross(np.array([0, 0, -1]), light_v) q_cam2light = tools.angleaxis_to_q((angle, *axis)) v[:3, :3] = quaternion.as_rotation_matrix(q_cam2light.conj()) mvs = {} for i, obj_idx in enumerate(obj_idxs): m = np.identity(4) m[:3, :3] = quaternion.as_rotation_matrix(rel_rot_q[obj_idx]) m[:3, 3] = rel_pos_v[i] mv = v.dot(m) mvs[obj_idx] = mv proj = self._ortho_mx(obj_idxs, mvs) bias = self._bias_mx( ) # map from [-1,1] x [-1,1] to [0,1]x[0,1] so that can use with "texture" command self._sfbo.use() self._ctx.enable(moderngl.DEPTH_TEST) self._ctx.enable(moderngl.CULL_FACE) self._ctx.front_face = 'ccw' # cull back faces (front faces suggested but that had glitches) self._ctx.clear(depth=float('inf')) shadow_mvps = {} for i in obj_idxs: mvp = proj.dot(mvs[i]) shadow_mvps[i] = bias.dot( mvp ) # used later to project model vertices to same 2d shadow frame self._shadow_prog['mvp'].write(mvp.T.astype('float32').tobytes()) self._s_objs[i].render() data = self._sdbo.read(alignment=1) n = int(math.sqrt(self._samples or 1)) self._shadow_map = self._ctx.texture( (self._width * n, self._height * n), 1, data=data, alignment=1, dtype='f4') if False: import cv2 d = np.frombuffer(data, dtype='f4').reshape( (self._width, self._height)) a = np.max(d.flatten()) b = np.min(d.flatten()) print('%s,%s' % (a, b)) cv2.imshow('distance from sun', d) #cv2.waitKey() #quit() return shadow_mvps
def set_camera_location(self, camera_name="Camera", location=(0, 0, 0), orientation=None, angleaxis=True): cam = self._cams[camera_name] cam.loc = np.array(location) if location is not None else None if orientation is not None: if len(orientation) == 4: if angleaxis: orientation = tools.angleaxis_to_q(orientation) else: orientation = np.quaternion(*orientation) assert isinstance(orientation, np.quaternion), 'orientation needs to be a quaternion or angle-axis with angle given last' cam.q = orientation
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 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 relative_pose_err(sm, imgs): try: img0, img1 = [cv2.imread(img, cv2.IMREAD_UNCHANGED) for img in imgs] kp0, desc0, det = KeypointAlgo.detect_features(img0, KeypointAlgo.AKAZE, max_feats=1000, for_ref=True, maxmem=0) kp1, desc1, det = KeypointAlgo.detect_features(img1, KeypointAlgo.AKAZE, max_feats=1000, for_ref=False, maxmem=0) matches = KeypointAlgo.match_features(desc1, desc0, det.defaultNorm(), method='brute') depth0 = cv2.imread(imgs[0][:-4] + '.d.exr', cv2.IMREAD_UNCHANGED) kp0_3d = KeypointAlgo.inverse_project( sm, [kp0[m.trainIdx].pt for m in matches], depth0, np.nanmax(depth0), 1) kp1_2d = np.array([kp1[m.queryIdx].pt for m in matches], dtype='float32') rvec, tvec, inliers = KeypointAlgo.solve_pnp_ransac( sm, kp1_2d, kp0_3d, 8) except PositioningException as e: inliers = [] err = np.nan if len(inliers) >= KeypointAlgo.MIN_FEATURES: rel_q = true_relative_pose(sm, imgs) sc2cv_q = sm.frm_conv_q(sm.SPACECRAFT_FRAME, sm.OPENCV_FRAME) est_rel_cv_q = tools.angleaxis_to_q(rvec) est_rel_q1 = sc2cv_q * est_rel_cv_q * sc2cv_q.conj() # solvePnPRansac has some bug that apparently randomly gives 180deg wrong answer est_rel_q2 = sc2cv_q * est_rel_cv_q * tools.ypr_to_q( 0, math.pi, 0) * sc2cv_q.conj() err = math.degrees( min(tools.angle_between_q(est_rel_q1, rel_q), tools.angle_between_q(est_rel_q2, rel_q))) return err
def get_pose(self, coord0, mes0, mes1, weight, full_rot): ypr0 = np.flip(mes0[3:6]) if full_rot else [mes0[5], 0, 0] ypr1 = np.flip(mes1[3:6]) if full_rot else [mes1[5], 0, 0] cf_w2c0 = tools.lla_ypr_to_loc_quat(coord0, mes0[:3], ypr0, b2c=self.b2c) cf_w2c1 = tools.lla_ypr_to_loc_quat(coord0, mes1[:3], ypr1, b2c=self.b2c) delta = cf_w2c1 - cf_w2c0 aa = tools.q_to_angleaxis(delta.quat) aa[0] = tools.wrap_rads(aa[0]) * weight cf_w2c = Pose(cf_w2c0.loc * (1 - weight) + cf_w2c1.loc * weight, tools.angleaxis_to_q(aa) * cf_w2c0.quat) cf_c2w = -cf_w2c return cf_c2w
def render_didymos(show=False): sm = DidymosSystemModel(use_narrow_cam=False, target_primary=False, hi_res_shape_model=False) 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_idx = re.load_object(sm.asteroid.real_shape_model) pos = np.array([0, 0, -sm.min_med_distance * 1]) q = tools.angleaxis_to_q((math.radians(20), 0, 1, 0)) light_v = np.array([1, 0, -1]) / math.sqrt(2) img = re.render(obj_idx, pos, q, light_v, gamma=1.8, get_depth=False, shadows=True, textures=True, reflection=RenderEngine.REFLMOD_HAPKE) output(img, show)
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 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
from visnav.algo.model import SystemModel from visnav.missions.didymos import DidymosSystemModel from visnav.render.render import RenderEngine from visnav.settings import * sm = DidymosSystemModel(use_narrow_cam=False, target_primary=False, hi_res_shape_model=True) 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:
def set_camera_rot(self, angle, axis, camera_name="Camera"): q = tools.angleaxis_to_q((angle, *axis)) self._cams[camera_name].q = q
def main(): parser = argparse.ArgumentParser( description= 'Estimate focal length and simple radial distortion based on ' 'flat ground at the starting location') parser.add_argument( '--path', nargs='+', required=True, help='path to folder with result.pickle and kapture-folder') parser.add_argument('--takeoff', nargs='+', type=int, required=True, help='take-off trajectory in question?') parser.add_argument('--first-frame', '-f', nargs='+', type=int, required=True, help='first frame') parser.add_argument('--last-frame', '-l', nargs='+', type=int, required=True, help='last frame') parser.add_argument('--img-sc', default=0.5, type=float, help='image scale') parser.add_argument('--nadir-looking', action='store_true', help='is cam looking down? used for plots only') parser.add_argument('--ini-fl', type=float, help='initial value for focal length') parser.add_argument('--ini-cx', type=float, help='initial value for x principal point') parser.add_argument('--ini-cy', type=float, help='initial value for y principal point') parser.add_argument('--ini-dist', nargs='+', default=[], type=float, help='initial values for radial distortion coefs') parser.add_argument('--fix-fl', action='store_true', help='do not optimize focal length') parser.add_argument('--fix-dist', action='store_true', help='do not optimize distortion coefs') parser.add_argument( '--pre-ba', action='store_true', help='run one ba pass to update params before optimization') parser.add_argument('--pre-ba-dist-opt', action='store_true', help='optimize r1 and r2 params during pre ba') parser.add_argument('--plot-init', action='store_true', help='plot initial result, after pre ba') parser.add_argument('--opt-loc', action='store_true', help='use gps measure error') parser.add_argument( '--opt-disp', action='store_true', help='use dispersion around estimated surface as a measure') parser.add_argument('--opt-pitch', action='store_true', help='expect pitch to not change') parser.add_argument( '--opt-ground', type=float, help='expect the ground to be at this altitude [m] in the final frame') args = parser.parse_args() cf_args = [] for i, path in enumerate(args.path): with open(os.path.join(path, 'result.pickle'), 'rb') as fh: results, map3d, frame_names, meta_names, gt, ba_errs = pickle.load( fh) kapt = kapture_from_dir(os.path.join(path, 'kapture')) sensor_id, width, height, fl_x, fl_y, pp_x, pp_y, *dist_coefs = get_cam_params( kapt, SENSOR_NAME) if args.ini_fl: fl_x = fl_y = args.ini_fl * args.img_sc if args.ini_cx: pp_x = args.ini_cx * args.img_sc if args.ini_cy: pp_y = args.ini_cy * args.img_sc dist_n = np.where(np.array(dist_coefs) != 0)[0][-1] + 1 dist_coefs = dist_coefs[:dist_n] x0 = list( (args.ini_fl * args.img_sc if args.ini_fl else (fl_x + fl_y) / 2, *(args.ini_dist if len(args.ini_dist) else dist_coefs))) print('loading poses and keypoints...') poses, pts3d, pts2d, cam_idxs, pt3d_idxs, meas_r, meas_aa, meas_idxs, obs_kp = \ get_ba_params(path, args.first_frame[i], args.last_frame[i], results, kapt, sensor_id) if args.pre_ba: if not args.pre_ba_dist_opt: print('running initial global bundle adjustment...') poses, pts3d, _, _, _ = run_ba(width, height, pp_x, pp_y, x0[0], x0[1:], poses, pts3d, pts2d, cam_idxs, pt3d_idxs, meas_r, meas_aa, meas_idxs, optimize_dist=True) if args.pre_ba_dist_opt: print( 'running initial global bundle adjustment with cam params...' ) poses, pts3d, dist, cam_intr, _ = run_ba( width, height, pp_x, pp_y, x0[0], x0[1:], poses, pts3d, pts2d, cam_idxs, pt3d_idxs, meas_r, meas_aa, meas_idxs, optimize_dist=PRE_BA_DIST_ENABLED, n_cam_intr=N_CAM_INTR) if PRE_BA_DIST_ENABLED: x0[1:3] = dist[:2] if cam_intr is not None: if len(cam_intr) != 2: x0[0] = cam_intr[0] if len(cam_intr) > 1: pp_x, pp_y = cam_intr[-2:] lbl = ['fl', 'cx, cy', 'fl, cx, cy'][len(cam_intr) - 1] print('new k1, k2: %s, new %s: %s' % (dist, lbl, list(np.array(cam_intr) / args.img_sc))) else: print('new k1, k2: %s' % (dist, )) results = results[:len(poses)] for j in range(len(results)): results[j][0].post = Pose(poses[j, 3:], tools.angleaxis_to_q(poses[j, :3])) map3d = [Keypoint(pt3d=pts3d[j, :]) for j in range(len(pts3d))] with open(os.path.join(path, 'global-ba-result.pickle'), 'wb') as fh: pickle.dump( (results, map3d, frame_names, meta_names, gt, ba_errs), fh) if args.plot_init: plot_results(results, map3d, frame_names, meta_names, nadir_looking=args.nadir_looking) error_types = {ERROR_TYPE_REPROJECTION} if args.opt_loc: error_types.add(ERROR_TYPE_LOCATION) if args.takeoff[i]: error_types.update({ERROR_TYPE_ALTITUDE, ERROR_TYPE_CURVATURE}) if args.opt_disp: error_types.add(ERROR_TYPE_DISPERSION) elif args.opt_pitch: error_types.add(ERROR_TYPE_PITCH) ground_alt = TAKEOFF_LAWN_ALT if args.opt_ground is not None: error_types.add(ERROR_TYPE_ALTITUDE) ground_alt = args.opt_ground cf_args.append( (width, height, pp_x, pp_y, poses, pts3d, pts2d, cam_idxs, pt3d_idxs, meas_r, meas_aa, meas_idxs, obs_kp, ground_alt, error_types, x0[0] if args.fix_fl else None, x0[1:] if args.fix_dist else None)) print('optimizing focal length and radial distortion coefs...') lo_bounds = (0.7 * x0[0], ) hi_bounds = (1.4 * x0[0], ) diff_step = (0.0003, ) if len(x0) - 1 == 1: lo_bounds += (-0.12, ) hi_bounds += (0.12, ) diff_step += (0.001, ) elif len(x0) - 1 == 2: lo_bounds += (-0.2, -0.6) hi_bounds += (0.2, 0.6) diff_step += (0.001, 0.01) elif len(x0) - 1 == 4: lo_bounds += (-0.5, -0.5, -0.01, -0.01) hi_bounds += (0.5, 0.5, 0.01, 0.01) diff_step += (0.001, 0.01, 0.01, 0.01) else: assert False, 'not supported' if 0: # optimize focal length, simple radial distortion using gaussian process hyperparameter search # TODO pass elif not args.fix_fl and not args.fix_dist: res = least_squares(costfun, tuple(x0), verbose=2, ftol=1e-5, xtol=1e-5, gtol=1e-8, max_nfev=1000, bounds=(lo_bounds, hi_bounds), diff_step=diff_step, args=(cf_args, )) elif args.fix_fl: res = least_squares(costfun, tuple(x0[1:]), verbose=2, ftol=1e-5, xtol=1e-5, gtol=1e-8, max_nfev=1000, bounds=(lo_bounds[1:], hi_bounds[1:]), diff_step=diff_step[1:], args=(cf_args, )) elif args.fix_dist: res = least_squares(costfun, tuple(x0[0:1]), verbose=2, ftol=1e-5, xtol=1e-5, gtol=1e-8, max_nfev=1000, bounds=(lo_bounds[0], hi_bounds[0]), diff_step=diff_step[0], args=(cf_args, )) costfun(res.x, cf_args, plot=True)
def traj_costfun(x, width, height, pp_x, pp_y, poses, pts3d, pts2d, cam_idxs, pt3d_idxs, meas_r, meas_aa, meas_idxs, obs_kp, ground_alt, error_types, def_fl=None, def_dist=None, plot=False): if def_fl is None and def_dist is None: fl, dist_coefs = abs(x[0]), x[1:] elif def_dist is None: fl, dist_coefs = def_fl, x else: fl, dist_coefs = abs(x[0]), def_dist # run ba new_poses, new_pts3d, _, _, ba_errs = run_ba(width, height, pp_x, pp_y, fl, dist_coefs, poses, pts3d, pts2d, cam_idxs, pt3d_idxs, meas_r, meas_aa, meas_idxs) errs = [] labels = [] if ERROR_TYPE_REPROJECTION in error_types: err_repr = ERROR_COEFS[ERROR_TYPE_REPROJECTION] * np.nanmean( ba_errs[:, 0]) errs.append(err_repr) labels.append('repr') if ERROR_TYPE_LOCATION in error_types: err_repr = ERROR_COEFS[ERROR_TYPE_LOCATION] * np.nanmean( np.linalg.norm(ba_errs[:, 1:4], axis=1)) errs.append(err_repr) labels.append('loc') if {ERROR_TYPE_ALTITUDE, ERROR_TYPE_CURVATURE, ERROR_TYPE_DISPERSION}.intersection(error_types): new_pts3d = new_pts3d[obs_kp, :] # fit a plane, based on https://math.stackexchange.com/questions/99299/best-fitting-plane-given-a-set-of-points centroid = np.median(new_pts3d.T, axis=1, keepdims=True) svd = np.linalg.svd(new_pts3d.T - centroid) normal = svd[0][:, -1:].T # fit a parabolic surface to keypoints x0 = 0, *centroid.flatten(), *normal.flatten() res = least_squares(paraboloid_costfun, x0, verbose=0, ftol=1e-5, xtol=1e-5, gtol=1e-8, max_nfev=1000, loss='huber', f_scale=1.0, args=(new_pts3d, )) # extract a plane that corresponds to the fitted parabolic surface (should be more accurate than the median based) c, x0, y0, z0, xn, yn, zn = res.x if ERROR_TYPE_CURVATURE in error_types: errs.append(ERROR_COEFS[ERROR_TYPE_CURVATURE] * c) labels.append('curv') if ERROR_TYPE_DISPERSION in error_types: displ = np.abs(res.fun) lim = np.quantile( displ, 0.8) # how well the keypoints lie on the estimated surface err_disp = np.mean(displ[displ < lim]) errs.append(ERROR_COEFS[ERROR_TYPE_DISPERSION] * err_disp) labels.append('disp') if ERROR_TYPE_ALTITUDE in error_types: centroid = np.array([x0, y0, z0]).reshape((1, 3)) normal = np.array([xn, yn, zn]).reshape((1, 3)) normal /= np.linalg.norm(normal) pose_cf = new_poses[meas_idxs[-1], :] loc_wf = ( -Pose(pose_cf[3:], tools.angleaxis_to_q(pose_cf[:3]))).loc is_takeoff = ERROR_TYPE_CURVATURE in error_types end_meas_alt = -meas_r[-1 if is_takeoff else 0, 2] - ground_alt # neg z-axis is altitude if 1: end_distance = np.abs( normal.dot(loc_wf[:, None] - centroid.flatten()[:, None])) err_alt = (end_meas_alt - end_distance)[0, 0] else: err_alt = np.quantile(-new_pts3d[:, 2], 0.2) - ground_alt errs.append(ERROR_COEFS[ERROR_TYPE_ALTITUDE] * err_alt / end_meas_alt) labels.append('alt') if ERROR_TYPE_PITCH in error_types: ypr = np.array( [tools.q_to_ypr(tools.angleaxis_to_q(p[:3])) for p in new_poses]) / np.pi * 180 err_pitch = ypr[-1, 1] - ypr[0, 1] errs.append(ERROR_COEFS[ERROR_TYPE_PITCH] * err_pitch) labels.append('pitch') print('=== fl: %s, dist: %s, cost: %s ===' % (fl / 0.5, dist_coefs, dict(zip(labels, errs)))) if plot: import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D fig = plt.figure() ax = fig.add_subplot(111, projection='3d') ax.plot(new_pts3d[:, 0], new_pts3d[:, 1], new_pts3d[:, 2], 'C0.') plt.show() return np.array(errs)
def load_coma(self, filename, dimensions, resolution, intensity, gf_ast_aa, scenes=None): if self.verbose: print('loading coma...', end='', flush=True) cell_size = dimensions[0] / resolution gf_ast_q = tools.angleaxis_to_q(gf_ast_aa) gf_vx_cam_q = np.quaternion(0.5, 0.5, -0.5, -0.5) lf_vx_ast_q = gf_vx_cam_q.conj() * gf_ast_q image = OpenEXR.InputFile(filename) header = image.header() mono = 'Y' in header['channels'] size = header["displayWindow"] shape = (size.max.x - size.min.x + 1, size.max.y - size.min.y + 1) if mono: data2d = np.frombuffer( image.channel('Y', Imath.PixelType(Imath.PixelType.FLOAT)), np.float32) else: g, b = 1.0, 0.3 # corresponds to gas? (~jets), particles? (~haze) # data2d = r * np.frombuffer(image.channel('R', Imath.PixelType(Imath.PixelType.FLOAT)), np.float32) data2d = g * np.frombuffer( image.channel('G', Imath.PixelType(Imath.PixelType.FLOAT)), np.float32) data2d = data2d + b * np.frombuffer( image.channel('B', Imath.PixelType(Imath.PixelType.FLOAT)), np.float32) data2d = data2d.reshape(shape) n = int(np.prod(shape)**(1 / 3) / 10) * 10 k = math.ceil(n**(1 / 2)) voxel_data = np.zeros((n, n, n), dtype=np.float32) assert n == resolution, 'something went wrong with the resolution calculation' \ + ' while importing voxel data (%d != %d)' % (n, resolution) for i in range(n): x0, y0 = (i % k) * n, (i // k) * n voxel_data[:, :, i] = data2d[y0:y0 + n, x0:x0 + n] voxel_data = np.transpose(np.flip(voxel_data, axis=2), axes=(1, 0, 2)) voxels = VoxelParticles(voxel_data=voxel_data, cell_size=cell_size, intensity=intensity, lf_ast_q=lf_vx_ast_q) self._particles = Particles(None, None, None, cones=None, haze=0.0, voxels=voxels) for s in self._iter_scenes(scenes): s.link_particles(self._particles) if self.verbose: print('done') return self._particles
def rotation_angleaxis(self, angleaxis): # better name as angle given first, then axis self.q = tools.angleaxis_to_q(angleaxis)
def rotation_axis_angle(self, angleaxis): self.q = tools.angleaxis_to_q(angleaxis)