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 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
# 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))
else:
print('Unknown child type:', child, child_type)
proj.cam.set_mount_params(args.yaw_deg, args.pitch_deg, args.roll_deg)
# note: dist_coeffs = array[5] = k1, k2, p1, p2, k3
# the following counts as cruft, but saving it for now until I can
# migrate these# over to official camera database files.
# if args.sentera_3M:
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
K = K * args.scale
K[2, 2] = 1.0
print "Opening ", args.movie
try:
capture = cv2.VideoCapture(args.movie)
#capture = cv2.VideoCapture() # webcam
except:
print "error opening video"
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
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 = []
flight_pilot = []
flight_act = []
