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)