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
output_csv = filename + ".csv"
camera_config = dirname + "/camera.json"
# load config file if it exists
config = PropertyNode()
props_json.load(camera_config, 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")
# weighted add of the HUD frame with the original frame to
# 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
