def parseDict(pynode, newdict, basepath):
if 'include' in newdict:
# include file handling before anything else (follow up
# entries implicitely overwrite the include file values.)
if re.match('^/', newdict['include']):
file = newdict['include']
elif re.match('^~', newdict['include']):
file = os.path.expanduser(newdict['include'])
else:
file = os.path.join(basepath, newdict['include'])
# print 'include:', file
load(file, pynode)
for tag in newdict:
# print tag, type(newdict[tag])
if type(newdict[tag]) is dict:
if not tag in pynode.__dict__:
node = PropertyNode()
pynode.__dict__[tag] = node
else:
node = pynode.__dict__[tag]
parseDict(node, newdict[tag], basepath)
elif type(newdict[tag]) is list:
if tag in pynode.__dict__:
# print 'tag exists:', type(pynode.__dict__[tag])
if type(pynode.__dict__[tag]) is list:
# completely overwrite whatever was there
pynode.__dict__[tag] = []
else:
# promote single node to enumerated
pynode.__dict__[tag] = [pynode.__dict__[tag]]
else:
pynode.__dict__[tag] = []
for i, ele in enumerate(newdict[tag]):
if type(ele) is dict:
if i < len(pynode.__dict__[tag]):
newnode = pynode.__dict__[tag][i]
else:
newnode = PropertyNode()
pynode.__dict__[tag].append(newnode)
parseDict(newnode, ele, basepath)
else:
pynode.__dict__[tag].append(mydecode(ele))
elif type(newdict[tag]) is int \
or type(newdict[tag]) is float \
or type(newdict[tag]) is str \
or type(newdict[tag]) is unicode:
if tag == 'include':
# already handled
pass
else:
# normal case
#print 'normal case:', tag, newdict[tag]
mydecode(newdict[tag])
pynode.__dict__[tag] = mydecode(newdict[tag])
else:
print('json parse skipping:', tag, type(newdict[tag]))
def set_camera(project_dir,
camera,
yaw_deg=0.0,
pitch_deg=-90.0,
roll_deg=0.0):
proj = ProjectMgr.ProjectMgr(project_dir)
if camera:
# specified on command line
camera_file = camera
else:
# auto detect camera from image meta data
camera, make, model, lens_model = proj.detect_camera()
camera_file = os.path.join("..", "cameras", camera + ".json")
print("Camera:", camera_file)
# copy/overlay/update the specified camera config into the existing
# project configuration
cam_node = getNode('/config/camera', True)
tmp_node = PropertyNode()
if props_json.load(camera_file, tmp_node):
for child in tmp_node.getChildren(expand=False):
if tmp_node.isEnum(child):
# print(child, tmp_node.getLen(child))
for i in range(tmp_node.getLen(child)):
cam_node.setFloatEnum(child, i,
tmp_node.getFloatEnum(child, i))
else:
# print(child, type(tmp_node.__dict__[child]))
child_type = type(tmp_node.__dict__[child])
if child_type is float:
cam_node.setFloat(child, tmp_node.getFloat(child))
elif child_type is int:
cam_node.setInt(child, tmp_node.getInt(child))
elif child_type is str:
cam_node.setString(child, tmp_node.getString(child))
else:
print('Unknown child type:', child, child_type)
proj.cam.set_mount_params(yaw_deg, pitch_deg, roll_deg)
# note: dist_coeffs = array[5] = k1, k2, p1, p2, k3
# ... and save
proj.save()
else:
# failed to load camera config file
if not camera:
print("Camera autodetection failed.")
print(
"Consider running the new camera script to create a camera config"
)
print("and then try running this script again.")
else:
print("Provided camera config not found:", camera)
def gen_property_tree(self):
# create a new design root
design = PropertyNode()
# overview
node = design.getChild('overview', True)
self.overview.save(node)
# wings
i = 0
for wing in self.wings:
if wing.valid:
node = design.getChild('wing[%d]' % i, True)
wing.save(node)
i += 1
return design
def gen_property_tree(self):
# create a new design root
design = PropertyNode()
# overview
node = design.getChild('overview', True)
self.overview.save(node)
# wings
i = 0
for wing in self.wings:
if wing.valid:
node = design.getChild('wing[%d]' % i, True)
wing.save(node)
i += 1
return design
def loads(self, stream, fileroot):
self.wipe_slate()
design = PropertyNode()
if props_json.loads(stream, design, ""):
print("json parse successful")
else:
error = QErrorMessage(self)
error.showMessage("json parse failed")
return False
# design.pretty_print()
self.setWindowTitle(self.default_title + " - " + fileroot)
node = design.getChild('overview', True)
self.overview.load(node)
design.setLen('wing', 1) # force to be enumerated if not already
num_wings = design.getLen('wing')
for i in range(num_wings):
wing_node = design.getChild('wing[%d]' % i)
wing = WingUI(changefunc=self.onChange)
wing.load(wing_node)
self.wings.append(wing)
self.tabs.addTab(wing.get_widget(), "Wing: " + wing.get_name())
self.rebuildWingLists()
self.setClean()
def loads(self, stream, fileroot):
self.wipe_slate()
design = PropertyNode()
if props_json.loads(stream, design, ""):
print "json parse successful"
else:
error = QErrorMessage(self)
error.showMessage( "json parse failed" )
return False
# design.pretty_print()
self.setWindowTitle( self.default_title + " - " + fileroot )
node = design.getChild('overview', True)
self.overview.load(node)
design.setLen('wing', 1) # force to be enumerated if not already
num_wings = design.getLen('wing')
for i in range(num_wings):
wing_node = design.getChild('wing[%d]' % i)
wing = WingUI(changefunc=self.onChange)
wing.load(wing_node)
self.wings.append(wing)
self.tabs.addTab( wing.get_widget(), "Wing: " + wing.get_name() )
self.rebuildWingLists()
self.setClean()
help='how many non-frames to absorb before we decide the movie is over')
args = parser.parse_args()
#file = args.movie
scale = args.scale
skip_frames = args.skip_frames
# pathname work
abspath = os.path.abspath(args.movie)
filename, ext = os.path.splitext(abspath)
dirname = os.path.dirname(args.movie)
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):
# this works, but is bad form because az is originally created as a
# PropertyNode() branch that can't have a value, only childredn
imu.az = -9.80
# this should work
root.sensors.imu[2].az = -9.81
root.pretty_print()
print "alt_m:", root.sensors.gps[5].alt_m
config = getNode('/', create=True)
#file = '/home/curt/Source/AuraUAS/aura-data/config/main-skywalker.xml'
#props_xml.load(file, config)
#props_xml.save("testing.xml", config)
#props_json.save("testing.json", config)
newroot = PropertyNode()
props_json.load("/home/curt/Source/AuraUAS/aura-data/config/main-skywalker.json", newroot)
print "pretty:"
newroot.pretty_print()
quit()
print "sensor children:", sensors.getChildren()
for child in sensors.getChildren():
node = sensors.getChild(child)
print node
global_tasks = getNode("/config/mission/global_tasks");
print "global_tasks children:", global_tasks.getChildren()
print global_tasks.task[0].name
a = getNode("/task/home", True)
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
parser.add_argument('--yaw-deg', required=True, type=float,
help='camera yaw mounting offset from aircraft')
parser.add_argument('--pitch-deg', required=True, type=float,
help='camera pitch mounting offset from aircraft')
parser.add_argument('--roll-deg', required=True, type=float,
help='camera roll mounting offset from aircraft')
args = parser.parse_args()
proj = ProjectMgr.ProjectMgr(args.project)
# copy/overlay/update the specified camera config into the existing
# project configuration
cam_node = getNode('/config/camera', True)
tmp_node = PropertyNode()
props_json.load(args.camera, tmp_node)
for child in tmp_node.getChildren(expand=False):
if tmp_node.isEnum(child):
# print(child, tmp_node.getLen(child))
for i in range(tmp_node.getLen(child)):
cam_node.setFloatEnum(child, i, tmp_node.getFloatEnum(child, i))
else:
# print(child, type(tmp_node.__dict__[child]))
child_type = type(tmp_node.__dict__[child])
if child_type is float:
cam_node.setFloat(child, tmp_node.getFloat(child))
elif child_type is int:
cam_node.setInt(child, tmp_node.getInt(child))
elif child_type is str:
cam_node.setString(child, tmp_node.getString(child))
# this works, but is bad form because az is originally created as a
# PropertyNode() branch that can't have a value, only childredn
imu.az = -9.80
# this should work
root.sensors.imu[2].az = -9.81
root.pretty_print()
print "alt_m:", root.sensors.gps[5].alt_m
config = getNode('/', create=True)
#file = '/home/curt/Source/AuraUAS/aura-data/config/main-skywalker.xml'
#props_xml.load(file, config)
#props_xml.save("testing.xml", config)
#props_json.save("testing.json", config)
newroot = PropertyNode()
props_json.load(
"/home/curt/Source/AuraUAS/aura-data/config/main-skywalker.json", newroot)
print "pretty:"
newroot.pretty_print()
quit()
print "sensor children:", sensors.getChildren()
for child in sensors.getChildren():
node = sensors.getChild(child)
print node
global_tasks = getNode("/config/mission/global_tasks")
print "global_tasks children:", global_tasks.getChildren()
print global_tasks.task[0].name
class VirtualCamera:
config = PropertyNode()
K = None
IK = None
dist = None
PROJ = None
def __init__(self):
pass
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 save(self, local_config):
props_json.save(local_config, self.config)
def get_name(self):
return self.config.getString('name')
def get_K(self):
if self.K is None:
K_list = []
for i in range(9):
K_list.append(self.config.getFloatEnum('K', i))
self.K = np.copy(np.array(K_list)).reshape(3, 3)
return self.K
def get_IK(self):
if self.IK is None:
self.IK = np.linalg.inv(self.get_K())
return self.IK
def get_dist(self):
if self.dist is None:
self.dist = []
for i in range(5):
self.dist.append(self.config.getFloatEnum("dist_coeffs", i))
return self.dist
def get_shape(self):
return self.config.getFloat("width_px"), self.config.getFloat(
"height_px")
def get_ypr(self):
cam_yaw = self.config.getFloatEnum('mount_ypr', 0)
cam_pitch = self.config.getFloatEnum('mount_ypr', 1)
cam_roll = self.config.getFloatEnum('mount_ypr', 2)
return cam_yaw, cam_pitch, cam_roll
def set_ypr(self, cam_yaw, cam_pitch, cam_roll):
self.config.setFloatEnum('mount_ypr', 0, cam_yaw)
self.config.setFloatEnum('mount_ypr', 1, cam_pitch)
self.config.setFloatEnum('mount_ypr', 2, cam_roll)
def set_yaw(self, cam_yaw):
self.config.setFloatEnum('mount_ypr', 0, cam_yaw)
def set_pitch(self, cam_pitch):
self.config.setFloatEnum('mount_ypr', 1, cam_pitch)
def set_roll(self, cam_roll):
self.config.setFloatEnum('mount_ypr', 2, cam_roll)
def update_PROJ(self, ned, yaw_rad, pitch_rad, roll_rad):
cam_yaw, cam_pitch, cam_roll = self.get_ypr()
body2cam = transformations.quaternion_from_euler(
cam_yaw * d2r, cam_pitch * d2r, cam_roll * d2r, 'rzyx')
# this function modifies the parameters you pass in so, avoid
# getting our data changed out from under us, by forcing copies
# (a = b, wasn't sufficient, but a = float(b) forced a copy.
tmp_yaw = float(yaw_rad)
tmp_pitch = float(pitch_rad)
tmp_roll = float(roll_rad)
ned2body = transformations.quaternion_from_euler(
tmp_yaw, tmp_pitch, tmp_roll, 'rzyx')
#body2ned = transformations.quaternion_inverse(ned2body)
#print 'ned2body(q):', ned2body
ned2cam_q = transformations.quaternion_multiply(ned2body, body2cam)
ned2cam = np.matrix(
transformations.quaternion_matrix(np.array(ned2cam_q))[:3, :3]).T
#print 'ned2cam:', ned2cam
R = ned2proj.dot(ned2cam)
rvec, jac = cv2.Rodrigues(R)
tvec = -np.matrix(R) * np.matrix(ned).T
R, jac = cv2.Rodrigues(rvec)
# is this R the same as the earlier R?
self.PROJ = np.concatenate((R, tvec), axis=1)
#print 'PROJ:', PROJ
#print lat_deg, lon_deg, altitude, ref[0], ref[1], ref[2]
#print ned
return self.PROJ
# project from ned coordinates to image uv coordinates using
# camera K and PROJ matrices
def project_ned(self, ned):
uvh = self.K.dot(self.PROJ.dot([ned[0], ned[1], ned[2], 1.0]).T)
if uvh[2] > 0.2:
uvh /= uvh[2]
uv = (int(round(np.squeeze(uvh[0, 0]))),
int(round(np.squeeze(uvh[1, 0]))))
return uv
else:
return None
# project from camera 3d coordinates to image uv coordinates using
# camera K matrix
def project_xyz(self, v):
uvh = self.K.dot([v[0], v[1], v[2]])
# print(uvh)
if uvh[2] > 0.2:
uvh /= uvh[2]
uv = (int(round(uvh[0])), int(round(uvh[1])))
# print(uv)
return uv
else:
return None
# utility functions to support various efforts
# precompute to save time
horiz_ned = [0, 0, 0] # any value works here (as long as it's consistent
horiz_divs = 10
horiz_pts = []
for i in range(horiz_divs + 1):
a = (float(i) * 360 / float(horiz_divs)) * d2r
n = math.cos(a) + horiz_ned[0]
e = math.sin(a) + horiz_ned[1]
d = 0.0 + horiz_ned[2]
horiz_pts.append([n, e, d])
# returns roll, pitch of horizon given current camera attitude.
# Note camera attitude typically includes aircraft body attitude
# and camera mount offset. (The attiude values and offsets should
# be set/updated by the calling function)
def find_horizon(self):
answers = []
K = self.get_K()
IK = self.get_IK()
cu = K[0, 2]
cv = K[1, 2]
for i in range(self.horiz_divs):
uv1 = self.project_ned(self.horiz_pts[i])
uv2 = self.project_ned(self.horiz_pts[i + 1])
if uv1 != None and uv2 != None:
#print(" ", uv1, uv2)
roll, pitch = self.get_projected_attitude(uv1, uv2, IK, cu, cv)
answers.append((roll, pitch))
if len(answers) > 0:
index = int(len(answers) / 2)
return answers[index]
else:
return None, None
# a, b are line end points, p is some other point
# returns the closest point on ab to p (orthogonal projection)
def ClosestPointOnLine(self, a, b, p):
ap = p - a
ab = b - a
return a + np.dot(ap, ab) / np.dot(ab, ab) * ab
# get the roll/pitch of camera orientation relative to specified
# horizon line
def get_projected_attitude(self, uv1, uv2, IK, cu, cv):
# print('line:', line)
du = uv2[0] - uv1[0]
dv = uv1[1] - uv2[
1] # account for (0,0) at top left corner in image space
roll = math.atan2(dv, du)
if False:
# temp test
w = cu * 2
h = cv * 2
for p in [(0, 0, 1), (w, 0, 1), (0, h, 1), (w, h, 1), (cu, cv, 1)]:
uvh = np.array(p)
proj = IK.dot(uvh)
print(p, "->", proj)
p0 = self.ClosestPointOnLine(np.array(uv1), np.array(uv2),
np.array([cu, cv]))
uvh = np.array([p0[0], p0[1], 1.0])
proj = IK.dot(uvh)
#print("proj:", proj, proj/np.linalg.norm(proj))
dot_product = np.dot(np.array([0, 0, 1]), proj / np.linalg.norm(proj))
pitch = np.arccos(dot_product)
if p0[1] < cv:
pitch = -pitch
#print("roll: %.1f pitch: %.1f" % (roll, pitch))
return roll, pitch
def _parseXML(pynode, xmlnode, basepath):
overlay = 'overlay' in xmlnode.attrib
exists = xmlnode.tag in pynode.__dict__
if len(xmlnode) or 'include' in xmlnode.attrib:
# has children
newnode = PropertyNode()
if 'include' in xmlnode.attrib:
filename = basepath + '/' + xmlnode.attrib['include']
print("calling load():", filename, xmlnode.attrib)
load(filename, newnode)
if 'n' in xmlnode.attrib:
# enumerated node
n = int(xmlnode.attrib['n'])
if not exists:
pynode.__dict__[xmlnode.tag] = []
elif not type(pynode.__dict__[xmlnode.tag]) is list:
savenode = pynode.__dict__[xmlnode.tag]
pynode.__dict__[xmlnode.tag] = [savenode]
tmp = pynode.__dict__[xmlnode.tag]
pynode.extendEnumeratedNode(tmp, n)
pynode.__dict__[xmlnode.tag][n] = newnode
elif exists:
if not overlay:
# append
# print "node exists:", xmlnode.tag, "overlay:", overlay
if not type(pynode.__dict__[xmlnode.tag]) is list:
# we need to convert this to an enumerated list
print("converting node to enumerated:", xmlnode.tag)
savenode = pynode.__dict__[xmlnode.tag]
pynode.__dict__[xmlnode.tag] = [savenode]
pynode.__dict__[xmlnode.tag].append(newnode)
else:
# overlay (follow existing tree)
newnode = pynode.__dict__[xmlnode.tag]
else:
# create new node
pynode.__dict__[xmlnode.tag] = newnode
for child in xmlnode:
_parseXML(newnode, child, basepath)
else:
# leaf
value = xmlnode.text
if 'type' in xmlnode.attrib:
if xmlnode.attrib['type'] == 'bool':
print(xmlnode.tag, "is bool")
if value == '0' or value == 'false' or value == '':
value = False
else:
value = True
if 'n' in xmlnode.attrib:
# enumerated node
n = int(xmlnode.attrib['n'])
if not exists:
pynode.__dict__[xmlnode.tag] = []
elif not type(pynode.__dict__[xmlnode.tag]) is list:
savenode = pynode.__dict__[xmlnode.tag]
pynode.__dict__[xmlnode.tag] = [savenode]
tmp = pynode.__dict__[xmlnode.tag]
pynode.extendEnumeratedLeaf(tmp, n, "")
pynode.__dict__[xmlnode.tag][n] = value
# print "leaf:", xmlnode.tag, value, xmlnode.attrib
elif exists:
if not overlay:
# append
if not type(pynode.__dict__[xmlnode.tag]) is list:
# convert to enumerated.
print("converting node to enumerated")
savenode = pynode.__dict__[xmlnode.tag]
pynode.__dict__[xmlnode.tag] = [savenode]
pynode.__dict__[xmlnode.tag].append(value)
else:
# overwrite
pynode.__dict__[xmlnode.tag] = value
elif type(xmlnode.tag) is str:
pynode.__dict__[xmlnode.tag] = value
else:
# print "Skipping unknown node:", xmlnode.tag, ":", value
pass
#!/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)
parser.add_argument('--stop-count', type=int, default=1, help='how many non-frames to absorb before we decide the movie is over')
args = parser.parse_args()
#file = args.movie
scale = args.scale
skip_frames = args.skip_frames
# pathname work
abspath = os.path.abspath(args.movie)
filename, ext = os.path.splitext(abspath)
dirname = os.path.dirname(args.movie)
output_csv = filename + ".csv"
output_avi = filename + "_smooth.avi"
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)
name = config.getString('name')
cam_yaw = config.getFloatEnum('mount_ypr', 0)
cam_pitch = config.getFloatEnum('mount_ypr', 1)
total = len(status)
#print '%s%d / %d inliers/matched' % (space, np.sum(status), len(status))
return M, status, np.float32(newp1), np.float32(newp2)
# pathname work
abspath = os.path.abspath(args.video)
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 = []
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
proj = project.ProjectMgr(args.project)
if args.camera:
# specified on command line
camera_file = args.camera
else:
# auto detect camera from image meta data
camera_name, make, model, lens_model = proj.detect_camera()
camera_file = os.path.join("..", "cameras", camera_name + ".json")
print("Camera:", camera_file)
# copy/overlay/update the specified camera config into the existing
# project configuration
cam_node = getNode('/config/camera', True)
tmp_node = PropertyNode()
if props_json.load(camera_file, tmp_node):
props_json.overlay(cam_node, tmp_node)
camera.set_mount_params(args.yaw_deg, args.pitch_deg, args.roll_deg)
# note: dist_coeffs = array[5] = k1, k2, p1, p2, k3
# ... and save
proj.save()
else:
# failed to load camera config file
if not args.camera:
print("Camera autodetection failed.")
print(
"Consider running the new camera script to create a camera config")
print("and then try running this script again.")
def load(self, cal_file):
config = PropertyNode()
try:
name, ext = os.path.splitext(cal_file)
if ext == '.json':
props_json.load(cal_file, config)
self.valid = True
elif ext == '.xml':
props_xml.load(cal_file, config)
self.valid = True
else:
return False
except:
print cal_file + ": load error:\n" + str(sys.exc_info()[1])
return False
root.pretty_print()
self.min_temp = config.getFloat('min_temp_C')
self.max_temp = config.getFloat('max_temp_C')
node = config.getChild('p')
if node:
p1, p2, p3 = node.getString('bias').split()
self.p_bias = np.array([p1, p2, p3], dtype=np.float64)
p1, p2, p3 = node.getString('scale').split()
self.p_scale = np.array([p1, p2, p3], dtype=np.float64)
node = config.getChild('q')
if node:
p1, p2, p3 = node.getString('bias').split()
self.q_bias = np.array([p1, p2, p3], dtype=np.float64)
p1, p2, p3 = node.getString('scale').split()
self.q_scale = np.array([p1, p2, p3], dtype=np.float64)
node = config.getChild('r')
if node:
p1, p2, p3 = node.getString('bias').split()
self.r_bias = np.array([p1, p2, p3], dtype=np.float64)
p1, p2, p3 = node.getString('scale').split()
self.r_scale = np.array([p1, p2, p3], dtype=np.float64)
node = config.getChild('ax')
if node:
p1, p2, p3 = node.getString('bias').split()
self.ax_bias = np.array([p1, p2, p3], dtype=np.float64)
p1, p2, p3 = node.getString('scale').split()
self.ax_scale = np.array([p1, p2, p3], dtype=np.float64)
node = config.getChild('ay')
if node:
p1, p2, p3 = node.getString('bias').split()
self.ay_bias = np.array([p1, p2, p3], dtype=np.float64)
p1, p2, p3 = node.getString('scale').split()
self.ay_scale = np.array([p1, p2, p3], dtype=np.float64)
node = config.getChild('az')
if node:
p1, p2, p3 = node.getString('bias').split()
self.az_bias = np.array([p1, p2, p3], dtype=np.float64)
p1, p2, p3 = node.getString('scale').split()
self.az_scale = np.array([p1, p2, p3], dtype=np.float64)
tokens = config.getString('mag_affine').split()
if len(tokens) == 16:
r = 0
c = 0
for i, x in enumerate(tokens):
self.mag_affine[r,c] = float(x)
c += 1
if c > 3:
c = 0
r += 1
self.mag_affine_inv = np.linalg.inv(self.mag_affine)
else:
print "mag_affine requires 16 values"
#print 'mag_affine:\n', self.mag_affine
#print 'mag_affine_inv:\n', self.mag_affine_inv
return True
def load(self, cal_file):
config = PropertyNode()
try:
name, ext = os.path.splitext(cal_file)
if ext == '.json':
props_json.load(cal_file, config)
self.valid = True
elif ext == '.xml':
props_xml.load(cal_file, config)
self.valid = True
else:
return False
except:
print(cal_file + ": load error:\n" + str(sys.exc_info()[1]))
return False
root.pretty_print()
self.min_temp = config.getFloat('min_temp_C')
self.max_temp = config.getFloat('max_temp_C')
node = config.getChild('p')
if node:
p1, p2, p3 = node.getString('bias').split()
self.p_bias = np.array([p1, p2, p3], dtype=np.float64)
p1, p2, p3 = node.getString('scale').split()
self.p_scale = np.array([p1, p2, p3], dtype=np.float64)
node = config.getChild('q')
if node:
p1, p2, p3 = node.getString('bias').split()
self.q_bias = np.array([p1, p2, p3], dtype=np.float64)
p1, p2, p3 = node.getString('scale').split()
self.q_scale = np.array([p1, p2, p3], dtype=np.float64)
node = config.getChild('r')
if node:
p1, p2, p3 = node.getString('bias').split()
self.r_bias = np.array([p1, p2, p3], dtype=np.float64)
p1, p2, p3 = node.getString('scale').split()
self.r_scale = np.array([p1, p2, p3], dtype=np.float64)
node = config.getChild('ax')
if node:
p1, p2, p3 = node.getString('bias').split()
self.ax_bias = np.array([p1, p2, p3], dtype=np.float64)
p1, p2, p3 = node.getString('scale').split()
self.ax_scale = np.array([p1, p2, p3], dtype=np.float64)
node = config.getChild('ay')
if node:
p1, p2, p3 = node.getString('bias').split()
self.ay_bias = np.array([p1, p2, p3], dtype=np.float64)
p1, p2, p3 = node.getString('scale').split()
self.ay_scale = np.array([p1, p2, p3], dtype=np.float64)
node = config.getChild('az')
if node:
p1, p2, p3 = node.getString('bias').split()
self.az_bias = np.array([p1, p2, p3], dtype=np.float64)
p1, p2, p3 = node.getString('scale').split()
self.az_scale = np.array([p1, p2, p3], dtype=np.float64)
tokens = config.getString('mag_affine').split()
if len(tokens) == 16:
r = 0
c = 0
for i, x in enumerate(tokens):
self.mag_affine[r, c] = float(x)
c += 1
if c > 3:
c = 0
r += 1
self.mag_affine_inv = np.linalg.inv(self.mag_affine)
else:
print("mag_affine requires 16 values")
#print 'mag_affine:\n', self.mag_affine
#print 'mag_affine_inv:\n', self.mag_affine_inv
return True
# pathname work
abspath = os.path.abspath(args.movie)
filename, ext = os.path.splitext(abspath)
movie_log = filename + ".csv"
movie_config = filename + ".json"
# combinations that seem to work on linux
# ext = avi, fourcc = MJPG
# ext = avi, fourcc = XVID
# ext = mov, fourcc = MP4V
tmp_movie = filename + "_tmp.mov"
output_movie = filename + "_hud.mov"
# load config file if it exists
config = PropertyNode()
props_json.load(movie_config, config)
cam_yaw = config.getFloat('cam_yaw_deg')
cam_pitch = config.getFloat('cam_pitch_deg')
cam_roll = config.getFloat('cam_roll_deg')
# load movie log
movie = []
with open(movie_log, 'rb') as f:
for line in f:
movie.append( re.split('[,\s]+', line.rstrip()) )
flight_imu = []
flight_gps = []
flight_filter = []
flight_air = []