def load(self, camera_config, local_config, scale=1.0):
if camera_config is None:
if os.path.exists(local_config):
# load local config file if it exists
result = props_json.load(local_config, self.config)
if not result:
print("Cannot continue with invalid camera file.")
quit()
else:
print(
"no camera config specified and no local camera config file found:",
local_config)
quit()
else:
# seed the camera calibration and distortion coefficients
# from a known camera config
print("Setting camera config from:", camera_config)
props_json.load(camera_config, self.config)
self.config.setString('name', camera_config)
props_json.save(local_config, self.config)
self.get_K()
if scale:
# adjust effective K to account for scaling
self.K = self.K * scale
self.K[2, 2] = 1.0
self.config.setLen('mount_ypr', 3, 0)
def write_configs():
config = getNode("/config", True)
file = os.path.join(flight_dir, 'master-config.json')
props_json.save(file, config)
config = getNode("/config/autopilot", True)
file = os.path.join(flight_dir, 'ap-config.json')
props_json.save(file, config)
def save(self):
# create a project dictionary and write it out as json
if not os.path.exists(self.analysis_dir):
print("Error: project doesn't exist:", self.analysis_dir)
return
project_file = os.path.join(self.analysis_dir, "config.json")
config_node = getNode("/config", True)
props_json.save(project_file, config_node)
def save_images_info(self):
# create a project dictionary and write it out as json
if not os.path.exists(self.analysis_dir):
print("Error: project doesn't exist:", self.analysis_dir)
return
meta_dir = os.path.join(self.analysis_dir, 'meta')
images_node = getNode("/images", True)
for name in images_node.getChildren():
image_node = images_node.getChild(name, True)
image_path = os.path.join(meta_dir, name + '.json')
props_json.save(image_path, image_node)
def save(self, cal_file):
config = PropertyNode()
config.setFloat('min_temp_C', self.min_temp)
config.setFloat('max_temp_C', self.max_temp)
node = config.getChild('p', create=True)
p = self.p_bias
node.setString('bias', "%.8f %.8f %.8f" % (p[0], p[1], p[2]))
p = self.p_scale
node.setString('scale', "%.8f %.8f %.8f" % (p[0], p[1], p[2]))
node = config.getChild('q', create=True)
p = self.q_bias
node.setString('bias', "%.8f %.8f %.8f" % (p[0], p[1], p[2]))
p = self.q_scale
node.setString('scale', "%.8f %.8f %.8f" % (p[0], p[1], p[2]))
node = config.getChild('r', create=True)
p = self.r_bias
node.setString('bias', "%.8f %.8f %.8f" % (p[0], p[1], p[2]))
p = self.r_scale
node.setString('scale', "%.8f %.8f %.8f" % (p[0], p[1], p[2]))
node = config.getChild('ax', create=True)
p = self.ax_bias
node.setString('bias', "%.8f %.8f %.8f" % (p[0], p[1], p[2]))
p = self.ax_scale
node.setString('scale', "%.8f %.8f %.8f" % (p[0], p[1], p[2]))
node = config.getChild('ay', create=True)
p = self.ay_bias
node.setString('bias', "%.8f %.8f %.8f" % (p[0], p[1], p[2]))
p = self.ay_scale
node.setString('scale', "%.8f %.8f %.8f" % (p[0], p[1], p[2]))
node = config.getChild('az', create=True)
p = self.az_bias
node.setString('bias', "%.8f %.8f %.8f" % (p[0], p[1], p[2]))
p = self.az_scale
node.setString('scale', "%.8f %.8f %.8f" % (p[0], p[1], p[2]))
affine_str = []
for x in self.mag_affine.flat:
affine_str.append('%.10f' % x)
print(' '.join(affine_str))
config.setString('mag_affine', ' '.join(affine_str))
try:
props_json.save(cal_file, config)
except:
print("error saving " + cal_file + ": " + str(sys.exc_info()[1]))
return
def save(self, cal_file):
config = PropertyNode()
config.setFloat('min_temp_C', self.min_temp)
config.setFloat('max_temp_C', self.max_temp)
node = config.getChild('p', create=True)
p = self.p_bias
node.setString('bias', "%.8f %.8f %.8f" % (p[0], p[1], p[2]))
p = self.p_scale
node.setString('scale', "%.8f %.8f %.8f" % (p[0], p[1], p[2]))
node = config.getChild('q', create=True)
p = self.q_bias
node.setString('bias', "%.8f %.8f %.8f" % (p[0], p[1], p[2]))
p = self.q_scale
node.setString('scale', "%.8f %.8f %.8f" % (p[0], p[1], p[2]))
node = config.getChild('r', create=True)
p = self.r_bias
node.setString('bias', "%.8f %.8f %.8f" % (p[0], p[1], p[2]))
p = self.r_scale
node.setString('scale', "%.8f %.8f %.8f" % (p[0], p[1], p[2]))
node = config.getChild('ax', create=True)
p = self.ax_bias
node.setString('bias', "%.8f %.8f %.8f" % (p[0], p[1], p[2]))
p = self.ax_scale
node.setString('scale', "%.8f %.8f %.8f" % (p[0], p[1], p[2]))
node = config.getChild('ay', create=True)
p = self.ay_bias
node.setString('bias', "%.8f %.8f %.8f" % (p[0], p[1], p[2]))
p = self.ay_scale
node.setString('scale', "%.8f %.8f %.8f" % (p[0], p[1], p[2]))
node = config.getChild('az', create=True)
p = self.az_bias
node.setString('bias', "%.8f %.8f %.8f" % (p[0], p[1], p[2]))
p = self.az_scale
node.setString('scale', "%.8f %.8f %.8f" % (p[0], p[1], p[2]))
affine_str = []
for x in self.mag_affine.flat:
affine_str.append('%.10f' % x)
print ' '.join(affine_str)
config.setString('mag_affine', ' '.join(affine_str))
try:
props_json.save(cal_file, config)
except:
print "error saving " + cal_file + ": " + str(sys.exc_info()[1])
return
def save(self):
if self.filename == "":
filename = str(self.setFileName())
if filename == "":
# print "cancelled save ..."
return
else:
self.filename = filename
self.fileroot, ext = os.path.splitext(self.filename)
# print self.fileroot, ext
# create the design as a property tree
design = self.gen_property_tree()
try:
props_json.save(self.filename, design)
except:
print "error saving file"
return
self.setWindowTitle( self.default_title + " - " + os.path.basename(str(self.filename)) )
self.setClean()
def save(self):
if self.filename == "":
filename = str(self.setFileName())
if filename == "":
# print "cancelled save ..."
return
else:
self.filename = filename
self.fileroot, ext = os.path.splitext(self.filename)
# print self.fileroot, ext
# create the design as a property tree
design = self.gen_property_tree()
try:
props_json.save(self.filename, design)
except:
print("error saving file")
return
self.setWindowTitle(self.default_title + " - " +
os.path.basename(str(self.filename)))
self.setClean()
# copy config.json from first listed source project
print("Copying config.json from source project")
config_src = os.path.join(args.source[0], "ImageAnalysis", "config.json")
config_dest = os.path.join(analysis_dir, "config.json")
if os.path.exists(config_src):
shutil.copyfile(config_src, config_dest)
# assemble the collective smart.json file
print("Combining all the smart.json files")
smart_node = getNode("/smart", True)
for p in args.source:
smart_src = os.path.join(p, "ImageAnalysis", "smart.json")
props_json.load(smart_src, smart_node)
smart_dst = os.path.join(project_dir, "ImageAnalysis", "smart.json")
props_json.save(smart_dst, smart_node)
# assemble the collective annotations.json file
print("Combining any annotations.json files")
ann_node = getNode("/annotations", True)
ann_list = []
for p in args.source:
ann_src = os.path.join(p, "ImageAnalysis", "annotations.json")
if os.path.exists(ann_src):
f = open(ann_src, 'r')
ann_list += json.load(f)
ann_dst = os.path.join(project_dir, "ImageAnalysis", "annotations.json")
f = open(ann_dst, 'w')
json.dump(ann_list, f, indent=4)
# populate the meta directory
def log_imu_calibration(config):
os.path.join(flight_dir, 'imucal.json')
props_json.save(config, jsonfile)
return True
#!/usr/bin/python from props import PropertyNode, root, getNode import props_xml import props_json import sys # run the system through it's paces xmlfile = sys.argv[1] jsonfile = sys.argv[2] config = PropertyNode() props_xml.load(xmlfile, config) props_json.save(jsonfile, config)
cam_yaw = config.getFloatEnum('mount_ypr', 0)
cam_pitch = config.getFloatEnum('mount_ypr', 1)
cam_roll = config.getFloatEnum('mount_ypr', 2)
# setup camera calibration and distortion coefficients
if args.select_cam:
# set the camera calibration from known preconfigured setups
name, K, dist = cam_calib.set_camera_calibration(args.select_cam)
config.setString('name', name)
config.setFloat("fx", K[0][0])
config.setFloat("fy", K[1][1])
config.setFloat("cu", K[0][2])
config.setFloat("cv", K[1][2])
for i, d in enumerate(dist):
config.setFloatEnum("dist_coeffs", i, d)
props_json.save(camera_config, config)
else:
# load the camera calibration from the config file
name = config.getString('name')
size = config.getLen("dist_coeffs")
dist = []
for i in range(size):
dist.append(config.getFloatEnum("dist_coeffs", i))
K = np.zeros((3, 3))
K[0][0] = config.getFloat("fx")
K[1][1] = config.getFloat("fy")
K[0][2] = config.getFloat("cu")
K[1][2] = config.getFloat("cv")
K[2][2] = 1.0
print 'Camera:', name
def update(self, dt):
if not self.active:
return False
# update filters
p = self.imu_node.getFloat("p_rad_sec")
q = self.imu_node.getFloat("q_rad_sec")
r = self.imu_node.getFloat("r_rad_sec")
ax = self.imu_node.getFloat("ax_mps_sec")
ay = self.imu_node.getFloat("ay_mps_sec")
az = self.imu_node.getFloat("az_mps_sec")
hx_raw = self.imu_node.getFloat("hx_raw")
hy_raw = self.imu_node.getFloat("hy_raw")
hz_raw = self.imu_node.getFloat("hz_raw")
self.p_filt.update(p, dt)
self.q_filt.update(q, dt)
self.r_filt.update(r, dt)
self.ax_filt.update(ax, dt)
self.ay_filt.update(ay, dt)
self.az_filt.update(az, dt)
up_axis = self.detect_up()
if up_axis == "none":
self.armed = True
self.rot = 0.0
if self.state < 1:
print("up axis:", up_axis, "armed:", self.armed,
" rot: %0f" % self.rot)
sample_time = 5
if self.state == 0:
if self.armed and up_axis != "none":
if self.axis_time[up_axis] < sample_time:
self.samples.append([hx_raw, hy_raw, hz_raw])
if hx_raw < self.min[0]: self.min[0] = hx_raw
if hx_raw > self.max[0]: self.max[0] = hx_raw
if hy_raw < self.min[1]: self.min[1] = hy_raw
if hy_raw > self.max[1]: self.max[1] = hy_raw
if hz_raw < self.min[2]: self.min[2] = hz_raw
if hz_raw > self.max[2]: self.max[2] = hz_raw
print(self.min, self.max)
self.axis_time[up_axis] += dt
done = True
for key in self.axis_time:
if self.axis_time[key] < sample_time:
message = "need more: " + self.axis_hint[key]
self.task_calib_node.setString("mag_status", message)
comms.display.show(message)
done = False
break
if done:
self.state += 1
elif self.state == 1:
# did we measure a bunch of samples?
if len(self.samples) < 100:
print("Somehow didn't get many samples. :-(")
self.state += 2
else:
# center, evecs, radii, v = ellipsoid_fit(s)
# print("center:\n", center)
# print("evecs:\n", evecs)
# print("radii:\n", radii)
# print("v:\n", v)
# ellipsoid fit with our sample data
s = np.array(self.samples).T
M, n, d = ellipsoid_fit(s)
# calibration parameters. Note: some implementations
# of sqrtm return complex type, taking real
M_1 = linalg.inv(M)
self.b = -np.dot(M_1, n)
self.A_1 = np.real(self.F /
np.sqrt(np.dot(n.T, np.dot(M_1, n)) - d) *
linalg.sqrtm(M))
print("b:\n", self.b)
print("A_1:\n", self.A_1)
# assemble the mag calibration matrix
T = tr.translation_matrix(-self.b.flatten())
A1_h = np.eye(4)
A1_h[:3, :3] = self.A_1
self.mag_affine = A1_h @ T # this is the correct order
print("mag_affine:\n", self.mag_affine)
scale, shear, angles, translate, perspective = tr.decompose_matrix(
self.mag_affine)
print("scale:", scale)
print("shear:", shear)
print("angles:", angles)
print("translate:", translate)
print("perspective:", perspective)
# fixme what can we look at to sanity check?
# nothing bad detected, goto success state
self.state += 1
elif self.state == 2:
# calibration complete, success, save, report!
print("calibration succeeded")
print("magnetometer calibration:")
# as if this wasn't already fancy enough, get even fancier!
mapped = []
for i, v in enumerate(self.samples):
#print("sample:", i, v)
v1 = self.mag_affine @ np.hstack((v, 1))
v2 = self.A_1 @ (np.array(v) - self.b.flatten())
#print(v, v1[:3], v2)
mapped.append(v2)
print("samples:")
samples = np.array(self.samples)
print("x range:", np.min(samples[:, 0]), np.max(samples[:, 0]))
print("y range:", np.min(samples[:, 1]), np.max(samples[:, 1]))
print("z range:", np.min(samples[:, 2]), np.max(samples[:, 2]))
print("mapped:")
mapped = np.array(mapped)
print("x range:", np.min(mapped[:, 0]), np.max(mapped[:, 0]))
print("y range:", np.min(mapped[:, 1]), np.max(mapped[:, 1]))
print("z range:", np.min(mapped[:, 2]), np.max(mapped[:, 2]))
self.state += 2
calib_node = self.config_imu_node.getChild("calibration", True)
calib_node.setLen("mag_affine", 16)
for i in range(16):
calib_node.setFloatEnum("mag_affine", i,
self.mag_affine.flatten()[i])
home = os.path.expanduser("~")
props_json.save(os.path.join(home, "imu_calibration.json"),
calib_node)
message = "mag calibration succeeded"
self.task_calib_node.setString("mag_status", message)
comms.display.show(message)
self.state += 2
elif self.state == 3:
# calibration complete, failed, sad face. :-(
message = "mag calibration failed"
self.task_calib_node.setString("mag_status", message)
comms.display.show(message)
self.state += 1
fx = focal_len_mm * width / args.ccd_width
fy = focal_len_mm * height / ccd_height
else:
print("Cannot autodetect calibrated focal length, please specify a ccd-width")
quit()
print('ccd: %.3f x %.3f' % (ccd_width, ccd_height))
print('fx fy = %.2f %.2f' % (fx, fy))
cu = width * 0.5
cv = height * 0.5
print('cu cv = %.2f %.2f' % (cu, cv))
cam = Camera.Camera()
cam.set_defaults()
cam.set_meta(make, model, lens_model)
cam.set_lens_params(ccd_width, ccd_height, focal_len_mm)
cam.set_K(fx, fy, cu, cv)
cam.set_image_params(width, height)
if os.path.exists(camera_file):
print("Camera config file already exists:", camera_file)
if args.force:
print("Overwriting ...")
else:
print("Use [ --force ] to overwrite ...")
quit()
print("Saving:", camera_file)
cam_node = getNode('/config/camera', True)
props_json.save(camera_file, cam_node)
# emulate alpha blending
alpha = args.alpha
if alpha < 0: alpha = 0
if alpha > 1: alpha = 1
cv2.addWeighted(hud1_frame, alpha, frame_undist, 1 - alpha, 0, hud1_frame)
cv2.imshow('hud', hud1_frame)
output.write(hud1_frame)
key = cv2.waitKey(5) & 0xFF
if key == 27:
break
elif key == ord('y'):
cam_yaw += 0.5
config.setFloat('cam_yaw_deg', cam_yaw)
props_json.save(movie_config, config)
elif key == ord('Y'):
cam_yaw -= 0.5
config.setFloat('cam_yaw_deg', cam_yaw)
props_json.save(movie_config, config)
elif key == ord('p'):
cam_pitch += 0.5
config.setFloat('cam_pitch_deg', cam_pitch)
props_json.save(movie_config, config)
elif key == ord('P'):
cam_pitch -= 0.5
config.setFloat('cam_pitch_deg', cam_pitch)
props_json.save(movie_config, config)
elif key == ord('r'):
cam_roll -= 0.5
config.setFloat('cam_roll_deg', cam_roll)
basename, ext = os.path.splitext(abspath)
srtname = basename + ".srt"
dirname = basename + "_frames"
print("basename:", basename)
print("srtname:", srtname)
print("dirname:", dirname)
local_config = os.path.join(dirname, "camera.json")
config = PropertyNode()
if args.camera:
# seed the camera calibration and distortion coefficients from a
# known camera config
print('Setting camera config from:', args.camera)
props_json.load(args.camera, config)
config.setString('name', args.camera)
props_json.save(local_config, config)
elif os.path.exists(local_config):
# load local config file if it exists
props_json.load(local_config, config)
K_list = []
for i in range(9):
K_list.append( config.getFloatEnum('K', i) )
K = np.copy(np.array(K_list)).reshape(3,3)
dist = []
for i in range(5):
dist.append( config.getFloatEnum("dist_coeffs", i) )
# check for required input files
if not os.path.isfile(args.video):
print("%s doesn't exist, aborting ..." % args.video)
quit()
def save(self, local_config):
props_json.save(local_config, self.config)
def update(self, dt):
if not self.active:
return False
# update filters
ax = self.imu_node.getFloat("ax_raw")
ay = self.imu_node.getFloat("ay_raw")
az = self.imu_node.getFloat("az_raw")
self.ax_slow.update(ax, dt)
self.ax_fast.update(ax, dt)
self.ay_slow.update(ay, dt)
self.ay_fast.update(ay, dt)
self.az_slow.update(az, dt)
self.az_fast.update(az, dt)
# (no) motion test
ax_diff = self.ax_slow.filter_value - self.ax_fast.filter_value
ay_diff = self.ay_slow.filter_value - self.ay_fast.filter_value
az_diff = self.az_slow.filter_value - self.az_fast.filter_value
d = math.sqrt(ax_diff*ax_diff + ay_diff*ay_diff + az_diff*az_diff)
if d < 0.04:
stable = True
else:
stable = False
up_axis = self.detect_up()
if up_axis == "none":
self.armed = True
if self.state < 6:
print("up axis:", up_axis, "armed:", self.armed, " slow-fast: %.3f" % d, " stable:", stable)
if not self.armed:
self.task_node.setInt("calib_state", 99)
else:
self.task_node.setInt("calib_state", self.state)
if self.state == 0:
print("Place level and right side up - stable:", stable)
if self.armed and stable and self.new_axis():
self.meas[self.state] = [ self.ax_fast.filter_value,
self.ay_fast.filter_value,
self.az_fast.filter_value ]
self.checked[up_axis] = True
self.state += 1
self.armed = False
elif self.state == 1:
print("Place up side down - stable:", stable)
if self.armed and stable and self.new_axis():
self.meas[self.state] = [ self.ax_fast.filter_value,
self.ay_fast.filter_value,
self.az_fast.filter_value ]
self.checked[up_axis] = True
self.state += 1
self.armed = False
elif self.state == 2:
print("Place nose down - stable:", stable)
if self.armed and stable and self.new_axis():
self.meas[self.state] = [ self.ax_fast.filter_value,
self.ay_fast.filter_value,
self.az_fast.filter_value ]
self.checked[up_axis] = True
self.state += 1
self.armed = False
elif self.state == 3:
print("Place nose up - stable:", stable)
if self.armed and stable and self.new_axis():
self.meas[self.state] = [ self.ax_fast.filter_value,
self.ay_fast.filter_value,
self.az_fast.filter_value ]
self.checked[up_axis] = True
self.state += 1
self.armed = False
elif self.state == 4:
print("Place right wing down - stable:", stable)
if self.armed and stable and self.new_axis():
self.meas[self.state] = [ self.ax_fast.filter_value,
self.ay_fast.filter_value,
self.az_fast.filter_value ]
self.checked[up_axis] = True
self.state += 1
self.armed = False
elif self.state == 5:
print("Place right wing up - stable:", stable)
if self.armed and stable and self.new_axis():
self.meas[self.state] = [ self.ax_fast.filter_value,
self.ay_fast.filter_value,
self.az_fast.filter_value ]
self.checked[up_axis] = True
self.state += 1
self.armed = False
elif self.state == 6:
# did we measure 6 unique axes?
if len(self.checked) != 6:
print("Somehow didn't calibrate 6 orientations. :-(")
self.state += 2
else:
# compute affine rotation fit
v0 = np.array(self.meas, dtype=np.float64, copy=True).T
v1 = np.array(self.ref, dtype=np.float64, copy=True).T
self.accel_affine = tr.affine_matrix_from_points(v0, v1, shear=True, scale=True)
print("accel_affine:\n", self.accel_affine)
self.scale, shear, angles, self.translate, perspective = tr.decompose_matrix(self.accel_affine)
print("scale:", self.scale)
print("shear:", shear)
print("angles:", angles)
print("translate:", self.translate)
print("perspective:", perspective)
# recompose the original affine matrix with:
# translate @ rotate @ scale
T = tr.translation_matrix(self.translate)
self.R = tr.euler_matrix(*angles)
S = np.diag([self.scale[0], self.scale[1], self.scale[2], 1.0])
print("T:\n", T)
print("R:\n", self.R)
print("S:\n", S)
print("R @ R.T:\n", self.R @ self.R.T)
recompose = T @ self.R @ S
print("recompose:\n", recompose)
# check rotation matrix, if any row or column doesn't
# have an element close to 1, then bomb
if np.max(np.abs(self.R[0])) < 0.9:
print("bad row 1")
self.state += 2
elif np.max(np.abs(self.R[1])) < 0.9:
print("bad row 2")
self.state += 2
elif np.max(np.abs(self.R[2])) < 0.9:
print("bad row 3")
self.state += 2
elif np.max(np.abs(self.R[:,0])) < 0.9:
print("bad column 1")
self.state += 2
elif np.max(np.abs(self.R[:,1])) < 0.9:
print("bad column 2")
self.state += 2
elif np.max(np.abs(self.R[:,2])) < 0.9:
print("bad column 3")
self.state += 2
else:
# nothing bad detected, goto success state
self.state += 1
elif self.state == 7:
# calibration complete, success, report!
comms.events.log("calibrate accels", "calibration succeeded.")
#print("strapdown calibration:")
#print(self.R)
# as if this wasn't already fancy enough, get even fancier!
errors = []
for i, v in enumerate(self.meas):
print("measure:", i, v)
v1 = self.accel_affine @ np.hstack((v, 1))
v0 = self.ref[i]
err = np.linalg.norm(v0 - v1[:3])
errors.append(err)
#print("errors:", errors)
mean = np.mean(errors)
std = np.std(errors)
#print("calibration mean:", mean, " std:", std)
self.state += 2
calib_node = self.config_imu_node.getChild("calibration", True)
calib_node.setLen("strapdown", 9)
for i in range(9):
calib_node.setFloatEnum("strapdown", i, self.R[:3,:3].flatten()[i])
calib_node.setFloat("accel_fit_mean", mean)
calib_node.setFloat("accel_fit_std", std)
calib_node.setLen("accel_scale", 3)
for i in range(3):
calib_node.setFloatEnum("accel_scale", i, self.scale[i])
calib_node.setLen("accel_translate", 3)
for i in range(3):
calib_node.setFloatEnum("accel_translate", i, self.translate[i])
home = os.path.expanduser("~")
filename = os.path.join(home, "imu_calibration.json")
props_json.save(filename, calib_node)
comms.events.log("calibrate accels", "saved results to: " + filename)
elif self.state == 8:
# calibration complete, but failed. :-(
comms.events.log("calibrate accels", "calibration FAILED!")
pass
def save(analysis_dir):
surface_file = os.path.join(analysis_dir, "smart.json")
props_json.save(surface_file, smart_node)
def set_aircraft_poses(proj, posefile="", order='ypr', max_angle=25.0):
log("Setting aircraft poses")
#analysis_dir = os.path.join(proj.project_dir, 'ImageAnalysis')
meta_dir = os.path.join(proj.analysis_dir, 'meta')
images_node = getNode("/images", True)
by_index = False
f = fileinput.input(posefile)
for line in f:
line.strip()
if re.match('^\s*#', line):
#print("skipping comment ", line)
continue
if re.match('^\s*File', line):
#print("skipping header ", line)
continue
if re.match('^\s*Image', line):
#print("skipping header ", line)
by_index = True
file_list = gen_image_list(proj.project_dir)
continue
field = line.split(',')
if not by_index:
name = field[0]
else:
index = int(field[0]) - 1
name = file_list[index]
lat_deg = float(field[1])
lon_deg = float(field[2])
alt_m = float(field[3])
if order == 'ypr':
yaw_deg = float(field[4])
pitch_deg = float(field[5])
roll_deg = float(field[6])
elif order == 'rpy':
roll_deg = float(field[4])
pitch_deg = float(field[5])
yaw_deg = float(field[6])
if len(field) >= 8:
flight_time = float(field[7])
else:
flight_time = -1.0
found_dir = ''
if not os.path.isfile(os.path.join(proj.project_dir, name)):
log("No image file:", name, "skipping ...")
continue
if camera.camera_node.getString(
"make") == "DJI" or camera.camera_node.getString(
"make") == "Hasselblad":
# camera is on a gimbal so check if pitch is nearly nadir (-90)
if pitch_deg > -45:
log("gimbal not looking down:", name, "roll:", roll_deg,
"pitch:", pitch_deg)
continue
elif abs(roll_deg) > max_angle or abs(pitch_deg) > max_angle:
# fairly 'extreme' attitude, skip image
log("extreme attitude:", name, "roll:", roll_deg, "pitch:",
pitch_deg)
continue
base, ext = os.path.splitext(name)
i1 = image.Image(proj.analysis_dir, base)
i1.set_aircraft_pose(lat_deg, lon_deg, alt_m, yaw_deg, pitch_deg,
roll_deg, flight_time)
image_node = images_node.getChild(base, True)
image_path = os.path.join(meta_dir, base + '.json')
props_json.save(image_path, image_node)
log("pose:", name,
"yaw=%.1f pitch=%.1f roll=%.1f" % (yaw_deg, pitch_deg, roll_deg))
# in this system we force fx == fy, but here fy computed separately
# just for fun.
fx = focal_len_mm * float(width) / args.ccd_width
fy = focal_len_mm * float(height) / ccd_height
print('fx fy = %.2f %.2f' % (fx, fy))
cu = width * 0.5
cv = height * 0.5
print('cu cv = %.2f %.2f' % (cu, cv))
cam = Camera.Camera()
cam.set_defaults()
cam.set_meta(make, model, lens_model)
cam.set_lens_params(args.ccd_width, ccd_height, focal_len_mm)
cam.set_K(fx, fy, cu, cv)
cam.set_image_params(width, height)
file_name = os.path.join(args.config, base_name + '.json')
if os.path.exists(file_name):
print('Camera config file already exists:', file_name)
if args.force:
print('Overwriting ...')
else:
print('Aborting ...')
quit()
print('Saving:', file_name)
cam_node = getNode('/config/camera', True)
props_json.save(file_name, cam_node)
